GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_bvhs.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 179 184 97.3%
Branches: 76 124 61.3%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38 #ifndef COAL_TRAVERSAL_NODE_MESHES_H
39 #define COAL_TRAVERSAL_NODE_MESHES_H
40
41 /// @cond INTERNAL
42
43 #include "coal/collision_data.h"
44 #include "coal/internal/traversal_node_base.h"
45 #include "coal/BV/BV_node.h"
46 #include "coal/BV/BV.h"
47 #include "coal/BVH/BVH_model.h"
48 #include "coal/internal/intersect.h"
49 #include "coal/shape/geometric_shapes.h"
50 #include "coal/narrowphase/narrowphase.h"
51 #include "coal/internal/traversal.h"
52 #include "coal/internal/shape_shape_func.h"
53
54 #include <cassert>
55
56 namespace coal {
57
58 /// @addtogroup Traversal_For_Collision
59 /// @{
60
61 /// @brief Traversal node for collision between BVH models
62 template <typename BV>
63 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase {
64 public:
65 21595 BVHCollisionTraversalNode(const CollisionRequest& request)
66 21595 : CollisionTraversalNodeBase(request) {
67 21595 model1 = NULL;
68 21595 model2 = NULL;
69
70 21595 num_bv_tests = 0;
71 21595 num_leaf_tests = 0;
72 21595 query_time_seconds = 0.0;
73 21595 }
74
75 /// @brief Whether the BV node in the first BVH tree is leaf
76 116998931 bool isFirstNodeLeaf(unsigned int b) const {
77
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 58499498 times.
 116998931 assert(model1 != NULL && "model1 is NULL");
78 116998931 return model1->getBV(b).isLeaf();
79 }
80
81 /// @brief Whether the BV node in the second BVH tree is leaf
82 116998931 bool isSecondNodeLeaf(unsigned int b) const {
83
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 58499498 times.
 116998931 assert(model2 != NULL && "model2 is NULL");
84 116998931 return model2->getBV(b).isLeaf();
85 }
86
87 /// @brief Determine the traversal order, is the first BVTT subtree better
88 41583777 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
89 41583777 Scalar sz1 = model1->getBV(b1).bv.size();
90 41583777 Scalar sz2 = model2->getBV(b2).bv.size();
91
92 41583777 bool l1 = model1->getBV(b1).isLeaf();
93 41583777 bool l2 = model2->getBV(b2).isLeaf();
94
95
6/6
✓ Branch 0 taken 9579863 times.
✓ Branch 1 taken 11212045 times.
✓ Branch 2 taken 3851809 times.
✓ Branch 3 taken 5728054 times.
✓ Branch 4 taken 1543658 times.
✓ Branch 5 taken 2308151 times.
 41583777 if (l2 || (!l1 && (sz1 > sz2))) return true;
96 16072382 return false;
97 }
98
99 /// @brief Obtain the left child of BV node in the first BVH
100 25511395 int getFirstLeftChild(unsigned int b) const {
101 25511395 return model1->getBV(b).leftChild();
102 }
103
104 /// @brief Obtain the right child of BV node in the first BVH
105 25511395 int getFirstRightChild(unsigned int b) const {
106 25511395 return model1->getBV(b).rightChild();
107 }
108
109 /// @brief Obtain the left child of BV node in the second BVH
110 16072382 int getSecondLeftChild(unsigned int b) const {
111 16072382 return model2->getBV(b).leftChild();
112 }
113
114 /// @brief Obtain the right child of BV node in the second BVH
115 16072382 int getSecondRightChild(unsigned int b) const {
116 16072382 return model2->getBV(b).rightChild();
117 }
118
119 /// @brief The first BVH model
120 const BVHModel<BV>* model1;
121 /// @brief The second BVH model
122 const BVHModel<BV>* model2;
123
124 /// @brief statistical information
125 mutable int num_bv_tests;
126 mutable int num_leaf_tests;
127 mutable Scalar query_time_seconds;
128 };
129
130 /// @brief Traversal node for collision between two meshes
131 template <typename BV, int _Options = RelativeTransformationIsIdentity>
132 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV> {
133 public:
134 enum {
135 Options = _Options,
136 RTIsIdentity = _Options & RelativeTransformationIsIdentity
137 };
138
139 21595 MeshCollisionTraversalNode(const CollisionRequest& request)
140
1/2
✓ Branch 2 taken 10697 times.
✗ Branch 3 not taken.
 21595 : BVHCollisionTraversalNode<BV>(request) {
141 21595 vertices1 = NULL;
142 21595 vertices2 = NULL;
143 21595 tri_indices1 = NULL;
144 21595 tri_indices2 = NULL;
145 21595 }
146
147 /// BV test between b1 and b2
148 /// @param b1, b2 Bounding volumes to test,
149 /// @retval sqrDistLowerBound square of a lower bound of the minimal
150 /// distance between bounding volumes.
151 41745725 bool BVDisjoints(unsigned int b1, unsigned int b2,
152 Scalar& sqrDistLowerBound) const {
153
2/2
✓ Branch 0 taken 3869604 times.
✓ Branch 1 taken 17003287 times.
 41745725 if (this->enable_statistics) this->num_bv_tests++;
154 bool disjoint;
155 if (RTIsIdentity)
156 83175980 disjoint = !this->model1->getBV(b1).overlap(
157 41587990 this->model2->getBV(b2), this->request, sqrDistLowerBound);
158 else {
159 157735 disjoint = !overlap(RT._R(), RT._T(), this->model2->getBV(b2).bv,
160 157735 this->model1->getBV(b1).bv, this->request,
161 sqrDistLowerBound);
162 }
163
2/2
✓ Branch 0 taken 80983 times.
✓ Branch 1 taken 20791908 times.
 41745725 if (disjoint)
164 161948 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
165 sqrDistLowerBound);
166 41745725 return disjoint;
167 }
168
169 /// Intersection testing between leaves (two triangles)
170 ///
171 /// @param b1, b2 id of primitive in bounding volume hierarchy
172 /// @retval sqrDistLowerBound squared lower bound of distance between
173 /// primitives if they are not in collision.
174 ///
175 /// This method supports a security margin. If the distance between
176 /// the primitives is less than the security margin, the objects are
177 /// considered as in collision. in this case a contact point is
178 /// returned in the CollisionResult.
179 ///
180 /// @note If the distance between objects is less than the security margin,
181 /// and the object are not colliding, the penetration depth is
182 /// negative.
183 58359872 void leafCollides(unsigned int b1, unsigned int b2,
184 Scalar& sqrDistLowerBound) const {
185
2/2
✓ Branch 0 taken 3846921 times.
✓ Branch 1 taken 25333019 times.
 58359872 if (this->enable_statistics) this->num_leaf_tests++;
186
187 58359872 const BVNode<BV>& node1 = this->model1->getBV(b1);
188 58359872 const BVNode<BV>& node2 = this->model2->getBV(b2);
189
190 58359872 int primitive_id1 = node1.primitiveId();
191 58359872 int primitive_id2 = node2.primitiveId();
192
193 58359872 const Triangle& tri_id1 = tri_indices1[primitive_id1];
194 58359872 const Triangle& tri_id2 = tri_indices2[primitive_id2];
195
196 58359872 const Vec3s& P1 = vertices1[tri_id1[0]];
197 58359872 const Vec3s& P2 = vertices1[tri_id1[1]];
198 58359872 const Vec3s& P3 = vertices1[tri_id1[2]];
199 58359872 const Vec3s& Q1 = vertices2[tri_id2[0]];
200 58359872 const Vec3s& Q2 = vertices2[tri_id2[1]];
201 58359872 const Vec3s& Q3 = vertices2[tri_id2[2]];
202
203
1/2
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
 58359872 TriangleP tri1(P1, P2, P3);
204
1/2
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
 58359872 TriangleP tri2(Q1, Q2, Q3);
205
206 // TODO(louis): MeshCollisionTraversalNode should have its own GJKSolver.
207
1/2
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
 58359872 GJKSolver solver(this->request);
208
209 58359872 const bool compute_penetration =
210
3/4
✓ Branch 0 taken 25329507 times.
✓ Branch 1 taken 3850433 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25329507 times.
 58359872 this->request.enable_contact || (this->request.security_margin < 0);
211
3/6
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29179940 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 29179940 times.
✗ Branch 8 not taken.
 58359872 Vec3s p1, p2, normal;
212
1/2
✓ Branch 2 taken 29179940 times.
✗ Branch 3 not taken.
 58359872 DistanceResult distanceResult;
213 116719744 Scalar distance = internal::ShapeShapeDistance<TriangleP, TriangleP>(
214
1/2
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
 58359872 &tri1, this->tf1, &tri2, this->tf2, &solver, compute_penetration, p1,
215 p2, normal);
216
217 58359872 const Scalar distToCollision = distance - this->request.security_margin;
218
219
1/2
✓ Branch 1 taken 29179940 times.
✗ Branch 2 not taken.
 58359872 internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
220 distToCollision, p1, p2, normal);
221
222 58359872 if (distToCollision <=
223
2/2
✓ Branch 0 taken 15561 times.
✓ Branch 1 taken 29164379 times.
 58359872 this->request.collision_distance_threshold) { // collision
224 31121 sqrDistLowerBound = 0;
225
1/2
✓ Branch 1 taken 15561 times.
✗ Branch 2 not taken.
 31121 if (this->result->numContacts() < this->request.num_max_contacts) {
226
2/4
✓ Branch 1 taken 15561 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15561 times.
✗ Branch 5 not taken.
 31121 this->result->addContact(Contact(this->model1, this->model2,
227 primitive_id1, primitive_id2, p1, p2,
228 normal, distance));
229 }
230 } else
231 58328751 sqrDistLowerBound = distToCollision * distToCollision;
232 58359872 }
233
234 Vec3s* vertices1;
235 Vec3s* vertices2;
236
237 Triangle* tri_indices1;
238 Triangle* tri_indices2;
239
240 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
241 };
242
243 /// @brief Traversal node for collision between two meshes if their underlying
244 /// BVH node is oriented node (OBB, RSS, OBBRSS, kIOS)
245 typedef MeshCollisionTraversalNode<OBB, 0> MeshCollisionTraversalNodeOBB;
246 typedef MeshCollisionTraversalNode<RSS, 0> MeshCollisionTraversalNodeRSS;
247 typedef MeshCollisionTraversalNode<kIOS, 0> MeshCollisionTraversalNodekIOS;
248 typedef MeshCollisionTraversalNode<OBBRSS, 0> MeshCollisionTraversalNodeOBBRSS;
249
250 /// @}
251
252 namespace details {
253 template <typename BV>
254 struct DistanceTraversalBVDistanceLowerBound_impl {
255 5556 static Scalar run(const BVNode<BV>& b1, const BVNode<BV>& b2) {
256 5556 return b1.distance(b2);
257 }
258 3175686 static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<BV>& b1,
259 const BVNode<BV>& b2) {
260 3178280 return distance(R, T, b1.bv, b2.bv);
261 }
262 };
263
264 template <>
265 struct DistanceTraversalBVDistanceLowerBound_impl<OBB> {
266 15564 static Scalar run(const BVNode<OBB>& b1, const BVNode<OBB>& b2) {
267 Scalar sqrDistLowerBound;
268
1/2
✓ Branch 1 taken 15564 times.
✗ Branch 2 not taken.
 15564 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
269 // request.break_distance = ?
270
3/4
✓ Branch 1 taken 15564 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10494 times.
✓ Branch 4 taken 5070 times.
 15564 if (b1.overlap(b2, request, sqrDistLowerBound)) {
271 // TODO A penetration upper bound should be computed.
272 10494 return -1;
273 }
274 5070 return std::sqrt(sqrDistLowerBound);
275 }
276 static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<OBB>& b1,
277 const BVNode<OBB>& b2) {
278 Scalar sqrDistLowerBound;
279 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
280 // request.break_distance = ?
281 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
282 // TODO A penetration upper bound should be computed.
283 return -1;
284 }
285 return std::sqrt(sqrDistLowerBound);
286 }
287 };
288
289 template <>
290 struct DistanceTraversalBVDistanceLowerBound_impl<AABB> {
291 ✗ static Scalar run(const BVNode<AABB>& b1, const BVNode<AABB>& b2) {
292 Scalar sqrDistLowerBound;
293 ✗ CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
294 // request.break_distance = ?
295 ✗ if (b1.overlap(b2, request, sqrDistLowerBound)) {
296 // TODO A penetration upper bound should be computed.
297 ✗ return -1;
298 }
299 ✗ return std::sqrt(sqrDistLowerBound);
300 }
301 static Scalar run(const Matrix3s& R, const Vec3s& T, const BVNode<AABB>& b1,
302 const BVNode<AABB>& b2) {
303 Scalar sqrDistLowerBound;
304 CollisionRequest request(DISTANCE_LOWER_BOUND, 0);
305 // request.break_distance = ?
306 if (overlap(R, T, b1.bv, b2.bv, request, sqrDistLowerBound)) {
307 // TODO A penetration upper bound should be computed.
308 return -1;
309 }
310 return std::sqrt(sqrDistLowerBound);
311 }
312 };
313 } // namespace details
314
315 /// @addtogroup Traversal_For_Distance
316 /// @{
317
318 /// @brief Traversal node for distance computation between BVH models
319 template <typename BV>
320 class BVHDistanceTraversalNode : public DistanceTraversalNodeBase {
321 public:
322 14374 BVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
323 14374 model1 = NULL;
324 14374 model2 = NULL;
325
326 14374 num_bv_tests = 0;
327 14374 num_leaf_tests = 0;
328 14374 query_time_seconds = 0.0;
329 14374 }
330
331 /// @brief Whether the BV node in the first BVH tree is leaf
332 2274508 bool isFirstNodeLeaf(unsigned int b) const {
333 2274508 return model1->getBV(b).isLeaf();
334 }
335
336 /// @brief Whether the BV node in the second BVH tree is leaf
337 2274508 bool isSecondNodeLeaf(unsigned int b) const {
338 2274508 return model2->getBV(b).isLeaf();
339 }
340
341 /// @brief Determine the traversal order, is the first BVTT subtree better
342 1607482 bool firstOverSecond(unsigned int b1, unsigned int b2) const {
343 1607482 Scalar sz1 = model1->getBV(b1).bv.size();
344 1607482 Scalar sz2 = model2->getBV(b2).bv.size();
345
346 1607482 bool l1 = model1->getBV(b1).isLeaf();
347 1607482 bool l2 = model2->getBV(b2).isLeaf();
348
349
6/6
✓ Branch 0 taken 352537 times.
✓ Branch 1 taken 451204 times.
✓ Branch 2 taken 279090 times.
✓ Branch 3 taken 73447 times.
✓ Branch 4 taken 112308 times.
✓ Branch 5 taken 166782 times.
 1607482 if (l2 || (!l1 && (sz1 > sz2))) return true;
350 480458 return false;
351 }
352
353 /// @brief Obtain the left child of BV node in the first BVH
354 1127024 int getFirstLeftChild(unsigned int b) const {
355 1127024 return model1->getBV(b).leftChild();
356 }
357
358 /// @brief Obtain the right child of BV node in the first BVH
359 1127024 int getFirstRightChild(unsigned int b) const {
360 1127024 return model1->getBV(b).rightChild();
361 }
362
363 /// @brief Obtain the left child of BV node in the second BVH
364 480458 int getSecondLeftChild(unsigned int b) const {
365 480458 return model2->getBV(b).leftChild();
366 }
367
368 /// @brief Obtain the right child of BV node in the second BVH
369 480458 int getSecondRightChild(unsigned int b) const {
370 480458 return model2->getBV(b).rightChild();
371 }
372
373 /// @brief The first BVH model
374 const BVHModel<BV>* model1;
375 /// @brief The second BVH model
376 const BVHModel<BV>* model2;
377
378 /// @brief statistical information
379 mutable int num_bv_tests;
380 mutable int num_leaf_tests;
381 mutable Scalar query_time_seconds;
382 };
383
384 /// @brief Traversal node for distance computation between two meshes
385 template <typename BV, int _Options = RelativeTransformationIsIdentity>
386 class MeshDistanceTraversalNode : public BVHDistanceTraversalNode<BV> {
387 public:
388 enum {
389 Options = _Options,
390 RTIsIdentity = _Options & RelativeTransformationIsIdentity
391 };
392
393 using BVHDistanceTraversalNode<BV>::enable_statistics;
394 using BVHDistanceTraversalNode<BV>::request;
395 using BVHDistanceTraversalNode<BV>::result;
396 using BVHDistanceTraversalNode<BV>::tf1;
397 using BVHDistanceTraversalNode<BV>::model1;
398 using BVHDistanceTraversalNode<BV>::model2;
399 using BVHDistanceTraversalNode<BV>::num_bv_tests;
400 using BVHDistanceTraversalNode<BV>::num_leaf_tests;
401
402
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14374 MeshDistanceTraversalNode() : BVHDistanceTraversalNode<BV>() {
403 14374 vertices1 = NULL;
404 14374 vertices2 = NULL;
405 14374 tri_indices1 = NULL;
406 14374 tri_indices2 = NULL;
407
408 14374 rel_err = this->request.rel_err;
409 14374 abs_err = this->request.abs_err;
410 14374 }
411
412 14374 void preprocess() {
413 14326 if (!RTIsIdentity) preprocessOrientedNode();
414 14374 }
415
416 14374 void postprocess() {
417 14326 if (!RTIsIdentity) postprocessOrientedNode();
418 14374 }
419
420 /// @brief BV culling test in one BVTT node
421 3214964 Scalar BVDistanceLowerBound(unsigned int b1, unsigned int b2) const {
422
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1607482 times.
 3214964 if (enable_statistics) num_bv_tests++;
423 if (RTIsIdentity)
424 36684 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
425 73368 model1->getBV(b1), model2->getBV(b2));
426 else
427 3178280 return details::DistanceTraversalBVDistanceLowerBound_impl<BV>::run(
428 6356560 RT._R(), RT._T(), model1->getBV(b1), model2->getBV(b2));
429 }
430
431 /// @brief Distance testing between leaves (two triangles)
432 665850 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
433
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 332925 times.
 665850 if (this->enable_statistics) this->num_leaf_tests++;
