From 4ae39d00e9363303ddd6cfbb2f24dd852f5d3d3c Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 30 Jul 2018 15:04:34 +0200
Subject: [PATCH] added convenience function for drawing trishmeshes from simox
in debugdrawer
---
.../components/DebugDrawer/CMakeLists.txt | 4 +-
.../components/DebugDrawer/DebugDrawerUtils.h | 80 +++++++++++++++++++
2 files changed, 83 insertions(+), 1 deletion(-)
create mode 100644 source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
diff --git a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt b/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
index b5462738c..754e65a5f 100644
--- a/source/RobotAPI/components/DebugDrawer/CMakeLists.txt
+++ b/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}")
diff --git a/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
new file mode 100644
index 000000000..b50b0a9d9
--- /dev/null
+++ b/source/RobotAPI/components/DebugDrawer/DebugDrawerUtils.h
@@ -0,0 +1,80 @@
+/*
+ * 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;
+ }
+
+ };
+}
--
GitLab