Mesh.cpp

Go to the documentation of this file.

#include "Mesh.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx::viz
{
    Mesh&
    Mesh::grid2D(Eigen::Vector2f extents,
                 Eigen::Vector2i numPoints,
                 std::function<viz::Color(size_t, size_t, const Eigen::Vector3f&)> colorFunc)
    {
        // Create vertices.
        std::vector<std::vector<Eigen::Vector3f>> vertices =
            grid::makeGrid2DVertices(extents, numPoints);
 
        // Create colors.
        std::vector<std::vector<viz::data::Color>> colors =
            grid::makeGrid2DColors(vertices, colorFunc);
 
        return this->grid2D(vertices, colors);
    }
 
    Mesh&
    Mesh::grid2D(const std::vector<std::vector<Eigen::Vector3f>>& vertices,
                 const std::vector<std::vector<viz::data::Color>>& colors)
    {
        ARMARX_CHECK_EQUAL(vertices.size(), colors.size())
            << "Numbers of vertices and colors must match.";
 
        if (vertices.empty())
        {
            return *this;
        }
 
        const size_t num_x = vertices.size();
        const size_t num_y = vertices.front().size();
 
        bool check = false; // This could unnecessarily slow down building large meshes.
        if (check)
        {
            // Check consistent sizes.
            for (const auto& vv : vertices)
            {
                ARMARX_CHECK_EQUAL(vv.size(), num_y)
                    << "All nested vectors must have equal length.";
            }
            for (const auto& cv : colors)
            {
                ARMARX_CHECK_EQUAL(cv.size(), num_y)
                    << "All nested vectors must have equal length.";
            }
        }
 
        // Create faces.
        const std::vector<viz::data::Face> faces = grid::makeGrid2DFaces(num_x, num_y);
 
        // Flatten
        return this->vertices(grid::flatten(vertices)).colors(grid::flatten(colors)).faces(faces);
    }
 
    std::vector<std::vector<Eigen::Vector3f>>
    grid::makeGrid2DVertices(Eigen::Vector2f extents, Eigen::Vector2i numPoints, float height)
    {
        const Eigen::Vector2f minimum = -extents / 2;
 
        // extents = (num - 1) * step  =>  step = extents / (num - 1)
        const Eigen::Vector2f step =
            (extents.array() / (numPoints.array() - 1).cast<float>()).matrix();
 
        ARMARX_CHECK_POSITIVE(numPoints.minCoeff())
            << "Number of points must be positive. " << VAROUT(numPoints);
        const size_t num_x = size_t(numPoints.x());
        const size_t num_y = size_t(numPoints.y());
 
        // Create vertices.
        std::vector<std::vector<Eigen::Vector3f>> gridVertices(
            num_x, std::vector<Eigen::Vector3f>(num_y, Eigen::Vector3f::Zero()));
 
        for (size_t i = 0; i < num_x; i++)
        {
            for (size_t j = 0; j < num_y; j++)
            {
                gridVertices[i][j].x() = minimum.x() + i * step.x();
                gridVertices[i][j].y() = minimum.y() + j * step.y();
                gridVertices[i][j].z() = height;
            }
        }
 
        return gridVertices;
    }
 
    std::vector<std::vector<viz::data::Color>>
    grid::makeGrid2DColors(
        const std::vector<std::vector<Eigen::Vector3f>>& vertices,
        std::function<viz::Color(size_t x, size_t y, const Eigen::Vector3f& p)> colorFunc)
    {
        size_t num_x = vertices.size();
        size_t num_y = vertices.front().size();
 
        std::vector<std::vector<viz::data::Color>> colors(
            num_x, std::vector<viz::data::Color>(num_y, viz::Color::black()));
 
        for (size_t i = 0; i < num_x; i++)
        {
            for (size_t j = 0; j < num_y; j++)
            {
                colors[i][j] = colorFunc(i, j, vertices[i][j]);
            }
        }
 
        return colors;
    }
 
    std::vector<viz::data::Face>
    grid::makeGrid2DFaces(size_t num_x, size_t num_y)
    {
        std::vector<viz::data::Face> faces(2 * (num_x - 1) * (num_y - 1));
 
        size_t index = 0;
        for (size_t x = 0; x < num_x - 1; x++)
        {
            for (size_t y = 0; y < num_y - 1; y++)
            {
                /* In counter-clockwise order.
                 *       (x)  (x+1)
                 * (y)   *----*
                 *       | \f1|
                 *       |f2\ |
                 * (y+1) *----*
                 */
                faces[index].v0 = faces[index].c0 = int(x * num_y + y);
                faces[index].v1 = faces[index].c1 = int((x + 1) * num_y + (y + 1));
                faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + y);
                index++;
 
                faces[index].v0 = faces[index].c0 = int(x * num_y + y);
                faces[index].v1 = faces[index].c1 = int(x * num_y + (y + 1));
                faces[index].v2 = faces[index].c2 = int((x + 1) * num_y + (y + 1));
                index++;
            }
        }
        return faces;
    }
 
 
} // namespace armarx::viz