434
435 665850 const BVNode<BV>& node1 = this->model1->getBV(b1);
436 665850 const BVNode<BV>& node2 = this->model2->getBV(b2);
437
438 665850 int primitive_id1 = node1.primitiveId();
439 665850 int primitive_id2 = node2.primitiveId();
440
441 665850 const Triangle& tri_id1 = tri_indices1[primitive_id1];
442 665850 const Triangle& tri_id2 = tri_indices2[primitive_id2];
443
444 665850 const Vec3s& t11 = vertices1[tri_id1[0]];
445 665850 const Vec3s& t12 = vertices1[tri_id1[1]];
446 665850 const Vec3s& t13 = vertices1[tri_id1[2]];
447
448 665850 const Vec3s& t21 = vertices2[tri_id2[0]];
449 665850 const Vec3s& t22 = vertices2[tri_id2[1]];
450 665850 const Vec3s& t23 = vertices2[tri_id2[2]];
451
452 // nearest point pair
453
3/6
✓ Branch 1 taken 332925 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 332925 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 332925 times.
✗ Branch 8 not taken.
 665850 Vec3s P1, P2, normal;
454
455 Scalar d2;
456 if (RTIsIdentity)
457
1/2
✓ Branch 1 taken 3601 times.
✗ Branch 2 not taken.
 7202 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23, P1,
