TAMSVIZ
Visualization and annotation tool for ROS
shapes.cpp
1 // TAMSVIZ
2 // (c) 2020 Philipp Ruppel
3 
4 #include "shapes.h"
5 
6 #include "../core/log.h"
7 #include "../render/mesh.h"
8 
9 #include <geometric_shapes/shape_operations.h>
10 
11 ShapeDisplay::ShapeDisplay(const std::shared_ptr<Mesh> &mesh) {
12  if (mesh) {
13  _mesh_renderer = node()->create<MeshRenderer>(mesh, material());
14  }
15 }
16 
17 MeshData makePlane() {
18  MeshData mesh;
19 
20  mesh.indices.emplace_back(0);
21  mesh.indices.emplace_back(2);
22  mesh.indices.emplace_back(1);
23  mesh.indices.emplace_back(0);
24  mesh.indices.emplace_back(3);
25  mesh.indices.emplace_back(2);
26 
27  mesh.positions.emplace_back(-1, -1, 0);
28  mesh.positions.emplace_back(-1, 1, 0);
29  mesh.positions.emplace_back(1, 1, 0);
30  mesh.positions.emplace_back(1, -1, 0);
31 
32  mesh.texcoords.emplace_back(0, 0);
33  mesh.texcoords.emplace_back(0, 1);
34  mesh.texcoords.emplace_back(1, 1);
35  mesh.texcoords.emplace_back(1, 0);
36 
37  for (size_t i = 0; i < 4; i++) {
38  mesh.normals.emplace_back(0, 0, 1);
39  mesh.tangents.emplace_back(1, 0, 0);
40  mesh.bitangents.emplace_back(0, 1, 0);
41  }
42 
43  return mesh;
44 }
45 
46 MeshData makeBox() {
47  MeshData mesh;
48  mesh += Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()) *
49  Eigen::Translation3f(0, 0, 1) * makePlane();
50  mesh += Eigen::Translation3f(0, 0, 1) * makePlane();
51  for (int i = 0; i < 4; i++) {
52  mesh += Eigen::AngleAxisf(M_PI * 0.5 * i, Eigen::Vector3f::UnitZ()) *
53  Eigen::AngleAxisf(M_PI * 0.5, Eigen::Vector3f::UnitX()) *
54  Eigen::Translation3f(0, 0, 1) * makePlane();
55  }
56  return mesh;
57 }
58 
59 MeshData makeSphere(size_t segments, size_t rings) {
60  MeshData mesh;
61  for (size_t y = 0; y <= rings; y++) {
62  for (size_t x = 0; x <= segments; x++) {
63  float u = x * 1.0 / segments;
64  float v = y * 1.0 / rings;
65  mesh.texcoords.emplace_back(u, v);
66  Eigen::Vector3f n(-std::cos(u * 2 * M_PI) * std::cos((v - 0.5) * M_PI),
67  std::sin(u * 2 * M_PI) * std::cos((v - 0.5) * M_PI),
68  std::sin((v - 0.5) * M_PI));
69  mesh.positions.emplace_back(n);
70  mesh.normals.emplace_back(n);
71  }
72  }
73  for (size_t y = 0; y < rings; y++) {
74  for (size_t x = 0; x < segments; x++) {
75  mesh.indices.emplace_back((y + 0) * (segments + 1) + (x + 0));
76  mesh.indices.emplace_back((y + 1) * (segments + 1) + (x + 0));
77  mesh.indices.emplace_back((y + 0) * (segments + 1) + (x + 1));
78  mesh.indices.emplace_back((y + 0) * (segments + 1) + (x + 1));
79  mesh.indices.emplace_back((y + 1) * (segments + 1) + (x + 0));
80  mesh.indices.emplace_back((y + 1) * (segments + 1) + (x + 1));
81  }
82  }
83  return mesh;
84 }
85 
86 MeshData makeDisk(size_t segments) {
87  MeshData mesh;
88  mesh.positions.emplace_back(0, 0, 0);
89  mesh.texcoords.emplace_back(0.5, 0.5);
90  mesh.normals.emplace_back(0, 0, 1);
91  for (size_t i = 0; i <= segments; i++) {
92  float a = i * 1.0 / segments;
93  float x = std::sin(a * 2 * M_PI);
94  float y = std::cos(a * 2 * M_PI);
95  mesh.positions.emplace_back(x, y, 0.0);
96  mesh.texcoords.emplace_back(x * 0.5 + 0.5, y * 0.5 + 0.5);
97  mesh.normals.emplace_back(0, 0, 1);
98  }
99  for (size_t i = 0; i < segments; i++) {
100  mesh.indices.emplace_back(0);
101  mesh.indices.emplace_back(i + 2);
102  mesh.indices.emplace_back(i + 1);
103  }
104  return mesh;
105 }
106 
107 MeshData makeCylinder(size_t segments) {
108  MeshData mesh;
109  for (int s : {-1, 1}) {
110  for (size_t i = 0; i <= segments; i++) {
111  float a = i * 1.0 / segments;
112  float x = std::sin(a * 2 * M_PI);
113  float y = std::cos(a * 2 * M_PI);
114  mesh.positions.emplace_back(x, y, s);
115  mesh.texcoords.emplace_back(a, s * 0.5 + 0.5);
116  mesh.normals.emplace_back(x, y, 0);
117  }
118  }
119  for (size_t i = 0; i < segments; i++) {
120  mesh.indices.emplace_back(i + 0);
121  mesh.indices.emplace_back(segments + 1 + i + 0);
122  mesh.indices.emplace_back(segments + 1 + i + 1);
123  mesh.indices.emplace_back(i + 0);
124  mesh.indices.emplace_back(segments + 1 + i + 1);
125  mesh.indices.emplace_back(i + 1);
126  }
127  mesh += Eigen::Translation3f(0.0f, 0.0f, 1.0f) * makeDisk(segments);
128  mesh += Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()) *
129  Eigen::Translation3f(0.0f, 0.0f, 1.0f) * makeDisk(segments);
130  return mesh;
131 }
132 
133 MeshData makeRing(double inner, double outer, size_t segments) {
134  MeshData mesh;
135  for (double s : {0.0, 1.0}) {
136  for (size_t i = 0; i <= segments; i++) {
137  float a = i * 1.0 / segments;
138  float x = std::sin(a * 2 * M_PI);
139  float y = std::cos(a * 2 * M_PI);
140  double f = inner + (outer - inner) * s;
141  mesh.positions.emplace_back(x * f, y * f, 0);
142  mesh.texcoords.emplace_back(a, s);
143  mesh.normals.emplace_back(0, 0, 1);
144  }
145  }
146  for (size_t i = 0; i < segments; i++) {
147  mesh.indices.emplace_back(i + 0);
148  mesh.indices.emplace_back(segments + 1 + i + 0);
149  mesh.indices.emplace_back(segments + 1 + i + 1);
150  mesh.indices.emplace_back(i + 0);
151  mesh.indices.emplace_back(segments + 1 + i + 1);
152  mesh.indices.emplace_back(i + 1);
153  }
154  return mesh;
155 }
156 
157 MeshData makeCone(size_t segments) {
158  MeshData mesh;
159  mesh.positions.emplace_back(0, 0, 1);
160  mesh.texcoords.emplace_back(0.5, 0.5);
161  mesh.normals.emplace_back(0, 0, 0);
162  for (size_t i = 0; i <= segments; i++) {
163  float a = i * 1.0 / segments;
164  float x = std::sin(a * 2 * M_PI);
165  float y = std::cos(a * 2 * M_PI);
166  mesh.positions.emplace_back(x, y, 0.0);
167  mesh.texcoords.emplace_back(x * 0.5 + 0.5, y * 0.5 + 0.5);
168  mesh.normals.emplace_back(x, y, 1);
169  }
170  for (size_t i = 0; i < segments; i++) {
171  mesh.indices.emplace_back(0);
172  mesh.indices.emplace_back(i + 2);
173  mesh.indices.emplace_back(i + 1);
174  }
175  mesh +=
176  Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()) * makeDisk(segments);
177  return mesh;
178 }
179 
181  PlaneDisplay()
182  : ShapeDisplay(
183  std::make_shared<Mesh>(Eigen::Scaling(0.5f) * makePlane())) {}
184  PROPERTY(SizeVector2d, size, SizeVector2d(10, 10));
185  virtual void renderSync(const RenderSyncContext &context) override {
186  _mesh_renderer->pose(
187  Eigen::Affine3d(Eigen::Scaling(size().x(), size().y(), 1.0)));
188  ShapeDisplay::renderSync(context);
189  }
190 };
191 DECLARE_TYPE_C(PlaneDisplay, ShapeDisplay, Primitive);
192 
194  DiskDisplay() {}
195  PROPERTY(double, radius, 1.0, min = 0.0);
196  PROPERTY(int, segments, 32, min = 3);
197  virtual void renderSync(const RenderSyncContext &context) override {
198  if (_watcher.changed(segments())) {
199  LOG_DEBUG("make sphere");
200  _mesh_renderer = node()->create<MeshRenderer>(
201  std::make_shared<Mesh>(makeDisk(segments())), material());
202  }
203  _mesh_renderer->pose(Eigen::Affine3d(Eigen::Scaling(radius())));
204  ShapeDisplay::renderSync(context);
205  }
206 };
207 DECLARE_TYPE_C(DiskDisplay, ShapeDisplay, Primitive);
208 
210  BoxDisplay()
211  : ShapeDisplay(std::make_shared<Mesh>(Eigen::Scaling(0.5f) * makeBox())) {
212  }
213  PROPERTY(SizeVector3d, size);
214  virtual void renderSync(const RenderSyncContext &context) override {
215  _mesh_renderer->pose(Eigen::Affine3d(Eigen::Scaling(size())));
216  ShapeDisplay::renderSync(context);
217  }
218 };
219 DECLARE_TYPE_C(BoxDisplay, ShapeDisplay, Primitive);
220 
222  SphereDisplay() {}
223  PROPERTY(double, radius, 0.5, min = 0.0);
224  PROPERTY(int, rings, 16, min = 3);
225  PROPERTY(int, segments, 32, min = 3);
226  virtual void renderSync(const RenderSyncContext &context) override {
227  if (_watcher.changed(segments(), rings())) {
228  _mesh_renderer = node()->create<MeshRenderer>(
229  std::make_shared<Mesh>(makeSphere(segments(), rings())), material());
230  }
231  _mesh_renderer->pose(Eigen::Affine3d(Eigen::Scaling(radius())));
232  ShapeDisplay::renderSync(context);
233  }
234 };
235 DECLARE_TYPE_C(SphereDisplay, ShapeDisplay, Primitive);
236 
238  CylinderDisplay() {}
239  PROPERTY(double, radius, 0.5, min = 0.0);
240  PROPERTY(double, height, 1.0, min = 1e-6);
241  PROPERTY(int, segments, 32, min = 3);
242  virtual void renderSync(const RenderSyncContext &context) override {
243  if (_watcher.changed(segments())) {
244  _mesh_renderer = node()->create<MeshRenderer>(
245  std::make_shared<Mesh>(makeCylinder(segments())), material());
246  }
247  _mesh_renderer->pose(
248  Eigen::Affine3d(Eigen::Scaling(radius(), radius(), height())));
249  ShapeDisplay::renderSync(context);
250  }
251 };
252 DECLARE_TYPE_C(CylinderDisplay, ShapeDisplay, Primitive);
253 
255  ConeDisplay() {}
256  PROPERTY(double, radius, 0.5, min = 0.0);
257  PROPERTY(double, height, 1.0, min = 1e-6);
258  PROPERTY(int, segments, 32, min = 3);
259  virtual void renderSync(const RenderSyncContext &context) override {
260  if (_watcher.changed(segments())) {
261  _mesh_renderer = node()->create<MeshRenderer>(
262  std::make_shared<Mesh>(makeCone(segments())), material());
263  }
264  _mesh_renderer->pose(
265  Eigen::Affine3d(Eigen::Scaling(radius(), radius(), height())));
266  ShapeDisplay::renderSync(context);
267  }
268 };
269 DECLARE_TYPE_C(ConeDisplay, ShapeDisplay, Primitive);
Definition: mesh.h:11