VisualizationPose.h
Go to the documentation of this file.
1#pragma once
2
3#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
4
5#include <RobotAPI/interface/ArViz/Elements.h>
6
7#include "ElementVisualizer.h"
8#include <Inventor/nodes/SoAsciiText.h>
9#include <Inventor/nodes/SoCube.h>
10#include <Inventor/nodes/SoMaterial.h>
11#include <Inventor/nodes/SoSeparator.h>
12#include <Inventor/nodes/SoTransform.h>
13#include <Inventor/nodes/SoTranslation.h>
14
15namespace armarx::viz::coin
16{
17 struct VisualizationPose : TypedElementVisualization<SoSeparator>
18 {
19 using ElementType = data::ElementPose;
20
21 static const int NumberOfBlocks = 3;
22
23 VisualizationPose()
24 {
25 for (int i = 0; i < 3; i++)
26 {
27 SoTransform* t = new SoTransform();
28 SoMaterial* m = new SoMaterial();
29 t_[i] = t;
30 m_[i] = m;
31
32 SoCube* c = new SoCube();
33 SoCube* c2 = new SoCube();
34 SoTransform* t2 = new SoTransform();
35 c_[i] = c;
36 c2_[i] = c2;
37 t2_[i] = t2;
38
39 SoSeparator* tmp1 = new SoSeparator();
40 tmp1->addChild(m);
41 tmp1->addChild(t);
42 tmp1->addChild(c);
43 node->addChild(tmp1);
44
45 SoSeparator* tmp2 = new SoSeparator();
46 SoMaterial* m2 = new SoMaterial();
47 m2->diffuseColor.setValue(1.0f, 1.0f, 1.0f);
48 tmp2->addChild(m2);
49
50 for (int j = 0; j < NumberOfBlocks; j++)
51 {
52 tmp2->addChild(t2);
53 tmp2->addChild(c2);
54 }
55
56 node->addChild(tmp2);
57 }
58
59 SoSeparator* textSep = new SoSeparator();
60 SoTranslation* moveT = new SoTranslation();
61 moveT->translation.setValue(2.0f, 2.0f, 0.0f);
62 textSep->addChild(moveT);
63 textNode = new SoAsciiText();
64 textSep->addChild(textNode);
65
66 node->addChild(textSep);
67 }
68
69 bool
70 update(ElementType const& element)
71 {
72 const float axisSize = 3.0f;
73 const float axisLength = 100.0f;
74 const int numberOfBlocks = NumberOfBlocks;
75
76 float scaling = 1.0f;
77
78 float blockSize = axisSize + 0.5f;
79 float blockWidth = 0.1f;
80
81 if (axisSize > 10.0f)
82 {
83 blockSize += axisSize / 10.0f;
84 blockWidth += axisSize / 10.0f;
85 }
86
87 float axisBlockTranslation = axisLength / numberOfBlocks;
88
89 for (int i = 0; i < 3; i++)
90 {
91 SoTransform* t = t_[i];
92 SoMaterial* m = m_[i];
93
94 float translation = (axisLength / 2.0f + axisSize / 2.0f) * scaling;
95 if (i == 0)
96 {
97 m->diffuseColor.setValue(1.0f, 0, 0);
98 t->translation.setValue(translation, 0, 0);
99 }
100 else if (i == 1)
101 {
102 m->diffuseColor.setValue(0, 1.0f, 0);
103 t->translation.setValue(0, translation, 0);
104 }
105 else
106 {
107 m->diffuseColor.setValue(0, 0, 1.0f);
108 t->translation.setValue(0, 0, translation);
109 }
110
111 SoCube* c = c_[i];
112 SoCube* c2 = c2_[i];
113 SoTransform* t2 = t2_[i];
114
115 if (i == 0)
116 {
117 c->width = axisLength * scaling;
118 c->height = axisSize * scaling;
119 c->depth = axisSize * scaling;
120 c2->width = blockWidth * scaling;
121 c2->height = blockSize * scaling;
122 c2->depth = blockSize * scaling;
123 t2->translation.setValue(axisBlockTranslation * scaling, 0, 0);
124 }
125 else if (i == 1)
126 {
127 c->height = axisLength * scaling;
128 c->width = axisSize * scaling;
129 c->depth = axisSize * scaling;
130 c2->width = blockSize * scaling;
131 c2->height = blockWidth * scaling;
132 c2->depth = blockSize * scaling;
133 t2->translation.setValue(0, axisBlockTranslation * scaling, 0);
134 }
135 else
136 {
137 c->depth = axisLength * scaling;
138 c->height = axisSize * scaling;
139 c->width = axisSize * scaling;
140 c2->width = blockSize * scaling;
141 c2->height = blockSize * scaling;
142 c2->depth = blockWidth * scaling;
143 t2->translation.setValue(0, 0, axisBlockTranslation * scaling);
144 }
145 }
146
147 textNode->string.setValue(element.id.c_str());
148
149 return true;
150 }
151
152 SoAsciiText* textNode;
153 std::array<SoTransform*, 3> t_;
154 std::array<SoMaterial*, 3> m_;
155 std::array<SoCube*, 3> c_;
156 std::array<SoCube*, 3> c2_;
157 std::array<SoTransform*, 3> t2_;
158 };
159} // namespace armarx::viz::coin
c
constexpr T c
Definition UnscentedKalmanFilterTest.cpp:46
armarx::viz::coin::VisualizationPose::update
bool update(ElementType const &element)
Definition VisualizationPose.h:70
armarx::viz::coin::VisualizationPose::t2_
std::array< SoTransform *, 3 > t2_
Definition VisualizationPose.h:157
armarx::viz::coin::VisualizationPose::t_
std::array< SoTransform *, 3 > t_
Definition VisualizationPose.h:153
armarx::viz::coin::VisualizationPose::m_
std::array< SoMaterial *, 3 > m_
Definition VisualizationPose.h:154