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;

            std::size_t indices_size = indices.size();

            for (std::size_t i = 1; i < indices_size - 1; ++i)
            {
                auto ia = indices[0];
                auto ib = indices[i];
                auto ic = indices[i + 1];

                if (ia >= vertices.size() || ib >= vertices.size() || ic >= vertices.size())
                    continue; //

                const auto& A = vertices[ia];
                const auto& B = vertices[ib];
                const auto& C = vertices[ic];

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

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

                if (area <= 0.0)
                    continue;

                double expected = m_density * area;

                std::size_t pointCount = static_cast<std::size_t>(expected);
                double remainder = expected - pointCount;

                double r = static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX);
                if (r < remainder)
                    pointCount++;

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

}