File PointsPerFaceSampler.cpp

File List > algos > mesh_to_pointcloud > PointsPerFaceSampler.cpp

Go to the documentation of this file

#include "mesh_to_pointcloud/PointsPerFaceSampler.h"

#include "mesh/Mesh.h"

namespace Argos {

    PointCloud PointsPerFaceSampler::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();
        cloud.reserve(m_pointPerFace * faceCount);

        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 = 0; i < m_pointPerFace; ++i)
            {
                // Triangulation fan
                std::size_t triIndex = 1 + std::rand() % (indices.size() - 2);

                const auto& A = vertices[indices[0]];
                const auto& B = vertices[indices[triIndex]];
                const auto& C = vertices[indices[triIndex + 1]];

                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;
    }

}