GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_bvh_shape.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 115 173 66.5%
Functions: 115 1090 10.6%
Branches: 42 104 40.4%
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_MESH_SHAPE_H
39 #define COAL_TRAVERSAL_NODE_MESH_SHAPE_H
40
41 /// @cond INTERNAL
42
43 #include "coal/collision_data.h"
44 #include "coal/shape/geometric_shapes.h"
45 #include "coal/narrowphase/narrowphase.h"
46 #include "coal/shape/geometric_shapes_utility.h"
47 #include "coal/internal/traversal_node_base.h"
48 #include "coal/internal/traversal.h"
49 #include "coal/BVH/BVH_model.h"
50 #include "coal/internal/shape_shape_func.h"
51
52 namespace coal {
53
54 /// @addtogroup Traversal_For_Collision
55 /// @{
56
57 /// @brief Traversal node for collision between BVH and shape
58 template <typename BV, typename S>
59 class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase {
60 public:
61 252 BVHShapeCollisionTraversalNode(const CollisionRequest& request)
62
1/2
✓ Branch 2 taken 126 times.
✗ Branch 3 not taken.
 252 : CollisionTraversalNodeBase(request) {
63 252 model1 = NULL;
64 252 model2 = NULL;
65
66 252 num_bv_tests = 0;
67 252 num_leaf_tests = 0;
68 252 query_time_seconds = 0.0;
69 252 }
70
71 /// @brief Whether the BV node in the first BVH tree is leaf
72 7688 bool isFirstNodeLeaf(unsigned int b) const {
73 7688 return model1->getBV(b).isLeaf();
74 }
75
76 /// @brief Obtain the left child of BV node in the first BVH
77 3840 int getFirstLeftChild(unsigned int b) const {
78 3840 return model1->getBV(b).leftChild();
79 }
80
81 /// @brief Obtain the right child of BV node in the first BVH
82 3840 int getFirstRightChild(unsigned int b) const {
83 3840 return model1->getBV(b).rightChild();
84 }
85
86 const BVHModel<BV>* model1;
87 const S* model2;
88 BV model2_bv;
89
90 mutable int num_bv_tests;
91 mutable int num_leaf_tests;
92 mutable Scalar query_time_seconds;
93 };
94
95 /// @brief Traversal node for collision between mesh and shape
96 template <typename BV, typename S,
97 int _Options = RelativeTransformationIsIdentity>
98 class MeshShapeCollisionTraversalNode
99 : public BVHShapeCollisionTraversalNode<BV, S> {
100 public:
101 enum {
102 Options = _Options,
103 RTIsIdentity = _Options & RelativeTransformationIsIdentity
104 };
105
106 252 MeshShapeCollisionTraversalNode(const CollisionRequest& request)
107 252 : BVHShapeCollisionTraversalNode<BV, S>(request) {
108 252 vertices = NULL;
109 252 tri_indices = NULL;
110
111 252 nsolver = NULL;
112 252 }
113
114 /// test between BV b1 and shape
115 /// @param b1 BV to test,
116 /// @retval sqrDistLowerBound square of a lower bound of the minimal
117 /// distance between bounding volumes.
118 /// @brief BV culling test in one BVTT node
119 6870 bool BVDisjoints(unsigned int b1, unsigned int /*b2*/,
120 Scalar& sqrDistLowerBound) const {
121
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3435 times.
 6870 if (this->enable_statistics) this->num_bv_tests++;
122 bool disjoint;
123 if (RTIsIdentity)
124 ✗ disjoint = !this->model1->getBV(b1).bv.overlap(
125 ✗ this->model2_bv, this->request, sqrDistLowerBound);
126 else
127 6870 disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(),
128 6870 this->model1->getBV(b1).bv, this->model2_bv,
129 this->request, sqrDistLowerBound);
130
2/2
✓ Branch 0 taken 1515 times.
✓ Branch 1 taken 1920 times.
 6870 if (disjoint)
131 3030 internal::updateDistanceLowerBoundFromBV(this->request, *this->result,
132 sqrDistLowerBound);
133
3/4
✓ Branch 0 taken 1515 times.
✓ Branch 1 taken 1920 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1515 times.
 6870 assert(!disjoint || sqrDistLowerBound > 0);
134 6870 return disjoint;
135 }
136
137 /// @brief Intersection testing between leaves (one triangle and one shape)
138 818 void leafCollides(unsigned int b1, unsigned int /*b2*/,
139 Scalar& sqrDistLowerBound) const {
140
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 409 times.
 818 if (this->enable_statistics) this->num_leaf_tests++;
141 818 const BVNode<BV>& node = this->model1->getBV(b1);
142
143 818 int primitive_id = node.primitiveId();
144
145 818 const Triangle32& tri_id = this->tri_indices[primitive_id];
146
1/2
✓ Branch 2 taken 409 times.
✗ Branch 3 not taken.
 818 const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
147 818 this->vertices[tri_id[2]]);
148
149 // When reaching this point, `this->solver` has already been set up
150 // by the CollisionRequest `this->request`.
151 // The only thing we need to (and can) pass to `ShapeShapeDistance` is
152 // whether or not penetration information is should be computed in case of
153 // collision.
154 818 const bool compute_penetration =
155
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 818 this->request.enable_contact || (this->request.security_margin < 0);
156
3/6
✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 409 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 409 times.
✗ Branch 8 not taken.
 818 Vec3s c1, c2, normal;
157 Scalar distance;
158
159 if (RTIsIdentity) {
160 ✗ static const Transform3s Id;
161 ✗ distance = internal::ShapeShapeDistance<TriangleP, S>(
162 ✗ &tri, Id, this->model2, this->tf2, this->nsolver, compute_penetration,
163 c1, c2, normal);
164 } else {
165 1636 distance = internal::ShapeShapeDistance<TriangleP, S>(
166
1/2
✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
 818 &tri, this->tf1, this->model2, this->tf2, this->nsolver,
167 compute_penetration, c1, c2, normal);
168 }
169 818 const Scalar distToCollision = distance - this->request.security_margin;
170
171
1/2
✓ Branch 1 taken 409 times.
✗ Branch 2 not taken.
 818 internal::updateDistanceLowerBoundFromLeaf(this->request, *(this->result),
172 distToCollision, c1, c2, normal);
173
174
2/2
✓ Branch 0 taken 46 times.
✓ Branch 1 taken 363 times.
 818 if (distToCollision <= this->request.collision_distance_threshold) {
175 92 sqrDistLowerBound = 0;
176
1/2
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
 92 if (this->result->numContacts() < this->request.num_max_contacts) {
177
2/4
✓ Branch 1 taken 46 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46 times.
✗ Branch 5 not taken.
 92 this->result->addContact(Contact(this->model1, this->model2,
178 primitive_id, Contact::NONE, c1, c2,
179 normal, distance));
180
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 46 times.
 92 assert(this->result->isCollision());
181 }
182 } else {
183 726 sqrDistLowerBound = distToCollision * distToCollision;
184 }
185
186
3/4
✓ Branch 1 taken 361 times.
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 361 times.
 818 assert(this->result->isCollision() || sqrDistLowerBound > 0);
187 818 } // leafCollides
188
189 Vec3s* vertices;
190 Triangle32* tri_indices;
191
192 const GJKSolver* nsolver;
193 };
194
195 /// @}
196
197 /// @addtogroup Traversal_For_Distance
198 /// @{
199
200 /// @brief Traversal node for distance computation between BVH and shape
201 template <typename BV, typename S>
202 class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase {
203 public:
204
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 204 BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase() {
205 204 model1 = NULL;
206 204 model2 = NULL;
207
208 204 num_bv_tests = 0;
209 204 num_leaf_tests = 0;
210 204 query_time_seconds = 0.0;
211 204 }
212
213 /// @brief Whether the BV node in the first BVH tree is leaf
214 13654 bool isFirstNodeLeaf(unsigned int b) const {
215 13654 return model1->getBV(b).isLeaf();
216 }
217
218 /// @brief Obtain the left child of BV node in the first BVH
219 10796 int getFirstLeftChild(unsigned int b) const {
220 10796 return model1->getBV(b).leftChild();
221 }
222
223 /// @brief Obtain the right child of BV node in the first BVH
224 10796 int getFirstRightChild(unsigned int b) const {
225 10796 return model1->getBV(b).rightChild();
226 }
227
228 /// @brief BV culling test in one BVTT node
229 ✗ Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
230 ✗ return model1->getBV(b1).bv.distance(model2_bv);
231 }
232
233 const BVHModel<BV>* model1;
234 const S* model2;
235 BV model2_bv;
236
237 mutable int num_bv_tests;
238 mutable int num_leaf_tests;
239 mutable Scalar query_time_seconds;
240 };
241
242 /// @brief Traversal node for distance computation between shape and BVH
243 template <typename S, typename BV>
244 class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase {
245 public:
246 ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase() {
247 model1 = NULL;
248 model2 = NULL;
249
250 num_bv_tests = 0;
251 num_leaf_tests = 0;
252 query_time_seconds = 0.0;
253 }
254
255 /// @brief Whether the BV node in the second BVH tree is leaf
256 bool isSecondNodeLeaf(unsigned int b) const {
257 return model2->getBV(b).isLeaf();
258 }
259
260 /// @brief Obtain the left child of BV node in the second BVH
261 int getSecondLeftChild(unsigned int b) const {
262 return model2->getBV(b).leftChild();
263 }
264
265 /// @brief Obtain the right child of BV node in the second BVH
266 int getSecondRightChild(unsigned int b) const {
267 return model2->getBV(b).rightChild();
268 }
269
270 /// @brief BV culling test in one BVTT node
271 Scalar BVDistanceLowerBound(unsigned int /*b1*/, unsigned int b2) const {
272 return model1_bv.distance(model2->getBV(b2).bv);
273 }
274
275 const S* model1;
276 const BVHModel<BV>* model2;
277 BV model1_bv;
278
279 mutable int num_bv_tests;
280 mutable int num_leaf_tests;
281 mutable Scalar query_time_seconds;
282 };
283
284 /// @brief Traversal node for distance between mesh and shape
285 template <typename BV, typename S>
286 class MeshShapeDistanceTraversalNode
287 : public BVHShapeDistanceTraversalNode<BV, S> {
288 public:
289 204 MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>() {
290 204 vertices = NULL;
291 204 tri_indices = NULL;
292
293 204 rel_err = 0;
294 204 abs_err = 0;
295
296 204 nsolver = NULL;
297 204 }
298
299 /// @brief Distance testing between leaves (one triangle and one shape)
300 ✗ void leafComputeDistance(unsigned int b1, unsigned int /*b2*/) const {
301 ✗ if (this->enable_statistics) this->num_leaf_tests++;
302
303 ✗ const BVNode<BV>& node = this->model1->getBV(b1);
304
305 ✗ int primitive_id = node.primitiveId();
306
307 ✗ const Triangle32& tri_id = tri_indices[primitive_id];
308 ✗ const TriangleP tri(this->vertices[tri_id[0]], this->vertices[tri_id[1]],
309 ✗ this->vertices[tri_id[2]]);
310
311 ✗ Vec3s p1, p2, normal;
312 ✗ const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>(
313 ✗ &tri, this->tf1, this->model2, this->tf2, this->nsolver,
314 ✗ this->request.enable_signed_distance, p1, p2, normal);
315
316 ✗ this->result->update(distance, this->model1, this->model2, primitive_id,
317 DistanceResult::NONE, p1, p2, normal);
318 ✗ }
319
320 /// @brief Whether the traversal process can stop early
321 21592 bool canStop(Scalar c) const {
322
2/2
✓ Branch 0 taken 4071 times.
✓ Branch 1 taken 6725 times.
 21592 if ((c >= this->result->min_distance - abs_err) &&
323
1/2
✓ Branch 0 taken 4071 times.
✗ Branch 1 not taken.
 8142 (c * (1 + rel_err) >= this->result->min_distance))
324 8142 return true;
325 13450 return false;
326 }
327
328 Vec3s* vertices;
329 Triangle32* tri_indices;
330
331 Scalar rel_err;
332 Scalar abs_err;
333
334 const GJKSolver* nsolver;
335 };
336
337 /// @cond IGNORE
338 namespace details {
339
340 template <typename BV, typename S>
341 2858 void meshShapeDistanceOrientedNodeleafComputeDistance(
342 unsigned int b1, unsigned int /* b2 */, const BVHModel<BV>* model1,
343 const S& model2, Vec3s* vertices, Triangle32* tri_indices,
344 const Transform3s& tf1, const Transform3s& tf2, const GJKSolver* nsolver,
345 bool enable_statistics, int& num_leaf_tests, const DistanceRequest& request,
346 DistanceResult& result) {
347
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1429 times.
 2858 if (enable_statistics) num_leaf_tests++;
348
349 2858 const BVNode<BV>& node = model1->getBV(b1);
350 2858 int primitive_id = node.primitiveId();
351
352 2858 const Triangle32& tri_id = tri_indices[primitive_id];
353
1/2
✓ Branch 2 taken 1429 times.
✗ Branch 3 not taken.
 2858 const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
354 2858 vertices[tri_id[2]]);
355
356
3/6
✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1429 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1429 times.
✗ Branch 8 not taken.
 2858 Vec3s p1, p2, normal;
357 5716 const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>(
358
1/2
✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
 2858 &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
359 normal);
360
361
1/2
✓ Branch 1 taken 1429 times.
✗ Branch 2 not taken.
 2858 result.update(distance, model1, &model2, primitive_id, DistanceResult::NONE,
362 p1, p2, normal);
363 2858 }
364
365 template <typename BV, typename S>
366 204 static inline void distancePreprocessOrientedNode(
367 const BVHModel<BV>* model1, Vec3s* vertices, Triangle32* tri_indices,
368 int init_tri_id, const S& model2, const Transform3s& tf1,
369 const Transform3s& tf2, const GJKSolver* nsolver,
370 const DistanceRequest& request, DistanceResult& result) {
371 204 const Triangle32& tri_id = tri_indices[init_tri_id];
372
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 204 const TriangleP tri(vertices[tri_id[0]], vertices[tri_id[1]],
373 204 vertices[tri_id[2]]);
374
375
3/6
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 102 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 102 times.
✗ Branch 8 not taken.
 204 Vec3s p1, p2, normal;
376 408 const Scalar distance = internal::ShapeShapeDistance<TriangleP, S>(
377
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
 204 &tri, tf1, &model2, tf2, nsolver, request.enable_signed_distance, p1, p2,
378 normal);
379
1/2
✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
 204 result.update(distance, model1, &model2, init_tri_id, DistanceResult::NONE,
380 p1, p2, normal);
381 204 }
382
383 } // namespace details
384
385 /// @endcond
386
387 /// @brief Traversal node for distance between mesh and shape, when mesh BVH is
388 /// one of the oriented node (RSS, kIOS, OBBRSS)
389 template <typename S>
390 class MeshShapeDistanceTraversalNodeRSS
391 : public MeshShapeDistanceTraversalNode<RSS, S> {
392 public:
393 ✗ MeshShapeDistanceTraversalNodeRSS()
394 ✗ : MeshShapeDistanceTraversalNode<RSS, S>() {}
395
396 ✗ void preprocess() {
397 ✗ details::distancePreprocessOrientedNode(
398 ✗ this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
399 ✗ this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
400 ✗ }
401
402 ✗ void postprocess() {}
403
404 ✗ Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
405 ✗ if (this->enable_statistics) this->num_bv_tests++;
406 ✗ return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
407 ✗ this->model2_bv, this->model1->getBV(b1).bv);
408 }
409
410 ✗ void leafComputeDistance(unsigned int b1, unsigned int b2) const {
411 ✗ details::meshShapeDistanceOrientedNodeleafComputeDistance(
412 ✗ b1, b2, this->model1, *(this->model2), this->vertices,
413 ✗ this->tri_indices, this->tf1, this->tf2, this->nsolver,
414 ✗ this->enable_statistics, this->num_leaf_tests, this->request,
415 ✗ *(this->result));
416 ✗ }
417 };
418
419 template <typename S>
420 class MeshShapeDistanceTraversalNodekIOS
421 : public MeshShapeDistanceTraversalNode<kIOS, S> {
422 public:
423 ✗ MeshShapeDistanceTraversalNodekIOS()
424 ✗ : MeshShapeDistanceTraversalNode<kIOS, S>() {}
425
426 ✗ void preprocess() {
427 ✗ details::distancePreprocessOrientedNode(
428 ✗ this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
429 ✗ this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
430 ✗ }
431
432 ✗ void postprocess() {}
433
434 ✗ Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
435 ✗ if (this->enable_statistics) this->num_bv_tests++;
436 ✗ return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
437 ✗ this->model2_bv, this->model1->getBV(b1).bv);
438 }
439
440 ✗ void leafComputeDistance(unsigned int b1, unsigned int b2) const {
441 ✗ details::meshShapeDistanceOrientedNodeleafComputeDistance(
442 ✗ b1, b2, this->model1, *(this->model2), this->vertices,
443 ✗ this->tri_indices, this->tf1, this->tf2, this->nsolver,
444 ✗ this->enable_statistics, this->num_leaf_tests, this->request,
445 ✗ *(this->result));
446 ✗ }
447 };
448
449 template <typename S>
450 class MeshShapeDistanceTraversalNodeOBBRSS
451 : public MeshShapeDistanceTraversalNode<OBBRSS, S> {
452 public:
453 204 MeshShapeDistanceTraversalNodeOBBRSS()
454 204 : MeshShapeDistanceTraversalNode<OBBRSS, S>() {}
455
456 204 void preprocess() {
457 204 details::distancePreprocessOrientedNode(
458 204 this->model1, this->vertices, this->tri_indices, 0, *(this->model2),
459 204 this->tf1, this->tf2, this->nsolver, this->request, *(this->result));
460 204 }
461
462 204 void postprocess() {}
463
464 21592 Scalar BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const {
465
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10796 times.
 21592 if (this->enable_statistics) this->num_bv_tests++;
466 21592 return distance(this->tf1.getRotation(), this->tf1.getTranslation(),
467 43184 this->model2_bv, this->model1->getBV(b1).bv);
468 }
469
470 2858 void leafComputeDistance(unsigned int b1, unsigned int b2) const {
471 2858 details::meshShapeDistanceOrientedNodeleafComputeDistance(
472 2858 b1, b2, this->model1, *(this->model2), this->vertices,
473 2858 this->tri_indices, this->tf1, this->tf2, this->nsolver,
474 2858 this->enable_statistics, this->num_leaf_tests, this->request,
475 2858 *(this->result));
476 2858 }
477 };
478
479 /// @}
480
481 } // namespace coal
482
483 /// @endcond
484
485 #endif
486