File DensitySampler.cpp

File List > algos > mesh_to_pointcloud > DensitySampler.cpp

Go to the documentation of this file

#include "mesh_to_pointcloud/DensitySampler.h"

namespace Argos {

    PointCloud DensitySampler::sample(const Mesh& mesh)
    {
        PointCloud cloud;

        const auto& faces = mesh.getFaces();
        const auto& vertices = mesh.getVertices();

        if (faces.empty())
            return cloud;

        std::size_t faceCount = faces.size();

        for (std::size_t f = 0; f < faceCount; ++f)
        {

            const Face& face = faces[f];
            const auto& indices = face.getIndices();

            if (indices.size() < 3)
                continue;

            for (std::size_t i = 1; i < indices.size() - 1; ++i)
            {
                const auto& A = vertices[indices[0]];
                const auto& B = vertices[indices[i]];
                const auto& C = vertices[indices[i + 1]];

                auto AB = B - A;
                auto AC = C - A;

                double area = 0.5 * (AB ^ AC).norm(); 

                std::size_t pointCount = static_cast<std::size_t>(m_density * area ); // density * area = nb points/triangle

                for (std::size_t j = 0; j < pointCount; j++) {
                    double u = static_cast<double>(std::rand()) / RAND_MAX;
                    double v = static_cast<double>(std::rand()) / RAND_MAX;

                    if (u + v > 1.0) {
                        u = 1.0 - u;
                        v = 1.0 - v;
                    }

                    double alpha = 1.0 - u - v;
                    double beta = u;
                    double gamma = v;

                    auto P = A * alpha + B * beta + C * gamma;

                    cloud.addPoint(P);
                }
            }
        }

        return cloud;
    }

}