VisualizationGrid.cpp
Go to the documentation of this file.
1#include "VisualizationGrid.h"
2
3#include <array>
4#include <cstddef>
5#include <cstdint>
6
7#include <Eigen/Core>
8#include <Eigen/Geometry>
9
10#include "RobotAPI/components/ArViz/Coin/ElementVisualizer.h"
11
12#include <Inventor/SbMatrix.h>
13#include <Inventor/nodes/SoCoordinate3.h>
14#include <Inventor/nodes/SoFaceSet.h>
15#include <Inventor/nodes/SoIndexedFaceSet.h>
16#include <Inventor/nodes/SoMaterial.h>
17#include <Inventor/nodes/SoMaterialBinding.h>
18#include <Inventor/nodes/SoMatrixTransform.h>
19#include <Inventor/nodes/SoNormal.h>
20#include <Inventor/nodes/SoSeparator.h>
21#include <Inventor/nodes/SoShapeHints.h>
22#include <Inventor/nodes/SoTexture2.h>
23#include <Inventor/nodes/SoTextureCoordinateBinding.h>
24#include <Inventor/nodes/SoUnits.h>
25
26namespace armarx::viz::coin
27{
28
29 VisualizationGrid::VisualizationGrid()
30 {
31 SoUnits* u2 = new SoUnits();
32 u2->units = SoUnits::MILLIMETERS;
33 node->addChild(u2);
34
35 const Eigen::Matrix4f initialPose = Eigen::Matrix4f::Identity();
36
37 origin = new SoMatrixTransform;
38 SbMatrix mat(reinterpret_cast<const SbMat*>(initialPose.data()));
39 origin->matrix.setValue(mat);
40 node->addChild(origin);
41
42 SoSeparator* pGrid = new SoSeparator;
43 // pGrid->ref();
44 SoUnits* u = new SoUnits();
45 u->units = SoUnits::MILLIMETERS;
46 pGrid->addChild(u);
47
48 pImagePlaneSoCoordinate3 = new SoCoordinate3;
49 // const double X = extents.x();
50 // const double Y = extents.y();
51 // pImagePlaneSoCoordinate3->point.set1Value(0, SbVec3f(X, Y, 0.0f));
52 // pImagePlaneSoCoordinate3->point.set1Value(1, SbVec3f(0, Y, 0.0f));
53 // pImagePlaneSoCoordinate3->point.set1Value(2, SbVec3f(0, 0, 0.0f));
54 // pImagePlaneSoCoordinate3->point.set1Value(3, SbVec3f(X, 0, 0.0f));
55
56 SoFaceSet* pSoFaceSet = new SoFaceSet;
57 pSoFaceSet->numVertices.set1Value(0, 4);
58
59 SoTextureCoordinateBinding* pSoTextureCoordinateBinding = new SoTextureCoordinateBinding;
60 pSoTextureCoordinateBinding->value.setValue(SoTextureCoordinateBinding::PER_VERTEX);
61
62 pSoTexture2 = new SoTexture2;
63
64 pSoTexture2->wrapS = pSoTexture2->wrapT = SoTexture2::REPEAT;
65 pGrid->addChild(pSoTextureCoordinateBinding);
66 pGrid->addChild(pSoTexture2);
67 pGrid->addChild(pImagePlaneSoCoordinate3);
68 pGrid->addChild(pSoFaceSet);
69 // pGrid->unrefNoDelete();
70
71 node->addChild(pGrid);
72 }
73
74 bool
75 VisualizationGrid::update(ElementType const& element)
76 {
77 // update origin
78 {
79 Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
80 pose.translation() = Eigen::Vector3f(element.pose.x, element.pose.y, element.pose.z);
81 Eigen::Quaternionf q{
82 element.pose.qw, element.pose.qx, element.pose.qy, element.pose.qz};
83 pose.linear() = q.toRotationMatrix();
84
85 // The coordinate system is different to the one used in ArmarX.
86 // Hence, we must rotate it.
87 // pose.linear() *= Eigen::AngleAxisf(M_PI / 2.0f, Eigen::Vector3f::UnitZ()).toRotationMatrix();
88 // pose.linear() *= Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()).toRotationMatrix();
89
90 SbMatrix mat(reinterpret_cast<const SbMat*>(pose.data()));
91 origin->matrix.setValue(mat);
92 }
93
94 // update grid extents
95 {
96 // intentionally swapped X and Y
97 const float X = element.sizeY * element.resolution;
98 const float Y = element.sizeX * element.resolution;
99
100 pImagePlaneSoCoordinate3->point.set1Value(0, SbVec3f(X, Y, 0.0f));
101 pImagePlaneSoCoordinate3->point.set1Value(1, SbVec3f(0, Y, 0.0f));
102 pImagePlaneSoCoordinate3->point.set1Value(2, SbVec3f(0, 0, 0.0f));
103 pImagePlaneSoCoordinate3->point.set1Value(3, SbVec3f(X, 0, 0.0f));
104 }
105
106
107 // update texture (grid values)
108 {
109 using Color = std::array<unsigned char, 4>; // RGBA
110
111 std::vector<Color> pixels(element.sizeX * element.sizeY);
112
113 for (std::size_t x = 0; x < static_cast<std::size_t>(element.sizeX); x++)
114 {
115 for (std::size_t y = 0; y < static_cast<std::size_t>(element.sizeY); y++)
116 {
117 const std::size_t idx = (x * element.sizeY) + y;
118
119 const auto& color = element.colors.at(idx);
120 pixels.at(idx) = Color{color.r, color.g, color.b, color.a};
121 }
122 }
123
124
125 pSoTexture2->image.setValue(SbVec2s{static_cast<std::int16_t>(element.sizeX),
126 static_cast<std::int16_t>(element.sizeY)},
127 4,
128 reinterpret_cast<const unsigned char*>(pixels.data()));
129 }
130
131
132 return true;
133 }
134} // namespace armarx::viz::coin
q
#define q
Eigen::Quaternionf
Quaternion< float, 0 > Quaternionf
Definition EigenForwardDeclarations.h:61
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
armarx::viz::coin::VisualizationGrid::update
bool update(ElementType const &element)
Definition VisualizationGrid.cpp:75