UFO 1.0.0
An Efficient Probabilistic 3D Mapping Framework That Embraces the Unknown
Loading...
Searching...
No Matches
triangulate.hpp
1
2#ifndef UFO_VIZ_TRIANGULATE_HPP
3#define UFO_VIZ_TRIANGULATE_HPP
4
5// STL
6#include <cstdint>
7#include <vector>
8
9// UFO
10#include <ufo/geometry/cone.hpp>
11#include <ufo/geometry/cylinder.hpp>
12#include <ufo/geometry/line_segment.hpp>
13#include <ufo/geometry/sphere.hpp>
14#include <ufo/numeric/vec3.hpp>
15#include <ufo/plan/graph.hpp>
16#include <ufo/plan/path.hpp>
17
18struct Vertex {
19 Vertex(float x, float y, float z, float nx, float ny, float nz)
20 : pos{x, y, z}, _pad0(0.0f), normal{nx, ny, nz}, _pad1(0.0f)
21 {
22 }
23 Vertex(ufo::Vec3f pos, ufo::Vec3f normal)
24 : pos(pos), _pad0(0.0f), normal(normal), _pad1(0.0f)
25 {
26 }
27
28 ufo::Vec3f pos;
29 float _pad0;
30 ufo::Vec3f normal;
31 float _pad1;
32};
33
34template <class Vertex>
35struct TriangleList {
36 std::vector<Vertex> vertices;
37 std::vector<std::uint32_t> indices;
38};
39
40template <class Vertex>
41inline TriangleList<Vertex> triangulate(ufo::BS3 const& sphere, int num_slices,
42 int num_stacks);
43
44template <>
45inline TriangleList<Vertex> triangulate(ufo::BS3 const& sphere, int num_slices,
46 int num_stacks)
47{
48 TriangleList<Vertex> result;
49
50 // Vertices
51 for (int i = 0; i <= num_stacks; ++i) {
52 float phi = M_PI * i / num_stacks;
53 for (int j = 0; j <= num_slices; ++j) {
54 float theta = 2 * M_PI * j / num_slices;
55
56 float x = sphere.radius * std::sin(phi) * std::cos(theta) + sphere.center.x;
57 float y = sphere.radius * std::sin(phi) * std::sin(theta) + sphere.center.y;
58 float z = sphere.radius * std::cos(phi) + sphere.center.z;
59
60 float nx = (x - sphere.center.x) / sphere.radius;
61 float ny = (y - sphere.center.y) / sphere.radius;
62 float nz = (z - sphere.center.z) / sphere.radius;
63
64 result.vertices.push_back({x, y, z, nx, ny, nz});
65 }
66 }
67
68 // Indices
69 for (int i = 0; i < num_stacks; ++i) {
70 for (int j = 0; j < num_slices; ++j) {
71 int first = i * (num_slices + 1) + j;
72 int second = first + num_slices + 1;
73
74 result.indices.push_back(first);
75 result.indices.push_back(second);
76 result.indices.push_back(first + 1);
77
78 result.indices.push_back(second);
79 result.indices.push_back(second + 1);
80 result.indices.push_back(first + 1);
81 }
82 }
83
84 return result;
85}
86
87template <>
88inline TriangleList<ufo::Vec3f> triangulate(ufo::BS3 const& sphere, int num_slices,
89 int num_stacks)
90{
91 TriangleList<ufo::Vec3f> result;
92
93 // Vertices
94 for (int i = 0; i <= num_stacks; ++i) {
95 float phi = M_PI * i / num_stacks;
96 for (int j = 0; j <= num_slices; ++j) {
97 float theta = 2 * M_PI * j / num_slices;
98
99 float x = sphere.radius * std::sin(phi) * std::cos(theta) + sphere.center.x;
100 float y = sphere.radius * std::sin(phi) * std::sin(theta) + sphere.center.y;
101 float z = sphere.radius * std::cos(phi) + sphere.center.z;
102
103 result.vertices.push_back({x, y, z});
104 }
105 }
106
107 // Indices
108 for (int i = 0; i < num_stacks; ++i) {
109 for (int j = 0; j < num_slices; ++j) {
110 int first = i * (num_slices + 1) + j;
111 int second = first + num_slices + 1;
112
113 result.indices.push_back(first);
114 result.indices.push_back(second);
115 result.indices.push_back(first + 1);
116
117 result.indices.push_back(second);
118 result.indices.push_back(second + 1);
119 result.indices.push_back(first + 1);
120 }
121 }
122
123 return result;
124}
125
126template <class Vertex>
127inline TriangleList<Vertex> triangulate(ufo::Cylinder3 const& cylinder, int segments);
128
129template <>
130inline TriangleList<Vertex> triangulate(ufo::Cylinder3 const& cylinder, int segments)
131{
132 TriangleList<Vertex> result;
133
134 // Vertices
135 ufo::Vec3f direction = cylinder.center_2 - cylinder.center_1;
136 ufo::Vec3f axis = normalize(direction);
137
138 // Create a local coordinate system
139 ufo::Vec3f up(0.0f, 1.0f, 0.0f);
140 if (std::abs(dot(axis, up)) > 0.99f) {
141 up = ufo::Vec3f(1.0f, 0.0f, 0.0f);
142 }
143
144 ufo::Vec3f right = normalize(cross(up, axis));
145 ufo::Vec3f forward = normalize(cross(axis, right));
146
147 result.vertices.push_back({cylinder.center_1, -direction});
148 result.vertices.push_back({cylinder.center_2, direction});
149
150 float angle_step = 2 * M_PI / segments;
151
152 // Generate vertices for the top and bottom circles
153 for (int i = 0; i <= segments; ++i) {
154 float angle = i * angle_step;
155 float x = cylinder.radius * std::cos(angle);
156 float y = cylinder.radius * std::sin(angle);
157
158 ufo::Vec3f p = x * right + y * forward;
159
160 // Top circle vertex
161 ufo::Vec3f top_vertex = cylinder.center_1 + p;
162 result.vertices.push_back({top_vertex, p / cylinder.radius});
163
164 // Bottom circle vertex
165 ufo::Vec3f bottom_vertex = cylinder.center_2 + p;
166 result.vertices.push_back({bottom_vertex, p / cylinder.radius});
167 }
168
169 // Indices
170 // cap indices
171 for (int i = 0; i <= segments; ++i) {
172 result.indices.push_back(0); // Center vertex of the top cap
173 result.indices.push_back(2 * i);
174 result.indices.push_back(2 * i + 2);
175
176 result.indices.push_back(1); // Center vertex of the bottom cap
177 result.indices.push_back(2 * i + 1);
178 result.indices.push_back(2 * i + 3);
179 }
180
181 // Side surface indices
182 for (int i = 0; i < segments; ++i) {
183 int top_index = 2 * i + 2;
184 int bottom_index = top_index + 1;
185
186 result.indices.push_back(top_index);
187 result.indices.push_back(bottom_index);
188 result.indices.push_back(top_index + 2);
189
190 result.indices.push_back(bottom_index);
191 result.indices.push_back(bottom_index + 2);
192 result.indices.push_back(top_index + 2);
193 }
194
195 return result;
196}
197
198template <>
199inline TriangleList<ufo::Vec3f> triangulate(ufo::Cylinder3 const& cylinder, int segments)
200{
201 TriangleList<ufo::Vec3f> result;
202
203 // Vertices
204 ufo::Vec3f direction = cylinder.center_2 - cylinder.center_1;
205 ufo::Vec3f axis = normalize(direction);
206
207 // Create a local coordinate system
208 ufo::Vec3f up(0.0f, 1.0f, 0.0f);
209 if (std::abs(dot(axis, up)) > 0.99f) {
210 up = ufo::Vec3f(1.0f, 0.0f, 0.0f);
211 }
212
213 ufo::Vec3f right = normalize(cross(up, axis));
214 ufo::Vec3f forward = normalize(cross(axis, right));
215
216 result.vertices.push_back(cylinder.center_1);
217 result.vertices.push_back(cylinder.center_2);
218
219 float angle_step = 2 * M_PI / segments;
220
221 // Generate vertices for the top and bottom circles
222 for (int i = 0; i <= segments; ++i) {
223 float angle = i * angle_step;
224 float x = cylinder.radius * std::cos(angle);
225 float y = cylinder.radius * std::sin(angle);
226
227 ufo::Vec3f p = x * right + y * forward;
228
229 // Top circle vertex
230 ufo::Vec3f top_vertex = cylinder.center_1 + p;
231 result.vertices.push_back(top_vertex);
232
233 // Bottom circle vertex
234 ufo::Vec3f bottom_vertex = cylinder.center_2 + p;
235 result.vertices.push_back(bottom_vertex);
236 }
237
238 // Indices
239 // cap indices
240 for (int i = 0; i <= segments; ++i) {
241 result.indices.push_back(0); // Center vertex of the top cap
242 result.indices.push_back(2 * i);
243 result.indices.push_back(2 * i + 2);
244
245 result.indices.push_back(1); // Center vertex of the bottom cap
246 result.indices.push_back(2 * i + 1);
247 result.indices.push_back(2 * i + 3);
248 }
249
250 // Side surface indices
251 for (int i = 0; i < segments; ++i) {
252 int top_index = 2 * i + 2;
253 int bottom_index = top_index + 1;
254
255 result.indices.push_back(top_index);
256 result.indices.push_back(bottom_index);
257 result.indices.push_back(top_index + 2);
258
259 result.indices.push_back(bottom_index);
260 result.indices.push_back(bottom_index + 2);
261 result.indices.push_back(top_index + 2);
262 }
263
264 return result;
265}
266
267template <class Vertex>
268inline TriangleList<Vertex> triangulate(ufo::Cone3 const& cone, int segments);
269
270template <>
271inline TriangleList<Vertex> triangulate(ufo::Cone3 const& cone, int segments)
272{
273 TriangleList<Vertex> result;
274
275 auto dir = normalize(cone.tip - cone.base_center);
276
277 result.vertices.push_back({cone.base_center, -dir});
278 result.vertices.push_back({cone.tip, dir});
279
280 // Create a local coordinate system (basis)
281 ufo::Vec3f up(0.0f, 1.0f, 0.0f);
282 if (std::abs(dot(dir, up)) > 0.99f) {
283 up = ufo::Vec3f(1.0f, 0.0f, 0.0f);
284 }
285
286 ufo::Vec3f right = normalize(cross(up, dir));
287 ufo::Vec3f forward = normalize(cross(dir, right));
288
289 // Base circle vertices
290 float angle_step = 2 * M_PI / segments;
291
292 // Generate vertices for base circle
293 for (int i = 0; i <= segments; ++i) {
294 float angle = i * angle_step;
295 float x = cone.radius * std::cos(angle);
296 float y = cone.radius * std::sin(angle);
297
298 auto p = x * right + y * forward;
299 result.vertices.push_back({cone.base_center + p, p / cone.radius});
300 }
301
302 // Generate indices for base circle triangles
303 for (int i = 2; i <= segments + 1; ++i) {
304 result.indices.push_back(0); // Base center
305 result.indices.push_back(i);
306 result.indices.push_back(i + 1);
307 }
308
309 // Generate the indices for the side triangles
310 for (int i = 2; i <= segments + 1; ++i) {
311 result.indices.push_back(1); // Tip index
312 result.indices.push_back(i);
313 result.indices.push_back(i + 1);
314 }
315
316 return result;
317}
318
319template <>
320inline TriangleList<ufo::Vec3f> triangulate(ufo::Cone3 const& cone, int segments)
321{
322 TriangleList<ufo::Vec3f> result;
323
324 auto dir = normalize(cone.tip - cone.base_center);
325
326 result.vertices.push_back(cone.base_center);
327 result.vertices.push_back(cone.tip);
328
329 // Create a local coordinate system (basis)
330 ufo::Vec3f up(0.0f, 1.0f, 0.0f);
331 if (std::abs(dot(dir, up)) > 0.99f) {
332 up = ufo::Vec3f(1.0f, 0.0f, 0.0f);
333 }
334
335 ufo::Vec3f right = normalize(cross(up, dir));
336 ufo::Vec3f forward = normalize(cross(dir, right));
337
338 // Base circle vertices
339 float angle_step = 2 * M_PI / segments;
340
341 // Generate vertices for base circle
342 for (int i = 0; i <= segments; ++i) {
343 float angle = i * angle_step;
344 float x = cone.radius * std::cos(angle);
345 float y = cone.radius * std::sin(angle);
346
347 auto p = x * right + y * forward;
348 result.vertices.push_back(cone.base_center + p);
349 }
350
351 // Generate indices for base circle triangles
352 for (int i = 2; i <= segments + 1; ++i) {
353 result.indices.push_back(0); // Base center
354 result.indices.push_back(i);
355 result.indices.push_back(i + 1);
356 }
357
358 // Generate the indices for the side triangles
359 for (int i = 2; i <= segments + 1; ++i) {
360 result.indices.push_back(1); // Tip index
361 result.indices.push_back(i);
362 result.indices.push_back(i + 1);
363 }
364
365 return result;
366}
367
368template <class Vertex>
369inline TriangleList<Vertex> triangulate(ufo::LineSegment3 const& ls, int segments,
370 float radius)
371{
372 ufo::Cylinder3 cylinder(ls.start, ls.end, radius);
373 return triangulate<Vertex>(cylinder, segments);
374}
375
376template <class Vertex>
377inline TriangleList<Vertex> triangulateArrow(ufo::Vec3f start, ufo::Vec3f tip_end,
378 float radius, float tip_height,
379 int num_slices)
380{
381 TriangleList<Vertex> result;
382
383 auto dir = normalize(tip_end - start);
384 auto b = tip_end - dir * tip_height;
385
386 auto cylinder = triangulate<Vertex>(ufo::Cylinder(start, b, radius), num_slices);
387 auto cone = triangulate<Vertex>(ufo::Cone(b, tip_end, 2 * radius), num_slices);
388 for (std::size_t i{}; i < cone.indices.size(); ++i) {
389 cone.indices[i] += cylinder.vertices.size();
390 }
391
392 result.vertices.insert(result.vertices.end(), cylinder.vertices.begin(),
393 cylinder.vertices.end());
394 result.indices.insert(result.indices.end(), cylinder.indices.begin(),
395 cylinder.indices.end());
396
397 result.vertices.insert(result.vertices.end(), cone.vertices.begin(),
398 cone.vertices.end());
399 result.indices.insert(result.indices.end(), cone.indices.begin(), cone.indices.end());
400
401 return result;
402}
403
404template <class Vertex>
405inline TriangleList<Vertex> triangulate(ufo::PlanGraph<3, float> const& graph,
406 float node_radius, float edge_radius,
407 int segments, bool edge_as_arrow = false,
408 float arrow_head_length = 0.0f)
409{
410 // TODO maybe separate Nodes and edges for easier coloring
411
412 TriangleList<Vertex> result;
413 for (auto [p, n] : graph) {
414 auto center = static_cast<ufo::Vec3f>(*n);
415
416 // make a sphere for each node
417 auto sphere =
418 triangulate<Vertex>(ufo::BS3(center, node_radius), segments, segments / 2);
419 for (std::size_t i{}; i < sphere.indices.size(); ++i) {
420 sphere.indices[i] += result.vertices.size();
421 }
422
423 result.vertices.insert(result.vertices.end(), sphere.vertices.begin(),
424 sphere.vertices.end());
425 result.indices.insert(result.indices.end(), sphere.indices.begin(),
426 sphere.indices.end());
427
428 // make a cylinder for each edge
429 // TODO handle bidirectional edges: don't add multiple cylinder per edge pair
430 for (auto e : n->edges) {
431 auto start = static_cast<ufo::Vec3f>(*(e.from));
432 auto end = static_cast<ufo::Vec3f>(*(e.to));
433 auto t = edge_as_arrow ? triangulateArrow<Vertex>(start, end, edge_radius,
434 arrow_head_length, segments)
435 : triangulate<Vertex>(
436 ufo::Cylinder3(start, end, edge_radius), segments);
437 for (std::size_t i{}; i < t.indices.size(); ++i) {
438 t.indices[i] += result.vertices.size();
439 }
440
441 result.vertices.insert(result.vertices.end(), t.vertices.begin(), t.vertices.end());
442 result.indices.insert(result.indices.end(), t.indices.begin(), t.indices.end());
443 }
444 }
445
446 return result;
447}
448
449template <class Vertex>
450inline TriangleList<Vertex> triangulate(ufo::PlanPath<3, float> const& path, float radius,
451 int segments, bool use_arrow = true,
452 float arrow_head_height = 0.5f)
453{
454 TriangleList<Vertex> result;
455
456 for (std::size_t i{}; i < path.size() - 1; ++i) {
457 auto a = static_cast<ufo::Vec3f>(*path[i]);
458 auto b = static_cast<ufo::Vec3f>(*path[i + 1]);
459
460 auto t = use_arrow
461 ? triangulateArrow<Vertex>(a, b, radius, arrow_head_height, segments)
462 : triangulate<Vertex>(ufo::Cylinder3(a, b, radius), segments);
463 for (std::size_t i{}; i < t.indices.size(); ++i) {
464 t.indices[i] += result.vertices.size();
465 }
466
467 result.vertices.insert(result.vertices.end(), t.vertices.begin(), t.vertices.end());
468 result.indices.insert(result.indices.end(), t.indices.begin(), t.indices.end());
469 }
470
471 return result;
472}
473
474#endif // UFO_VIZ_TRIANGULATE_HPP
ufo::angle
T angle(Quat< T > const &q) noexcept
Extracts the rotation angle (in radians) from a unit quaternion.
Definition quat.hpp:852
ufo::dot
constexpr T dot(Quat< T > const &a, Quat< T > const &b) noexcept
Computes the four-component dot product a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z.
Definition quat.hpp:643
ufo
All vision-related classes and functions.
Definition cloud.hpp:49
ufo::b
constexpr T b(Lab< T, Flags > color) noexcept
Returns the un-weighted blue–yellow axis value.
Definition lab.hpp:326
ufo::a
constexpr T a(Lab< T, Flags > color) noexcept
Returns the un-weighted green–red axis value.
Definition lab.hpp:310
ufo::normalize
constexpr Quat< T > normalize(Quat< T > const &q) noexcept
Returns a unit quaternion in the same direction as q.
Definition quat.hpp:679
ufo::cross
constexpr Quat< T > cross(Quat< T > const &q1, Quat< T > const &q2) noexcept
Computes the Hamilton cross product of two quaternions.
Definition quat.hpp:721
ufo::axis
Vec< 3, T > axis(Quat< T > const &q) noexcept
Extracts the unit rotation axis from a unit quaternion.
Definition quat.hpp:869
ufo::Cylinder
Cylinder in Dim-dimensional space.
Definition cylinder.hpp:67
ufo::Cylinder::radius
T radius
The radius of the cylinder.
Definition cylinder.hpp:83
ufo::Cylinder::end
Vec< Dim, T > end
The end point of the second cap of the cylinder.
Definition cylinder.hpp:78
ufo::LineSegment
Line segment in Dim-dimensional space.
Definition line_segment.hpp:68
ufo::LineSegment::start
Vec< Dim, T > start
The start point of the line segment.
Definition line_segment.hpp:74
ufo::LineSegment::end
Vec< Dim, T > end
The end point of the line segment.
Definition line_segment.hpp:79