DebugDrawerUtils.cpp

Go to the documentation of this file.

#include "DebugDrawerUtils.h"
 
#include <VirtualRobot/Visualization/TriMeshModel.h>
 
armarx::DebugDrawerTriMesh
armarx::DebugDrawerUtils::convertTriMesh(const VirtualRobot::TriMeshModel& trimesh)
{
    DebugDrawerTriMesh ddMesh;
 
    auto addVertex = [&](const Eigen::Vector3f& v)
    {
        ddMesh.vertices.push_back({v(0), v(1), v(2)});
        return (int)ddMesh.vertices.size() - 1;
    };
 
    auto addColor = [&](const VirtualRobot::VisualizationFactory::Color& c)
    {
        ddMesh.colors.push_back(DrawColor{c.r, c.g, c.b, c.transparency});
        return (int)ddMesh.colors.size() - 1;
    };
    ddMesh.faces.reserve(trimesh.faces.size());
    ddMesh.vertices.reserve(trimesh.vertices.size());
    ddMesh.colors.reserve(trimesh.colors.size());
 
    for (auto& f : trimesh.faces)
    {
        DebugDrawerFace f2;
        f2.vertex1.vertexID = addVertex(trimesh.vertices.at(f.id1));
        f2.vertex2.vertexID = addVertex(trimesh.vertices.at(f.id2));
        f2.vertex3.vertexID = addVertex(trimesh.vertices.at(f.id3));
        if (f.idNormal1 != UINT_MAX)
        {
            f2.vertex1.normalID = addVertex(trimesh.normals.at(f.idNormal1));
            f2.vertex2.normalID = addVertex(trimesh.normals.at(f.idNormal2));
            f2.vertex3.normalID = addVertex(trimesh.normals.at(f.idNormal3));
        }
        else
        {
            //                    f2.vertex1.normalID = f2.vertex2.normalID = f2.vertex3.normalID = addVertex(f.normal);
            f2.normal = {f.normal.x(), f.normal.y(), f.normal.z()};
        }
        f2.vertex1.colorID = addColor(trimesh.colors.at(f.idColor1));
        f2.vertex2.colorID = addColor(trimesh.colors.at(f.idColor2));
        f2.vertex3.colorID = addColor(trimesh.colors.at(f.idColor3));
 
        ddMesh.faces.push_back(f2);
    }
    return ddMesh;
}