1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
|
// AABB tree built upon external data set, referencing the external data by integer indices.
// The AABB tree balancing and traversal (ray casting, closest triangle of an indexed triangle mesh)
// were adapted from libigl AABB.{cpp,hpp} Copyright (C) 2015 Alec Jacobson <alecjacobson@gmail.com>
// while the implicit balanced tree representation and memory optimizations are Vojtech's.
#ifndef slic3r_AABBTreeIndirect_hpp_
#define slic3r_AABBTreeIndirect_hpp_
#include <algorithm>
#include <limits>
#include <type_traits>
#include <vector>
#include "Utils.hpp" // for next_highest_power_of_2()
extern "C"
{
// Ray-Triangle Intersection Test Routines by Tomas Moller, May 2000
#include <igl/raytri.c>
}
// Definition of the ray intersection hit structure.
#include <igl/Hit.h>
namespace Slic3r {
namespace AABBTreeIndirect {
// Static balanced AABB tree for raycasting and closest triangle search.
// The balanced tree is built over a single large std::vector of nodes, where the children of nodes
// are addressed implicitely using a power of two indexing rule.
// Memory for a full balanced tree is allocated, but not all nodes at the last level are used.
// This may seem like a waste of memory, but one saves memory for the node links and there is zero
// overhead of a memory allocator management (usually the memory allocator adds at least one pointer
// before the memory returned). However, allocating memory in a single vector is very fast even
// in multi-threaded environment and it is cache friendly.
//
// A balanced tree is built upon a vector of bounding boxes and their centroids, storing the reference
// to the source entity (a 3D triangle, a 2D segment etc, a 3D or 2D point etc).
// The source bounding boxes may have an epsilon applied to fight numeric rounding errors when
// traversing the AABB tree.
template<int ANumDimensions, typename ACoordType>
class Tree
{
public:
static constexpr int NumDimensions = ANumDimensions;
using CoordType = ACoordType;
using VectorType = Eigen::Matrix<CoordType, NumDimensions, 1, Eigen::DontAlign>;
using BoundingBox = Eigen::AlignedBox<CoordType, NumDimensions>;
// Following could be static constexpr size_t, but that would not link in C++11
enum : size_t {
// Node is not used.
npos = size_t(-1),
// Inner node (not leaf).
inner = size_t(-2)
};
// Single node of the implicit balanced AABB tree. There are no links to the children nodes,
// as these links are calculated implicitely using a power of two rule.
struct Node {
// Index of the external source entity, for which this AABB tree was built, npos for internal nodes.
size_t idx = npos;
// Bounding box around this entity, possibly with epsilons applied to fight numeric rounding errors
// when traversing the AABB tree.
BoundingBox bbox;
bool is_valid() const { return this->idx != npos; }
bool is_inner() const { return this->idx == inner; }
bool is_leaf() const { return ! this->is_inner(); }
template<typename SourceNode>
void set(const SourceNode &rhs) {
this->idx = rhs.idx();
this->bbox = rhs.bbox();
}
};
void clear() { m_nodes.clear(); }
// SourceNode shall implement
// size_t SourceNode::idx() const
// - Index to the outside entity (triangle, edge, point etc).
// const VectorType& SourceNode::centroid() const
// - Centroid of this node. The centroid is used for balancing the tree.
// const BoundingBox& SourceNode::bbox() const
// - Bounding box of this node, likely expanded with epsilon to account for numeric rounding during tree traversal.
// Union of bounding boxes at a single level of the AABB tree is used for deciding the longest axis aligned dimension
// to split around.
template<typename SourceNode>
void build(std::vector<SourceNode> &&input)
{
if (input.empty())
clear();
else {
// Allocate enough memory for a full binary tree.
m_nodes.assign(next_highest_power_of_2(input.size()) * 2 - 1, Node());
build_recursive(input, 0, 0, input.size() - 1);
}
input.clear();
}
const std::vector<Node>& nodes() const { return m_nodes; }
const Node& node(size_t idx) const { return m_nodes[idx]; }
bool empty() const { return m_nodes.empty(); }
// Addressing the child nodes using the power of two rule.
static size_t left_child_idx(size_t idx) { return idx * 2 + 1; }
static size_t right_child_idx(size_t idx) { return left_child_idx(idx) + 1; }
const Node& left_child(size_t idx) const { return m_nodes[left_child_idx(idx)]; }
const Node& right_child(size_t idx) const { return m_nodes[right_child_idx(idx)]; }
template<typename SourceNode>
void build(const std::vector<SourceNode> &input)
{
std::vector<SourceNode> copy(input);
this->build(std::move(copy));
}
private:
// Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension.
template<typename SourceNode>
void build_recursive(std::vector<SourceNode> &input, size_t node, const size_t left, const size_t right)
{
assert(node < m_nodes.size());
assert(left <= right);
if (left == right) {
// Insert a node into the balanced tree.
m_nodes[node].set(input[left]);
return;
}
// Calculate bounding box of the input.
BoundingBox bbox(input[left].bbox());
for (size_t i = left + 1; i <= right; ++ i)
bbox.extend(input[i].bbox());
int dimension = -1;
bbox.diagonal().maxCoeff(&dimension);
// Partition the input to left / right pieces of the same length to produce a balanced tree.
size_t center = (left + right) / 2;
partition_input(input, size_t(dimension), left, right, center);
// Insert an inner node into the tree. Inner node does not reference any input entity (triangle, line segment etc).
m_nodes[node].idx = inner;
m_nodes[node].bbox = bbox;
build_recursive(input, node * 2 + 1, left, center);
build_recursive(input, node * 2 + 2, center + 1, right);
}
// Partition the input m_nodes <left, right> at "k" and "dimension" using the QuickSelect method:
// https://en.wikipedia.org/wiki/Quickselect
// Items left of the k'th item are lower than the k'th item in the "dimension",
// items right of the k'th item are higher than the k'th item in the "dimension",
template<typename SourceNode>
void partition_input(std::vector<SourceNode> &input, const size_t dimension, size_t left, size_t right, const size_t k) const
{
while (left < right) {
size_t center = (left + right) / 2;
CoordType pivot;
{
// Bubble sort the input[left], input[center], input[right], so that a median of the three values
// will end up in input[center].
CoordType left_value = input[left ].centroid()(dimension);
CoordType center_value = input[center].centroid()(dimension);
CoordType right_value = input[right ].centroid()(dimension);
if (left_value > center_value) {
std::swap(input[left], input[center]);
std::swap(left_value, center_value);
}
if (left_value > right_value) {
std::swap(input[left], input[right]);
right_value = left_value;
}
if (center_value > right_value) {
std::swap(input[center], input[right]);
center_value = right_value;
}
pivot = center_value;
}
if (right <= left + 2)
// The <left, right> interval is already sorted.
break;
size_t i = left;
size_t j = right - 1;
std::swap(input[center], input[j]);
// Partition the set based on the pivot.
for (;;) {
// Skip left points that are already at correct positions.
// Search will certainly stop at position (right - 1), which stores the pivot.
while (input[++ i].centroid()(dimension) < pivot) ;
// Skip right points that are already at correct positions.
while (input[-- j].centroid()(dimension) > pivot && i < j) ;
if (i >= j)
break;
std::swap(input[i], input[j]);
}
// Restore pivot to the center of the sequence.
std::swap(input[i], input[right - 1]);
// Which side the kth element is in?
if (k < i)
right = i - 1;
else if (k == i)
// Sequence is partitioned, kth element is at its place.
break;
else
left = i + 1;
}
}
// The balanced tree storage.
std::vector<Node> m_nodes;
};
using Tree2f = Tree<2, float>;
using Tree3f = Tree<3, float>;
using Tree2d = Tree<2, double>;
using Tree3d = Tree<3, double>;
namespace detail {
template<typename AVertexType, typename AIndexedFaceType, typename ATreeType, typename AVectorType>
struct RayIntersector {
using VertexType = AVertexType;
using IndexedFaceType = AIndexedFaceType;
using TreeType = ATreeType;
using VectorType = AVectorType;
const std::vector<VertexType> &vertices;
const std::vector<IndexedFaceType> &faces;
const TreeType &tree;
const VectorType origin;
const VectorType dir;
const VectorType invdir;
};
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
struct RayIntersectorHits : RayIntersector<VertexType, IndexedFaceType, TreeType, VectorType> {
std::vector<igl::Hit> hits;
};
//FIXME implement SSE for float AABB trees with float ray queries.
// SSE/SSE2 is supported by any Intel/AMD x64 processor.
// SSE support requires 16 byte alignment of the AABB nodes, representing the bounding boxes with 4+4 floats,
// storing the node index as the 4th element of the bounding box min value etc.
// https://www.flipcode.com/archives/SSE_RayBox_Intersection_Test.shtml
template <typename Derivedsource, typename Deriveddir, typename Scalar>
inline bool ray_box_intersect_invdir(
const Eigen::MatrixBase<Derivedsource> &origin,
const Eigen::MatrixBase<Deriveddir> &inv_dir,
Eigen::AlignedBox<Scalar,3> box,
const Scalar &t0,
const Scalar &t1) {
// http://people.csail.mit.edu/amy/papers/box-jgt.pdf
// "An Efficient and Robust Ray–Box Intersection Algorithm"
if (inv_dir.x() < 0)
std::swap(box.min().x(), box.max().x());
if (inv_dir.y() < 0)
std::swap(box.min().y(), box.max().y());
Scalar tmin = (box.min().x() - origin.x()) * inv_dir.x();
Scalar tymax = (box.max().y() - origin.y()) * inv_dir.y();
if (tmin > tymax)
return false;
Scalar tmax = (box.max().x() - origin.x()) * inv_dir.x();
Scalar tymin = (box.min().y() - origin.y()) * inv_dir.y();
if (tymin > tmax)
return false;
if (tymin > tmin)
tmin = tymin;
if (tymax < tmax)
tmax = tymax;
if (inv_dir.z() < 0)
std::swap(box.min().z(), box.max().z());
Scalar tzmin = (box.min().z() - origin.z()) * inv_dir.z();
if (tzmin > tmax)
return false;
Scalar tzmax = (box.max().z() - origin.z()) * inv_dir.z();
if (tmin > tzmax)
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return tmin < t1 && tmax > t0;
}
template<typename V, typename W>
std::enable_if_t<std::is_same<typename V::Scalar, double>::value && std::is_same<typename W::Scalar, double>::value, bool>
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W v2, double &t, double &u, double &v) {
return intersect_triangle1(const_cast<double*>(origin.data()), const_cast<double*>(dir.data()),
const_cast<double*>(v0.data()), const_cast<double*>(v1.data()), const_cast<double*>(v2.data()),
&t, &u, &v);
}
template<typename V, typename W>
std::enable_if_t<std::is_same<typename V::Scalar, double>::value && !std::is_same<typename W::Scalar, double>::value, bool>
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W v2, double &t, double &u, double &v) {
using Vector = Eigen::Matrix<double, 3, 1>;
Vector w0 = v0.template cast<double>();
Vector w1 = v1.template cast<double>();
Vector w2 = v2.template cast<double>();
return intersect_triangle1(const_cast<double*>(origin.data()), const_cast<double*>(dir.data()),
w0.data(), w1.data(), w2.data(), &t, &u, &v);
}
template<typename V, typename W>
std::enable_if_t<! std::is_same<typename V::Scalar, double>::value && std::is_same<typename W::Scalar, double>::value, bool>
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W v2, double &t, double &u, double &v) {
using Vector = Eigen::Matrix<double, 3, 1>;
Vector o = origin.template cast<double>();
Vector d = dir.template cast<double>();
return intersect_triangle1(o.data(), d.data(), const_cast<double*>(v0.data()), const_cast<double*>(v1.data()), const_cast<double*>(v2.data()), &t, &u, &v);
}
template<typename V, typename W>
std::enable_if_t<! std::is_same<typename V::Scalar, double>::value && ! std::is_same<typename W::Scalar, double>::value, bool>
intersect_triangle(const V &origin, const V &dir, const W &v0, const W &v1, const W v2, double &t, double &u, double &v) {
using Vector = Eigen::Matrix<double, 3, 1>;
Vector o = origin.template cast<double>();
Vector d = dir.template cast<double>();
Vector w0 = v0.template cast<double>();
Vector w1 = v1.template cast<double>();
Vector w2 = v2.template cast<double>();
return intersect_triangle1(o.data(), d.data(), w0.data(), w1.data(), w2.data(), &t, &u, &v);
}
template<typename RayIntersectorType, typename Scalar>
static inline bool intersect_ray_recursive_first_hit(
RayIntersectorType &ray_intersector,
size_t node_idx,
Scalar min_t,
igl::Hit &hit)
{
const auto &node = ray_intersector.tree.node(node_idx);
assert(node.is_valid());
if (! ray_box_intersect_invdir(ray_intersector.origin, ray_intersector.invdir, node.bbox.template cast<Scalar>(), Scalar(0), min_t))
return false;
if (node.is_leaf()) {
// shoot ray, record hit
auto face = ray_intersector.faces[node.idx];
double t, u, v;
if (intersect_triangle(
ray_intersector.origin, ray_intersector.dir,
ray_intersector.vertices[face(0)], ray_intersector.vertices[face(1)], ray_intersector.vertices[face(2)],
t, u, v)
&& t > 0.) {
hit = igl::Hit { int(node.idx), -1, float(u), float(v), float(t) };
return true;
} else
return false;
} else {
// Left / right child node index.
size_t left = node_idx * 2 + 1;
size_t right = left + 1;
igl::Hit left_hit;
igl::Hit right_hit;
bool left_ret = intersect_ray_recursive_first_hit(ray_intersector, left, min_t, left_hit);
if (left_ret && left_hit.t < min_t) {
min_t = left_hit.t;
hit = left_hit;
} else
left_ret = false;
bool right_ret = intersect_ray_recursive_first_hit(ray_intersector, right, min_t, right_hit);
if (right_ret && right_hit.t < min_t)
hit = right_hit;
else
right_ret = false;
return left_ret || right_ret;
}
}
template<typename RayIntersectorType>
static inline void intersect_ray_recursive_all_hits(RayIntersectorType &ray_intersector, size_t node_idx)
{
using Scalar = typename RayIntersectorType::VectorType::Scalar;
const auto &node = ray_intersector.tree.node(node_idx);
assert(node.is_valid());
if (! ray_box_intersect_invdir(ray_intersector.origin, ray_intersector.invdir, node.bbox.template cast<Scalar>(),
Scalar(0), std::numeric_limits<Scalar>::infinity()))
return;
if (node.is_leaf()) {
auto face = ray_intersector.faces[node.idx];
double t, u, v;
if (intersect_triangle(
ray_intersector.origin, ray_intersector.dir,
ray_intersector.vertices[face(0)], ray_intersector.vertices[face(1)], ray_intersector.vertices[face(2)],
t, u, v)
&& t > 0.) {
ray_intersector.hits.emplace_back(igl::Hit{ int(node.idx), -1, float(u), float(v), float(t) });
}
} else {
// Left / right child node index.
size_t left = node_idx * 2 + 1;
size_t right = left + 1;
intersect_ray_recursive_all_hits(ray_intersector, left);
intersect_ray_recursive_all_hits(ray_intersector, right);
}
}
// Nothing to do with COVID-19 social distancing.
template<typename AVertexType, typename AIndexedFaceType, typename ATreeType, typename AVectorType>
struct IndexedTriangleSetDistancer {
using VertexType = AVertexType;
using IndexedFaceType = AIndexedFaceType;
using TreeType = ATreeType;
using VectorType = AVectorType;
const std::vector<VertexType> &vertices;
const std::vector<IndexedFaceType> &faces;
const TreeType &tree;
const VectorType origin;
};
// Real-time collision detection, Ericson, Chapter 5
template<typename Vector>
static inline Vector closest_point_to_triangle(const Vector &p, const Vector &a, const Vector &b, const Vector &c)
{
using Scalar = typename Vector::Scalar;
// Check if P in vertex region outside A
Vector ab = b - a;
Vector ac = c - a;
Vector ap = p - a;
Scalar d1 = ab.dot(ap);
Scalar d2 = ac.dot(ap);
if (d1 <= 0 && d2 <= 0)
return a;
// Check if P in vertex region outside B
Vector bp = p - b;
Scalar d3 = ab.dot(bp);
Scalar d4 = ac.dot(bp);
if (d3 >= 0 && d4 <= d3)
return b;
// Check if P in edge region of AB, if so return projection of P onto AB
Scalar vc = d1*d4 - d3*d2;
if (a != b && vc <= 0 && d1 >= 0 && d3 <= 0) {
Scalar v = d1 / (d1 - d3);
return a + v * ab;
}
// Check if P in vertex region outside C
Vector cp = p - c;
Scalar d5 = ab.dot(cp);
Scalar d6 = ac.dot(cp);
if (d6 >= 0 && d5 <= d6)
return c;
// Check if P in edge region of AC, if so return projection of P onto AC
Scalar vb = d5*d2 - d1*d6;
if (vb <= 0 && d2 >= 0 && d6 <= 0) {
Scalar w = d2 / (d2 - d6);
return a + w * ac;
}
// Check if P in edge region of BC, if so return projection of P onto BC
Scalar va = d3*d6 - d5*d4;
if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) {
Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
return b + w * (c - b);
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
Scalar denom = Scalar(1.0) / (va + vb + vc);
Scalar v = vb * denom;
Scalar w = vc * denom;
return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w
};
template<typename IndexedTriangleSetDistancerType, typename Scalar>
static inline Scalar squared_distance_to_indexed_triangle_set_recursive(
IndexedTriangleSetDistancerType &distancer,
size_t node_idx,
Scalar low_sqr_d,
Scalar up_sqr_d,
size_t &i,
Eigen::PlainObjectBase<typename IndexedTriangleSetDistancerType::VectorType> &c)
{
using Vector = typename IndexedTriangleSetDistancerType::VectorType;
if (low_sqr_d > up_sqr_d)
return low_sqr_d;
// Save the best achieved hit.
auto set_min = [&i, &c, &up_sqr_d](const Scalar sqr_d_candidate, const size_t i_candidate, const Vector &c_candidate) {
if (sqr_d_candidate < up_sqr_d) {
i = i_candidate;
c = c_candidate;
up_sqr_d = sqr_d_candidate;
}
};
const auto &node = distancer.tree.node(node_idx);
assert(node.is_valid());
if (node.is_leaf())
{
const auto &triangle = distancer.faces[node.idx];
Vector c_candidate = closest_point_to_triangle<Vector>(
distancer.origin,
distancer.vertices[triangle(0)].template cast<Scalar>(),
distancer.vertices[triangle(1)].template cast<Scalar>(),
distancer.vertices[triangle(2)].template cast<Scalar>());
set_min((c_candidate - distancer.origin).squaredNorm(), node.idx, c_candidate);
}
else
{
size_t left_node_idx = node_idx * 2 + 1;
size_t right_node_idx = left_node_idx + 1;
const auto &node_left = distancer.tree.node(left_node_idx);
const auto &node_right = distancer.tree.node(right_node_idx);
assert(node_left.is_valid());
assert(node_right.is_valid());
bool looked_left = false;
bool looked_right = false;
const auto &look_left = [&]()
{
size_t i_left;
Vector c_left = c;
Scalar sqr_d_left = squared_distance_to_indexed_triangle_set_recursive(distancer, left_node_idx, low_sqr_d, up_sqr_d, i_left, c_left);
set_min(sqr_d_left, i_left, c_left);
looked_left = true;
};
const auto &look_right = [&]()
{
size_t i_right;
Vector c_right = c;
Scalar sqr_d_right = squared_distance_to_indexed_triangle_set_recursive(distancer, right_node_idx, low_sqr_d, up_sqr_d, i_right, c_right);
set_min(sqr_d_right, i_right, c_right);
looked_right = true;
};
// must look left or right if in box
using BBoxScalar = typename IndexedTriangleSetDistancerType::TreeType::BoundingBox::Scalar;
if (node_left.bbox.contains(distancer.origin.template cast<BBoxScalar>()))
look_left();
if (node_right.bbox.contains(distancer.origin.template cast<BBoxScalar>()))
look_right();
// if haven't looked left and could be less than current min, then look
Scalar left_up_sqr_d = node_left.bbox.squaredExteriorDistance(distancer.origin);
Scalar right_up_sqr_d = node_right.bbox.squaredExteriorDistance(distancer.origin);
if (left_up_sqr_d < right_up_sqr_d) {
if (! looked_left && left_up_sqr_d < up_sqr_d)
look_left();
if (! looked_right && right_up_sqr_d < up_sqr_d)
look_right();
} else {
if (! looked_right && right_up_sqr_d < up_sqr_d)
look_right();
if (! looked_left && left_up_sqr_d < up_sqr_d)
look_left();
}
}
return up_sqr_d;
}
} // namespace detail
// Build a balanced AABB Tree over an indexed triangles set, balancing the tree
// on centroids of the triangles.
// Epsilon is applied to the bounding boxes of the AABB Tree to cope with numeric inaccuracies
// during tree traversal.
template<typename VertexType, typename IndexedFaceType>
inline Tree<3, typename VertexType::Scalar> build_aabb_tree_over_indexed_triangle_set(
// Indexed triangle set - 3D vertices.
const std::vector<VertexType> &vertices,
// Indexed triangle set - triangular faces, references to vertices.
const std::vector<IndexedFaceType> &faces,
//FIXME do we want to apply an epsilon?
const typename VertexType::Scalar eps = 0)
{
using TreeType = Tree<3, typename VertexType::Scalar>;
// using CoordType = typename TreeType::CoordType;
using VectorType = typename TreeType::VectorType;
using BoundingBox = typename TreeType::BoundingBox;
struct InputType {
size_t idx() const { return m_idx; }
const BoundingBox& bbox() const { return m_bbox; }
const VectorType& centroid() const { return m_centroid; }
size_t m_idx;
BoundingBox m_bbox;
VectorType m_centroid;
};
std::vector<InputType> input;
input.reserve(faces.size());
const VectorType veps(eps, eps, eps);
for (size_t i = 0; i < faces.size(); ++ i) {
const IndexedFaceType &face = faces[i];
const VertexType &v1 = vertices[face(0)];
const VertexType &v2 = vertices[face(1)];
const VertexType &v3 = vertices[face(2)];
InputType n;
n.m_idx = i;
n.m_centroid = (1./3.) * (v1 + v2 + v3);
n.m_bbox = BoundingBox(v1, v1);
n.m_bbox.extend(v2);
n.m_bbox.extend(v3);
n.m_bbox.min() -= veps;
n.m_bbox.max() += veps;
input.emplace_back(n);
}
TreeType out;
out.build(std::move(input));
return out;
}
// Find a first intersection of a ray with indexed triangle set.
// Intersection test is calculated with the accuracy of VectorType::Scalar
// even if the triangle mesh and the AABB Tree are built with floats.
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
inline bool intersect_ray_first_hit(
// Indexed triangle set - 3D vertices.
const std::vector<VertexType> &vertices,
// Indexed triangle set - triangular faces, references to vertices.
const std::vector<IndexedFaceType> &faces,
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
const TreeType &tree,
// Origin of the ray.
const VectorType &origin,
// Direction of the ray.
const VectorType &dir,
// First intersection of the ray with the indexed triangle set.
igl::Hit &hit)
{
using Scalar = typename VectorType::Scalar;
auto ray_intersector = detail::RayIntersector<VertexType, IndexedFaceType, TreeType, VectorType> {
vertices, faces, tree,
origin, dir, VectorType(dir.cwiseInverse())
};
return ! tree.empty() && detail::intersect_ray_recursive_first_hit(
ray_intersector, size_t(0), std::numeric_limits<Scalar>::infinity(), hit);
}
// Find all intersections of a ray with indexed triangle set.
// Intersection test is calculated with the accuracy of VectorType::Scalar
// even if the triangle mesh and the AABB Tree are built with floats.
// The output hits are sorted by the ray parameter.
// If the ray intersects a shared edge of two triangles, hits for both triangles are returned.
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
inline bool intersect_ray_all_hits(
// Indexed triangle set - 3D vertices.
const std::vector<VertexType> &vertices,
// Indexed triangle set - triangular faces, references to vertices.
const std::vector<IndexedFaceType> &faces,
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
const TreeType &tree,
// Origin of the ray.
const VectorType &origin,
// Direction of the ray.
const VectorType &dir,
// All intersections of the ray with the indexed triangle set, sorted by parameter t.
std::vector<igl::Hit> &hits)
{
auto ray_intersector = detail::RayIntersectorHits<VertexType, IndexedFaceType, TreeType, VectorType> {
vertices, faces, tree,
origin, dir, VectorType(dir.cwiseInverse())
};
if (! tree.empty()) {
ray_intersector.hits.reserve(8);
detail::intersect_ray_recursive_all_hits(ray_intersector, 0);
std::swap(hits, ray_intersector.hits);
std::sort(hits.begin(), hits.end(), [](const auto &l, const auto &r) { return l.t < r.t; });
}
return ! hits.empty();
}
// Finding a closest triangle, its closest point and squared distance to the closest point
// on a 3D indexed triangle set using a pre-built AABBTreeIndirect::Tree.
// Closest point to triangle test will be performed with the accuracy of VectorType::Scalar
// even if the triangle mesh and the AABB Tree are built with floats.
// Returns squared distance to the closest point or -1 if the input is empty.
template<typename VertexType, typename IndexedFaceType, typename TreeType, typename VectorType>
inline typename VectorType::Scalar squared_distance_to_indexed_triangle_set(
// Indexed triangle set - 3D vertices.
const std::vector<VertexType> &vertices,
// Indexed triangle set - triangular faces, references to vertices.
const std::vector<IndexedFaceType> &faces,
// AABBTreeIndirect::Tree over vertices & faces, bounding boxes built with the accuracy of vertices.
const TreeType &tree,
// Point to which the closest point on the indexed triangle set is searched for.
const VectorType &point,
// Index of the closest triangle in faces.
size_t &hit_idx_out,
// Position of the closest point on the indexed triangle set.
Eigen::PlainObjectBase<VectorType> &hit_point_out)
{
using Scalar = typename VectorType::Scalar;
auto distancer = detail::IndexedTriangleSetDistancer<VertexType, IndexedFaceType, TreeType, VectorType>
{ vertices, faces, tree, point };
return tree.empty() ? Scalar(-1) :
detail::squared_distance_to_indexed_triangle_set_recursive(distancer, size_t(0), Scalar(0), std::numeric_limits<Scalar>::infinity(), hit_idx_out, hit_point_out);
}
} // namespace AABBTreeIndirect
} // namespace Slic3r
#endif /* slic3r_AABBTreeIndirect_hpp_ */
|