DebugDrawerUtils.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package ArmarX
19  * @author Mirko Waechter( mirko.waechter at kit dot edu)
20  * @date 2018
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <VirtualRobot/Visualization/TriMeshModel.h>
27 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
28 #include <Eigen/Core>
29 namespace armarx
30 {
32  {
33  public:
34  static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel& trimesh)
35  {
36  DebugDrawerTriMesh ddMesh;
37 
38  auto addVertex = [&](const Eigen::Vector3f & v)
39  {
40  ddMesh.vertices.push_back({v(0), v(1), v(2)});
41  return (int)ddMesh.vertices.size() - 1;
42  };
43 
44  auto addColor = [&](const VirtualRobot::VisualizationFactory::Color & c)
45  {
46  ddMesh.colors.push_back(DrawColor {c.r, c.g, c.b, c.transparency});
47  return (int)ddMesh.colors.size() - 1;
48  };
49  ddMesh.faces.reserve(trimesh.faces.size());
50  ddMesh.vertices.reserve(trimesh.vertices.size());
51  ddMesh.colors.reserve(trimesh.colors.size());
52 
53  for (auto& f : trimesh.faces)
54  {
55  DebugDrawerFace f2;
56  f2.vertex1.vertexID = addVertex(trimesh.vertices.at(f.id1));
57  f2.vertex2.vertexID = addVertex(trimesh.vertices.at(f.id2));
58  f2.vertex3.vertexID = addVertex(trimesh.vertices.at(f.id3));
59  if (f.idNormal1 != UINT_MAX)
60  {
61  f2.vertex1.normalID = addVertex(trimesh.normals.at(f.idNormal1));
62  f2.vertex2.normalID = addVertex(trimesh.normals.at(f.idNormal2));
63  f2.vertex3.normalID = addVertex(trimesh.normals.at(f.idNormal3));
64  }
65  else
66  {
67  // f2.vertex1.normalID = f2.vertex2.normalID = f2.vertex3.normalID = addVertex(f.normal);
68  f2.normal = {f.normal.x(), f.normal.y(), f.normal.z()};
69  }
70  f2.vertex1.colorID = addColor(trimesh.colors.at(f.idColor1));
71  f2.vertex2.colorID = addColor(trimesh.colors.at(f.idColor2));
72  f2.vertex3.colorID = addColor(trimesh.colors.at(f.idColor3));
73 
74  ddMesh.faces.push_back(f2);
75  }
76  return ddMesh;
77  }
78 
79  };
80 }
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::DebugDrawerUtils
Definition: DebugDrawerUtils.h:31
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DebugDrawerUtils::convertTriMesh
static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel &trimesh)
Definition: DebugDrawerUtils.h:34