added convenience function for drawing trishmeshes from simox in debugdrawer

source/RobotAPI/components/DebugDrawer/CMakeLists.txt

@@ -19,7 +19,9 @@ endif()
 set(COMPONENT_LIBS ArmarXCore RobotAPIInterfaces RobotAPICore ${Simox_LIBRARIES})
 
 set(SOURCES DebugDrawerComponent.cpp)
-set(HEADERS DebugDrawerComponent.h)
+set(HEADERS DebugDrawerComponent.h
+    DebugDrawerUtils.h)
+
 
 armarx_add_component("${SOURCES}" "${HEADERS}")
 

source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    ArmarX
+ * @author     Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date       2018
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <VirtualRobot/Visualization/TriMeshModel.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <Eigen/Core>
+namespace armarx
+{
+    class DebugDrawerUtils
+    {
+    public:
+        static DebugDrawerTriMesh 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;
+        }
+
+    };
+}