458 P2);
459 else
460
1/2
✓ Branch 3 taken 329324 times.
✗ Branch 4 not taken.
 658648 d2 = TriangleDistance::sqrTriDistance(t11, t12, t13, t21, t22, t23,
461 RT._R(), RT._T(), P1, P2);
462 665850 Scalar d = sqrt(d2);
463
464
1/2
✓ Branch 1 taken 332925 times.
✗ Branch 2 not taken.
 665850 this->result->update(d, this->model1, this->model2, primitive_id1,
465 primitive_id2, P1, P2, normal);
466 665850 }
467
468 /// @brief Whether the traversal process can stop early
469 3208866 bool canStop(Scalar c) const {
470
2/2
✓ Branch 0 taken 474954 times.
✓ Branch 1 taken 1129479 times.
 3208866 if ((c >= this->result->min_distance - abs_err) &&
471
1/2
✓ Branch 0 taken 474954 times.
✗ Branch 1 not taken.
 949908 (c * (1 + rel_err) >= this->result->min_distance))
472 949908 return true;
473 2258958 return false;
474 }
475
476 Vec3s* vertices1;
477 Vec3s* vertices2;
478
479 Triangle* tri_indices1;
480 Triangle* tri_indices2;
481
482 /// @brief relative and absolute error, default value is 0.01 for both terms
483 Scalar rel_err;
484 Scalar abs_err;
485
486 details::RelativeTransformation<!bool(RTIsIdentity)> RT;
487
488 private:
489 14326 void preprocessOrientedNode() {
490 14326 const int init_tri_id1 = 0, init_tri_id2 = 0;
491 14326 const Triangle& init_tri1 = tri_indices1[init_tri_id1];
492 14326 const Triangle& init_tri2 = tri_indices2[init_tri_id2];
493
494
3/4
✓ Branch 1 taken 21489 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21489 times.
✓ Branch 4 taken 7163 times.
 57304 Vec3s init_tri1_points[3];
495
3/4
✓ Branch 1 taken 21489 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21489 times.
✓ Branch 4 taken 7163 times.
 57304 Vec3s init_tri2_points[3];
496
497
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri1_points[0] = vertices1[init_tri1[0]];
498
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri1_points[1] = vertices1[init_tri1[1]];
499
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri1_points[2] = vertices1[init_tri1[2]];
500
501
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri2_points[0] = vertices2[init_tri2[0]];
502
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri2_points[1] = vertices2[init_tri2[1]];
503
1/2
✓ Branch 2 taken 7163 times.
✗ Branch 3 not taken.
 14326 init_tri2_points[2] = vertices2[init_tri2[2]];
504
505
3/6
✓ Branch 1 taken 7163 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7163 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7163 times.
✗ Branch 8 not taken.
 14326 Vec3s p1, p2, normal;
506
1/2
✓ Branch 3 taken 7163 times.
✗ Branch 4 not taken.
 14326 Scalar distance = sqrt(TriangleDistance::sqrTriDistance(
507 init_tri1_points[0], init_tri1_points[1], init_tri1_points[2],
508 init_tri2_points[0], init_tri2_points[1], init_tri2_points[2], RT._R(),
509 RT._T(), p1, p2));
510
511
1/2
✓ Branch 1 taken 7163 times.
✗ Branch 2 not taken.
 14326 result->update(distance, model1, model2, init_tri_id1, init_tri_id2, p1, p2,
512 normal);
513 14326 }
514 7163 void postprocessOrientedNode() {
515 /// the points obtained by triDistance are not in world space: both are in
516 /// object1's local coordinate system, so we need to convert them into the
517 /// world space.
518
3/4
✓ Branch 0 taken 7163 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 1901 times.
✓ Branch 3 taken 5262 times.
 14326 if (request.enable_nearest_points && (result->o1 == model1) &&
519
2/2
✓ Branch 0 taken 1673 times.
✓ Branch 1 taken 228 times.
 3802 (result->o2 == model2)) {
520 3346 result->nearest_points[0] = tf1.transform(result->nearest_points[0]);
521 3346 result->nearest_points[1] = tf1.transform(result->nearest_points[1]);
522 }
523 14326 }
524 };
525
526 /// @brief Traversal node for distance computation between two meshes if their
527 /// underlying BVH node is oriented node (RSS, OBBRSS, kIOS)
528 typedef MeshDistanceTraversalNode<RSS, 0> MeshDistanceTraversalNodeRSS;
529 typedef MeshDistanceTraversalNode<kIOS, 0> MeshDistanceTraversalNodekIOS;
530 typedef MeshDistanceTraversalNode<OBBRSS, 0> MeshDistanceTraversalNodeOBBRSS;
531
532 /// @}
533
534 /// @brief for OBB and RSS, there is local coordinate of BV, so normal need to
535 /// be transformed
536 namespace details {
537
538 template <typename BV>
539 inline const Matrix3s& getBVAxes(const BV& bv) {
540 return bv.axes;
541 }
542
543 template <>
544 inline const Matrix3s& getBVAxes<OBBRSS>(const OBBRSS& bv) {
545 return bv.obb.axes;
546 }
547
548 } // namespace details
549
550 } // namespace coal
551
552 /// @endcond
553
554 #endif
555