Added drawTriMesh variations()

source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.cpp

@@ -177,6 +177,121 @@ namespace armarx
         drawArrow(from, dir / length, length, width, color, name, layer);
     }
 
+    void DebugDrawerTopic::drawTriMesh(
+        const VirtualRobot::TriMeshModel& triMesh, const DrawColor& color,
+        const std::string& name, const std::string& layer)
+    {
+        if (!enabled())
+        {
+            return;
+        }
+
+        DebugDrawerTriMesh dd;
+        dd.colors.push_back(color);
+
+        for (const auto& vertex : triMesh.vertices)
+        {
+            dd.vertices.push_back({ vertex.x(), vertex.y(), vertex.z() });
+        }
+
+        const std::size_t normalBase = dd.vertices.size();
+        for (const auto& normal : triMesh.normals)
+        {
+            dd.vertices.push_back({ normal.x(), normal.y(), normal.z() });
+        }
+
+        for (const auto& face : triMesh.faces)
+        {
+            DebugDrawerFace ddf;
+            ddf.vertex1.vertexID = static_cast<Ice::Int>(face.id1);
+            ddf.vertex2.vertexID = static_cast<Ice::Int>(face.id2);
+            ddf.vertex3.vertexID = static_cast<Ice::Int>(face.id3);
+
+            ddf.vertex1.colorID = ddf.vertex2.colorID = ddf.vertex3.colorID = 0;
+            ddf.vertex1.normalID = ddf.vertex2.normalID = ddf.vertex3.normalID = -1;
+
+            bool validNormalIDs = true;
+            for (const auto& id :
+                 {
+                     face.idNormal1, face.idNormal2, face.idNormal3
+                 })
+            {
+                validNormalIDs &= id < triMesh.normals.size();
+            }
+
+            if (validNormalIDs)
+            {
+                ddf.vertex1.normalID = static_cast<Ice::Int>(normalBase + face.idNormal1);
+                ddf.vertex2.normalID = static_cast<Ice::Int>(normalBase + face.idNormal2);
+                ddf.vertex3.normalID = static_cast<Ice::Int>(normalBase + face.idNormal3);
+            }
+            else
+            {
+                const Eigen::Vector3f& normal = face.normal;
+                ddf.normal = { normal.x(), normal.y(), normal.z() };
+            }
+
+            dd.faces.push_back(ddf);
+        }
+
+        topic->setTriMeshVisu(theLayer(layer), name, dd);
+    }
+
+    void DebugDrawerTopic::drawTriMeshAsPolygons(const VirtualRobot::TriMeshModel& trimesh,
+            float lineWidth, const DrawColor& colorInner, const DrawColor& colorBorder,
+            const std::string& name, const std::string& layer)
+    {
+        if (!enabled())
+        {
+            return;
+        }
+
+        ARMARX_INFO << "Drawing trimesh as polygons";
+
+        int counter = 0;
+        for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
+        {
+            const auto& face = trimesh.faces[i];
+            PolygonPointList points
+            {
+                new Vector3(trimesh.vertices[face.id1]),
+                new Vector3(trimesh.vertices[face.id2]),
+                new Vector3(trimesh.vertices[face.id3])
+            };
+
+            topic->setPolygonVisu(theLayer(layer), name + "_" + std::to_string(counter), points,
+                                  colorInner, colorBorder, lineWidth);
+            ++counter;
+        }
+    }
+
+    void DebugDrawerTopic::drawTriMeshAsPolygons(
+        const VirtualRobot::TriMeshModel& trimesh,
+        float lineWidth, const std::vector<DrawColor>& faceColorsInner, const DrawColor& colorBorder,
+        const std::string& name, const std::string& layer)
+    {
+        if (!enabled())
+        {
+            return;
+        }
+
+        ARMARX_INFO << "Drawing trimesh as polygons colored by area";
+
+        for (std::size_t i = 0; i < trimesh.faces.size(); ++i)
+        {
+            const auto& face = trimesh.faces[i];
+            PolygonPointList points
+            {
+                new Vector3(trimesh.vertices[face.id1]),
+                new Vector3(trimesh.vertices[face.id2]),
+                new Vector3(trimesh.vertices[face.id3])
+            };
+
+            topic->setPolygonVisu(theLayer(layer), name + "_" + std::to_string(i), points,
+                                  faceColorsInner.at(i), colorBorder, lineWidth);
+        }
+    }
+
 
     void DebugDrawerTopic::shortSleep()
     {

source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h

@@ -3,6 +3,8 @@
 #include <chrono>
 #include <thread>
 
+#include <VirtualRobot/Visualization/TriMeshModel.h>  // must be included before Eigen/Geometry
+
 #include <Eigen/Geometry>
 
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
@@ -115,6 +117,23 @@ namespace armarx
                              const DrawColor& color, const std::string& name, const std::string& layer = "");
 
 
+        /// Draw a TriMeshModel as DebugDrawerTriMesh with the given color.
+        void drawTriMesh(const VirtualRobot::TriMeshModel& triMesh, const DrawColor& color,
+                         const std::string& name, const std::string& layer = "");
+
+        /// Draw a TriMeshModel as individual polygons.
+        void drawTriMeshAsPolygons(
+            const VirtualRobot::TriMeshModel& trimesh,
+            float lineWidth, const DrawColor& colorInner, const DrawColor& colorBorder,
+            const std::string& name, const std::string& layer = "");
+
+        /// Draw a TriMeshModel as individual polygons with individual colors.
+        void drawTriMeshAsPolygons(
+            const VirtualRobot::TriMeshModel& trimesh,
+            float lineWidth, const std::vector<DrawColor>& faceColorsInner, const DrawColor& colorBorder,
+            const std::string& name, const std::string& layer = "");
+
+
         /// Indicate whether a topic is set, i.e. visualization is enabled.
         bool enabled() const
         {