GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_octree.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 265 597 44.4%
Functions: 17 313 5.4%
Branches: 253 910 27.8%
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 * Copyright (c) 2022-2023, INRIA
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Open Source Robotics Foundation nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 */
36
37 /** \author Jia Pan */
38
39 #ifndef COAL_TRAVERSAL_NODE_OCTREE_H
40 #define COAL_TRAVERSAL_NODE_OCTREE_H
41
42 /// @cond INTERNAL
43
44 #include "coal/collision_data.h"
45 #include "coal/internal/traversal_node_base.h"
46 #include "coal/internal/traversal_node_hfield_shape.h"
47 #include "coal/narrowphase/narrowphase.h"
48 #include "coal/hfield.h"
49 #include "coal/octree.h"
50 #include "coal/BVH/BVH_model.h"
51 #include "coal/shape/geometric_shapes_utility.h"
52 #include "coal/internal/shape_shape_func.h"
53
54 namespace coal {
55
56 /// @brief Algorithms for collision related with octree
57 class COAL_DLLAPI OcTreeSolver {
58 private:
59 const GJKSolver* solver;
60
61 mutable const CollisionRequest* crequest;
62 mutable const DistanceRequest* drequest;
63
64 mutable CollisionResult* cresult;
65 mutable DistanceResult* dresult;
66
67 public:
68 3100 OcTreeSolver(const GJKSolver* solver_)
69 3100 : solver(solver_),
70 3100 crequest(NULL),
71 3100 drequest(NULL),
72 3100 cresult(NULL),
73 3100 dresult(NULL) {}
74
75 /// @brief collision between two octrees
76 ✗ void OcTreeIntersect(const OcTree* tree1, const OcTree* tree2,
77 const Transform3s& tf1, const Transform3s& tf2,
78 const CollisionRequest& request_,
79 CollisionResult& result_) const {
80 ✗ crequest = &request_;
81 ✗ cresult = &result_;
82
83 ✗ OcTreeIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
84 ✗ tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
85 ✗ }
86
87 /// @brief distance between two octrees
88 ✗ void OcTreeDistance(const OcTree* tree1, const OcTree* tree2,
89 const Transform3s& tf1, const Transform3s& tf2,
90 const DistanceRequest& request_,
91 DistanceResult& result_) const {
92 ✗ drequest = &request_;
93 ✗ dresult = &result_;
94
95 ✗ OcTreeDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(), tree2,
96 ✗ tree2->getRoot(), tree2->getRootBV(), tf1, tf2);
97 ✗ }
98
99 /// @brief collision between octree and mesh
100 template <typename BV>
101 200 void OcTreeMeshIntersect(const OcTree* tree1, const BVHModel<BV>* tree2,
102 const Transform3s& tf1, const Transform3s& tf2,
103 const CollisionRequest& request_,
104 CollisionResult& result_) const {
105 200 crequest = &request_;
106 200 cresult = &result_;
107
108
3/6
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
 200 OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
109 tree2, 0, tf1, tf2);
110 200 }
111
112 /// @brief distance between octree and mesh
113 template <typename BV>
114 ✗ void OcTreeMeshDistance(const OcTree* tree1, const BVHModel<BV>* tree2,
115 const Transform3s& tf1, const Transform3s& tf2,
116 const DistanceRequest& request_,
117 DistanceResult& result_) const {
118 ✗ drequest = &request_;
119 ✗ dresult = &result_;
120
121 ✗ OcTreeMeshDistanceRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
122 tree2, 0, tf1, tf2);
123 ✗ }
124
125 /// @brief collision between mesh and octree
126 template <typename BV>
127 void MeshOcTreeIntersect(const BVHModel<BV>* tree1, const OcTree* tree2,
128 const Transform3s& tf1, const Transform3s& tf2,
129 const CollisionRequest& request_,
130 CollisionResult& result_) const
131
132 {
133 crequest = &request_;
134 cresult = &result_;
135
136 OcTreeMeshIntersectRecurse(tree2, tree2->getRoot(), tree2->getRootBV(),
137 tree1, 0, tf2, tf1);
138 }
139
140 /// @brief distance between mesh and octree
141 template <typename BV>
142 void MeshOcTreeDistance(const BVHModel<BV>* tree1, const OcTree* tree2,
143 const Transform3s& tf1, const Transform3s& tf2,
144 const DistanceRequest& request_,
145 DistanceResult& result_) const {
146 drequest = &request_;
147 dresult = &result_;
148
149 OcTreeMeshDistanceRecurse(tree1, 0, tree2, tree2->getRoot(),
150 tree2->getRootBV(), tf1, tf2);
151 }
152
153 template <typename BV>
154 2000 void OcTreeHeightFieldIntersect(
155 const OcTree* tree1, const HeightField<BV>* tree2, const Transform3s& tf1,
156 const Transform3s& tf2, const CollisionRequest& request_,
157 CollisionResult& result_, Scalar& sqrDistLowerBound) const {
158 2000 crequest = &request_;
159 2000 cresult = &result_;
160
161
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 2000 OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
162 tree1->getRootBV(), tree2, 0, tf1, tf2,
163 sqrDistLowerBound);
164 2000 }
165
166 template <typename BV>
167 2000 void HeightFieldOcTreeIntersect(const HeightField<BV>* tree1,
168 const OcTree* tree2, const Transform3s& tf1,
169 const Transform3s& tf2,
170 const CollisionRequest& request_,
171 CollisionResult& result_,
172 Scalar& sqrDistLowerBound) const {
173 2000 crequest = &request_;
174 2000 cresult = &result_;
175
176
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 2000 HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
177 tree2->getRootBV(), tf1, tf2,
178 sqrDistLowerBound);
179 2000 }
180
181 /// @brief collision between octree and shape
182 template <typename S>
183 2000 void OcTreeShapeIntersect(const OcTree* tree, const S& s,
184 const Transform3s& tf1, const Transform3s& tf2,
185 const CollisionRequest& request_,
186 CollisionResult& result_) const {
187 2000 crequest = &request_;
188 2000 cresult = &result_;
189
190
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 2000 AABB bv2;
191
2/4
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
 2000 computeBV<AABB>(s, Transform3s(), bv2);
192
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 2000 OBB obb2;
193
1/2
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
 2000 convertBV(bv2, tf2, obb2);
194
3/6
✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
 2000 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
195 obb2, tf1, tf2);
196 2000 }
197
198 /// @brief collision between shape and octree
199 template <typename S>
200 void ShapeOcTreeIntersect(const S& s, const OcTree* tree,
201 const Transform3s& tf1, const Transform3s& tf2,
202 const CollisionRequest& request_,
203 CollisionResult& result_) const {
204 crequest = &request_;
205 cresult = &result_;
206
207 AABB bv1;
208 computeBV<AABB>(s, Transform3s(), bv1);
209 OBB obb1;
210 convertBV(bv1, tf1, obb1);
211 OcTreeShapeIntersectRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
212 obb1, tf2, tf1);
213 }
214
215 /// @brief distance between octree and shape
216 template <typename S>
217 ✗ void OcTreeShapeDistance(const OcTree* tree, const S& s,
218 const Transform3s& tf1, const Transform3s& tf2,
219 const DistanceRequest& request_,
220 DistanceResult& result_) const {
221 ✗ drequest = &request_;
222 ✗ dresult = &result_;
223
224 ✗ AABB aabb2;
225 ✗ computeBV<AABB>(s, tf2, aabb2);
226 ✗ OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
227 aabb2, tf1, tf2);
228 ✗ }
229
230 /// @brief distance between shape and octree
231 template <typename S>
232 void ShapeOcTreeDistance(const S& s, const OcTree* tree,
233 const Transform3s& tf1, const Transform3s& tf2,
234 const DistanceRequest& request_,
235 DistanceResult& result_) const {
236 drequest = &request_;
237 dresult = &result_;
238
239 AABB aabb1;
240 computeBV<AABB>(s, tf1, aabb1);
241 OcTreeShapeDistanceRecurse(tree, tree->getRoot(), tree->getRootBV(), s,
242 aabb1, tf2, tf1);
243 }
244
245 private:
246 template <typename S>
247 ✗ bool OcTreeShapeDistanceRecurse(const OcTree* tree1,
248 const OcTree::OcTreeNode* root1,
249 const AABB& bv1, const S& s,
250 const AABB& aabb2, const Transform3s& tf1,
251 const Transform3s& tf2) const {
252 ✗ if (!tree1->nodeHasChildren(root1)) {
253 ✗ if (tree1->isNodeOccupied(root1)) {
254 ✗ Box box;
255 ✗ Transform3s box_tf;
256 ✗ constructBox(bv1, tf1, box, box_tf);
257
258 ✗ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
259 ✗ box.computeLocalAABB();
260 }
261
262 ✗ Vec3s p1, p2, normal;
263 ✗ const Scalar distance = internal::ShapeShapeDistance<Box, S>(
264 ✗ &box, box_tf, &s, tf2, this->solver,
265 ✗ this->drequest->enable_signed_distance, p1, p2, normal);
266
267 ✗ this->dresult->update(distance, tree1, &s,
268 ✗ (int)(root1 - tree1->getRoot()),
269 DistanceResult::NONE, p1, p2, normal);
270
271 ✗ return drequest->isSatisfied(*dresult);
272 ✗ } else
273 ✗ return false;
274 }
275
276 ✗ if (!tree1->isNodeOccupied(root1)) return false;
277
278 ✗ for (unsigned int i = 0; i < 8; ++i) {
279 ✗ if (tree1->nodeChildExists(root1, i)) {
280 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
281 ✗ AABB child_bv;
282 ✗ computeChildBV(bv1, i, child_bv);
283
284 ✗ AABB aabb1;
285 ✗ convertBV(child_bv, tf1, aabb1);
286 ✗ Scalar d = aabb1.distance(aabb2);
287 ✗ if (d < dresult->min_distance) {
288 ✗ if (OcTreeShapeDistanceRecurse(tree1, child, child_bv, s, aabb2, tf1,
289 tf2))
290 ✗ return true;
291 }
292 }
293 }
294
295 ✗ return false;
296 }
297
298 template <typename S>
299 56052 bool OcTreeShapeIntersectRecurse(const OcTree* tree1,
300 const OcTree::OcTreeNode* root1,
301 const AABB& bv1, const S& s, const OBB& obb2,
302 const Transform3s& tf1,
303 const Transform3s& tf2) const {
304 // Empty OcTree is considered free.
305
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28026 times.
 56052 if (!root1) return false;
306
307 /// stop when 1) bounding boxes of two objects not overlap; OR
308 /// 2) at least of one the nodes is free; OR
309 /// 2) (two uncertain nodes or one node occupied and one node
310 /// uncertain) AND cost not required
311
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28026 times.
 56052 if (tree1->isNodeFree(root1))
312 ✗ return false;
313
3/6
✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28026 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28026 times.
 56052 else if ((tree1->isNodeUncertain(root1) || s.isUncertain()))
314 ✗ return false;
315 else {
316
1/2
✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
 56052 OBB obb1;
317
1/2
✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
 56052 convertBV(bv1, tf1, obb1);
318 Scalar sqrDistLowerBound;
319
3/4
✓ Branch 1 taken 28026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17739 times.
✓ Branch 4 taken 10287 times.
 56052 if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
320 35478 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
321 sqrDistLowerBound);
322 35478 return false;
323 }
324 }
325
326
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 10271 times.
 20574 if (!tree1->nodeHasChildren(root1)) {
327
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
328
329
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Box box;
330
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Transform3s box_tf;
331
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 constructBox(bv1, tf1, box, box_tf);
332
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
333 ✗ box.computeLocalAABB();
334 }
335
336 32 bool contactNotAdded =
337 32 (cresult->numContacts() >= crequest->num_max_contacts);
338 64 std::size_t ncontact = ShapeShapeCollide<Box, S>(
339
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 &box, box_tf, &s, tf2, solver, *crequest, *cresult);
340
2/4
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 32 assert(ncontact == 0 || ncontact == 1);
341
2/4
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 if (!contactNotAdded && ncontact == 1) {
342 // Update contact information.
343
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 32 const Contact& c = cresult->getContact(cresult->numContacts() - 1);
344
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 cresult->setContact(
345 32 cresult->numContacts() - 1,
346
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 64 Contact(tree1, c.o2, static_cast<int>(root1 - tree1->getRoot()),
347
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 c.b2, c.pos, c.normal, c.penetration_depth));
348 }
349
350 // no need to call `internal::updateDistanceLowerBoundFromLeaf` here
351 // as it is already done internally in `ShapeShapeCollide` above.
352
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 return crequest->isSatisfied(*cresult);
353 32 }
354
355
2/2
✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
 182828 for (unsigned int i = 0; i < 8; ++i) {
356
2/2
✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
 162778 if (tree1->nodeChildExists(root1, i)) {
357
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
358
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 AABB child_bv;
359
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 computeChildBV(bv1, i, child_bv);
360
361
3/4
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
 54052 if (OcTreeShapeIntersectRecurse(tree1, child, child_bv, s, obb2, tf1,
362 tf2))
363 492 return true;
364 }
365 }
366
367 20050 return false;
368 }
369
370 template <typename BV>
371 ✗ bool OcTreeMeshDistanceRecurse(const OcTree* tree1,
372 const OcTree::OcTreeNode* root1,
373 const AABB& bv1, const BVHModel<BV>* tree2,
374 unsigned int root2, const Transform3s& tf1,
375 const Transform3s& tf2) const {
376 ✗ if (!tree1->nodeHasChildren(root1) && tree2->getBV(root2).isLeaf()) {
377 ✗ if (tree1->isNodeOccupied(root1)) {
378 ✗ Box box;
379 ✗ Transform3s box_tf;
380 ✗ constructBox(bv1, tf1, box, box_tf);
381
382 ✗ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
383 ✗ box.computeLocalAABB();
384 }
385
386 ✗ size_t primitive_id =
387 ✗ static_cast<size_t>(tree2->getBV(root2).primitiveId());
388 ✗ const Triangle32& tri_id = (*(tree2->tri_indices))[primitive_id];
389 ✗ const TriangleP tri((*(tree2->vertices))[tri_id[0]],
390 ✗ (*(tree2->vertices))[tri_id[1]],
391 ✗ (*(tree2->vertices))[tri_id[2]]);
392
393 ✗ Vec3s p1, p2, normal;
394 ✗ const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
395 ✗ &box, box_tf, &tri, tf2, this->solver,
396 ✗ this->drequest->enable_signed_distance, p1, p2, normal);
397
398 ✗ this->dresult->update(distance, tree1, tree2,
399 ✗ (int)(root1 - tree1->getRoot()),
400 static_cast<int>(primitive_id), p1, p2, normal);
401
402 ✗ return this->drequest->isSatisfied(*dresult);
403 ✗ } else
404 ✗ return false;
405 }
406
407 ✗ if (!tree1->isNodeOccupied(root1)) return false;
408
409 ✗ if (tree2->getBV(root2).isLeaf() ||
410 ✗ (tree1->nodeHasChildren(root1) &&
411 ✗ (bv1.size() > tree2->getBV(root2).bv.size()))) {
412 ✗ for (unsigned int i = 0; i < 8; ++i) {
413 ✗ if (tree1->nodeChildExists(root1, i)) {
414 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
415 ✗ AABB child_bv;
416 ✗ computeChildBV(bv1, i, child_bv);
417
418 Scalar d;
419 ✗ AABB aabb1, aabb2;
420 ✗ convertBV(child_bv, tf1, aabb1);
421 ✗ convertBV(tree2->getBV(root2).bv, tf2, aabb2);
422 ✗ d = aabb1.distance(aabb2);
423
424 ✗ if (d < dresult->min_distance) {
425 ✗ if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
426 tf1, tf2))
427 ✗ return true;
428 }
429 }
430 }
431 } else {
432 Scalar d;
433 ✗ AABB aabb1, aabb2;
434 ✗ convertBV(bv1, tf1, aabb1);
435 ✗ unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
436 ✗ convertBV(tree2->getBV(child).bv, tf2, aabb2);
437 ✗ d = aabb1.distance(aabb2);
438
439 ✗ if (d < dresult->min_distance) {
440 ✗ if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
441 tf2))
442 ✗ return true;
443 }
444
445 ✗ child = (unsigned int)tree2->getBV(root2).rightChild();
446 ✗ convertBV(tree2->getBV(child).bv, tf2, aabb2);
447 ✗ d = aabb1.distance(aabb2);
448
449 ✗ if (d < dresult->min_distance) {
450 ✗ if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
451 tf2))
452 ✗ return true;
453 }
454 }
455
456 ✗ return false;
457 }
458
459 /// \return True if the request is satisfied.
460 template <typename BV>
461 57964 bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
462 const OcTree::OcTreeNode* root1,
463 const AABB& bv1, const BVHModel<BV>* tree2,
464 unsigned int root2, const Transform3s& tf1,
465 const Transform3s& tf2) const {
466 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
467 // code in this if(!root1) did not output anything so the empty OcTree is
468 // considered free. Should an empty OcTree be considered free ?
469
470 // Empty OcTree is considered free.
471
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28982 times.
 57964 if (!root1) return false;
472 57964 BVNode<BV> const& bvn2 = tree2->getBV(root2);
473
474 /// stop when 1) bounding boxes of two objects not overlap; OR
475 /// 2) at least one of the nodes is free; OR
476 /// 2) (two uncertain nodes OR one node occupied and one node
477 /// uncertain) AND cost not required
478
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28982 times.
 57964 if (tree1->isNodeFree(root1))
479 ✗ return false;
480
3/6
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28982 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28982 times.
 57964 else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
481 ✗ return false;
482 else {
483
2/4
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28982 times.
✗ Branch 5 not taken.
 57964 OBB obb1, obb2;
484
1/2
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
 57964 convertBV(bv1, tf1, obb1);
485
1/2
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
 57964 convertBV(bvn2.bv, tf2, obb2);
486 Scalar sqrDistLowerBound;
487
3/4
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17430 times.
✓ Branch 4 taken 11552 times.
 57964 if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
488 34860 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
489 sqrDistLowerBound);
490 34860 return false;
491 }
492 }
493
494 // Check if leaf collides.
495
5/6
✓ Branch 1 taken 539 times.
✓ Branch 2 taken 11013 times.
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 539 times.
✓ Branch 7 taken 11013 times.
 23104 if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
496
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 539 times.
 1078 assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
497
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 Box box;
498
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 Transform3s box_tf;
499
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 constructBox(bv1, tf1, box, box_tf);
500
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
 1078 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
501 ✗ box.computeLocalAABB();
502 }
503
504 1078 size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
505 1078 const Triangle32& tri_id = (*(tree2->tri_indices))[primitive_id];
506
1/2
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
 1078 const TriangleP tri((*(tree2->vertices))[tri_id[0]],
507 1078 (*(tree2->vertices))[tri_id[1]],
508 1078 (*(tree2->vertices))[tri_id[2]]);
509
510 // When reaching this point, `this->solver` has already been set up
511 // by the CollisionRequest `this->crequest`.
512 // The only thing we need to (and can) pass to `ShapeShapeDistance` is
513 // whether or not penetration information is should be computed in case of
514 // collision.
515
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
 1078 const bool compute_penetration = this->crequest->enable_contact ||
516 ✗ (this->crequest->security_margin < 0);
517
3/6
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 539 times.
✗ Branch 8 not taken.
 1078 Vec3s c1, c2, normal;
518 2156 const Scalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
519
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
520 normal);
521 1078 const Scalar distToCollision = distance - this->crequest->security_margin;
522
523 1078 internal::updateDistanceLowerBoundFromLeaf(
524
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
525
526
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 if (cresult->numContacts() < crequest->num_max_contacts) {
527
2/2
✓ Branch 0 taken 53 times.
✓ Branch 1 taken 486 times.
 1078 if (distToCollision <= crequest->collision_distance_threshold) {
528
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 106 cresult->addContact(Contact(
529
2/4
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53 times.
✗ Branch 5 not taken.
 106 tree1, tree2, (int)(root1 - tree1->getRoot()),
530 static_cast<int>(primitive_id), c1, c2, normal, distance));
531 }
532 }
533
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 return crequest->isSatisfied(*cresult);
534 1078 }
535
536 // Determine which tree to traverse first.
537
4/4
✓ Branch 1 taken 8970 times.
✓ Branch 2 taken 2043 times.
✓ Branch 3 taken 5600 times.
✓ Branch 4 taken 5413 times.
 39966 if (bvn2.isLeaf() ||
538
3/4
✓ Branch 1 taken 8970 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3557 times.
✓ Branch 6 taken 5413 times.
 17940 (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
539
2/2
✓ Branch 0 taken 42996 times.
✓ Branch 1 taken 4790 times.
 95572 for (unsigned int i = 0; i < 8; ++i) {
540
2/2
✓ Branch 1 taken 18305 times.
✓ Branch 2 taken 24691 times.
 85992 if (tree1->nodeChildExists(root1, i)) {
541
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
542
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 AABB child_bv;
543
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 computeChildBV(bv1, i, child_bv);
544
545
3/4
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 810 times.
✓ Branch 4 taken 17495 times.
 36610 if (OcTreeMeshIntersectRecurse(tree1, child, child_bv, tree2, root2,
546 tf1, tf2))
547 1620 return true;
548 }
549 }
550 } else {
551
2/2
✓ Branch 1 taken 249 times.
✓ Branch 2 taken 5164 times.
 10826 if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
552 10826 (unsigned int)bvn2.leftChild(), tf1, tf2))
553 498 return true;
554
555
2/2
✓ Branch 1 taken 147 times.
✓ Branch 2 taken 5017 times.
 10328 if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
556 10328 (unsigned int)bvn2.rightChild(), tf1, tf2))
557 294 return true;
558 }
559
560 19614 return false;
561 }
562
563 /// \return True if the request is satisfied.
564 template <typename BV>
565 56584 bool OcTreeHeightFieldIntersectRecurse(
566 const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
567 const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
568 const Transform3s& tf2, Scalar& sqrDistLowerBound) const {
569 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
570 // code in this if(!root1) did not output anything so the empty OcTree is
571 // considered free. Should an empty OcTree be considered free ?
572
573 // Empty OcTree is considered free.
574
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
 56584 if (!root1) return false;
575 56584 HFNode<BV> const& bvn2 = tree2->getBV(root2);
576
577 /// stop when 1) bounding boxes of two objects not overlap; OR
578 /// 2) at least one of the nodes is free; OR
579 /// 2) (two uncertain nodes OR one node occupied and one node
580 /// uncertain) AND cost not required
581
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
 56584 if (tree1->isNodeFree(root1))
582 ✗ return false;
583
3/6
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28292 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28292 times.
 56584 else if ((tree1->isNodeUncertain(root1) || tree2->isUncertain()))
584 ✗ return false;
585 else {
586
2/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
 56584 OBB obb1, obb2;
587
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bv1, tf1, obb1);
588
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bvn2.bv, tf2, obb2);
589 Scalar sqrDistLowerBound_;
590
3/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17800 times.
✓ Branch 4 taken 10492 times.
 56584 if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound_)) {
591
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
 35600 if (sqrDistLowerBound_ < sqrDistLowerBound)
592 ✗ sqrDistLowerBound = sqrDistLowerBound_;
593 35600 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
594 sqrDistLowerBound);
595 35600 return false;
596 }
597 }
598
599 // Check if leaf collides.
600
6/6
✓ Branch 1 taken 221 times.
✓ Branch 2 taken 10271 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 205 times.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 10476 times.
 20984 if (!tree1->nodeHasChildren(root1) && bvn2.isLeaf()) {
601
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
602
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Box box;
603
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Transform3s box_tf;
604
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 constructBox(bv1, tf1, box, box_tf);
605
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
606 ✗ box.computeLocalAABB();
607 }
608
609 typedef ConvexTpl<Triangle32> ConvexTriangle;
610
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ConvexTriangle convex1, convex2;
611 int convex1_active_faces, convex2_active_faces;
612
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
613 convex2, convex2_active_faces);
614
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
615 ✗ convex1.computeLocalAABB();
616 ✗ convex2.computeLocalAABB();
617 }
618
619
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 32 Vec3s c1, c2, normal, normal_top;
620 Scalar distance;
621 bool hfield_witness_is_on_bin_side;
622
623 64 bool collision = details::shapeDistance<Triangle32, Box, 0>(
624
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 solver, *crequest, convex1, convex1_active_faces, convex2,
625 convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
626 normal_top, hfield_witness_is_on_bin_side);
627
628 32 Scalar distToCollision =
629
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 distance - crequest->security_margin * (normal_top.dot(normal));
630
631
1/2
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
 32 if (distToCollision <= crequest->collision_distance_threshold) {
632 32 sqrDistLowerBound = 0;
633
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 if (crequest->num_max_contacts > cresult->numContacts()) {
634
4/8
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
 32 if (normal_top.isApprox(normal) &&
635 ✗ (collision || !hfield_witness_is_on_bin_side)) {
636
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 cresult->addContact(
637
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 64 Contact(tree1, tree2, (int)(root1 - tree1->getRoot()),
638 (int)Contact::NONE, c1, c2, -normal, distance));
639 }
640 }
641 } else
642 ✗ sqrDistLowerBound = distToCollision * distToCollision;
643
644 // const Vec3s c1 = contact_point - distance * 0.5 * normal;
645 // const Vec3s c2 = contact_point + distance * 0.5 * normal;
646
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 internal::updateDistanceLowerBoundFromLeaf(
647
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 *crequest, *cresult, distToCollision, c1, c2, -normal);
648
649
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 32 assert(cresult->isCollision() || sqrDistLowerBound > 0);
650
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 return crequest->isSatisfied(*cresult);
651 32 }
652
653 // Determine which tree to traverse first.
654
3/4
✓ Branch 1 taken 10476 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10271 times.
✓ Branch 4 taken 205 times.
 41904 if (bvn2.isLeaf() ||
655
3/4
✓ Branch 1 taken 10271 times.
✓ Branch 2 taken 205 times.
✓ Branch 5 taken 10271 times.
✗ Branch 6 not taken.
 20952 (tree1->nodeHasChildren(root1) && (bv1.size() > bvn2.bv.size()))) {
656
2/2
✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
 182828 for (unsigned int i = 0; i < 8; ++i) {
657
2/2
✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
 162778 if (tree1->nodeChildExists(root1, i)) {
658
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
659
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 AABB child_bv;
660
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 computeChildBV(bv1, i, child_bv);
661
662
3/4
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
 54052 if (OcTreeHeightFieldIntersectRecurse(tree1, child, child_bv, tree2,
663 root2, tf1, tf2,
664 sqrDistLowerBound))
665 492 return true;
666 }
667 }
668 } else {
669
2/2
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
 410 if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
670 410 (unsigned int)bvn2.leftChild(), tf1,
671 tf2, sqrDistLowerBound))
672 288 return true;
673
674
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 122 if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
675 122 (unsigned int)bvn2.rightChild(),
676 tf1, tf2, sqrDistLowerBound))
677 122 return true;
678 }
679
680 20050 return false;
681 }
682
683 /// \return True if the request is satisfied.
684 template <typename BV>
685 56584 bool HeightFieldOcTreeIntersectRecurse(
686 const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
687 const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
688 const Transform3s& tf2, Scalar& sqrDistLowerBound) const {
689 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
690 // code in this if(!root1) did not output anything so the empty OcTree is
691 // considered free. Should an empty OcTree be considered free ?
692
693 // Empty OcTree is considered free.
694
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
 56584 if (!root2) return false;
695 56584 HFNode<BV> const& bvn1 = tree1->getBV(root1);
696
697 /// stop when 1) bounding boxes of two objects not overlap; OR
698 /// 2) at least one of the nodes is free; OR
699 /// 2) (two uncertain nodes OR one node occupied and one node
700 /// uncertain) AND cost not required
701
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
 56584 if (tree2->isNodeFree(root2))
702 ✗ return false;
703
3/6
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28292 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 28292 times.
 56584 else if ((tree2->isNodeUncertain(root2) || tree1->isUncertain()))
704 ✗ return false;
705 else {
706
2/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
 56584 OBB obb1, obb2;
707
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bvn1.bv, tf1, obb1);
708
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bv2, tf2, obb2);
709 Scalar sqrDistLowerBound_;
710
3/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17800 times.
✓ Branch 4 taken 10492 times.
 56584 if (!obb2.overlap(obb1, *crequest, sqrDistLowerBound_)) {
711
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
 35600 if (sqrDistLowerBound_ < sqrDistLowerBound)
712 ✗ sqrDistLowerBound = sqrDistLowerBound_;
713 35600 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
714 sqrDistLowerBound);
715 35600 return false;
716 }
717 }
718
719 // Check if leaf collides.
720
6/6
✓ Branch 1 taken 221 times.
✓ Branch 2 taken 10271 times.
✓ Branch 4 taken 16 times.
✓ Branch 5 taken 205 times.
✓ Branch 6 taken 16 times.
✓ Branch 7 taken 10476 times.
 20984 if (!tree2->nodeHasChildren(root2) && bvn1.isLeaf()) {
721
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain.
722
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Box box;
723
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Transform3s box_tf;
724
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 constructBox(bv2, tf2, box, box_tf);
725
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
726 ✗ box.computeLocalAABB();
727 }
728
729 typedef ConvexTpl<Triangle32> ConvexTriangle;
730
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ConvexTriangle convex1, convex2;
731 int convex1_active_faces, convex2_active_faces;
732
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
733 convex2, convex2_active_faces);
734
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
735 ✗ convex1.computeLocalAABB();
736 ✗ convex2.computeLocalAABB();
737 }
738
739
4/8
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 16 times.
✗ Branch 11 not taken.
 32 Vec3s c1, c2, normal, normal_top;
740 Scalar distance;
741 bool hfield_witness_is_on_bin_side;
742
743 64 bool collision = details::shapeDistance<Triangle32, Box, 0>(
744
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 solver, *crequest, convex1, convex1_active_faces, convex2,
745 convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
746 normal_top, hfield_witness_is_on_bin_side);
747
748 32 Scalar distToCollision =
749
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 distance - crequest->security_margin * (normal_top.dot(normal));
750
751
1/2
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
 32 if (distToCollision <= crequest->collision_distance_threshold) {
752 32 sqrDistLowerBound = 0;
753
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 if (crequest->num_max_contacts > cresult->numContacts()) {
754
4/8
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 16 times.
✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
 32 if (normal_top.isApprox(normal) &&
755 ✗ (collision || !hfield_witness_is_on_bin_side)) {
756
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
757
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 (int)(root2 - tree2->getRoot()), c1, c2,
758 normal, distance));
759 }
760 }
761 } else
762 ✗ sqrDistLowerBound = distToCollision * distToCollision;
763
764 // const Vec3s c1 = contact_point - distance * 0.5 * normal;
765 // const Vec3s c2 = contact_point + distance * 0.5 * normal;
766 32 internal::updateDistanceLowerBoundFromLeaf(
767
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 *crequest, *cresult, distToCollision, c1, c2, normal);
768
769
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 32 assert(cresult->isCollision() || sqrDistLowerBound > 0);
770
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 return crequest->isSatisfied(*cresult);
771 32 }
772
773 // Determine which tree to traverse first.
774
3/4
✓ Branch 1 taken 10476 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10271 times.
✓ Branch 4 taken 205 times.
 41904 if (bvn1.isLeaf() ||
775
3/4
✓ Branch 1 taken 10271 times.
✓ Branch 2 taken 205 times.
✓ Branch 5 taken 10271 times.
✗ Branch 6 not taken.
 20952 (tree2->nodeHasChildren(root2) && (bv2.size() > bvn1.bv.size()))) {
776
2/2
✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
 182828 for (unsigned int i = 0; i < 8; ++i) {
777
2/2
✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
 162778 if (tree2->nodeChildExists(root2, i)) {
778
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
779
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 AABB child_bv;
780
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 computeChildBV(bv2, i, child_bv);
781
782
3/4
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 246 times.
✓ Branch 4 taken 26780 times.
 54052 if (HeightFieldOcTreeIntersectRecurse(tree1, root1, tree2, child,
783 child_bv, tf1, tf2,
784 sqrDistLowerBound))
785 492 return true;
786 }
787 }
788 } else {
789
2/2
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
 410 if (HeightFieldOcTreeIntersectRecurse(
790 410 tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
791 tf2, sqrDistLowerBound))
792 288 return true;
793
794
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 122 if (HeightFieldOcTreeIntersectRecurse(
795 122 tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
796 tf2, sqrDistLowerBound))
797 122 return true;
798 }
799
800 20050 return false;
801 }
802
803 ✗ bool OcTreeDistanceRecurse(const OcTree* tree1,
804 const OcTree::OcTreeNode* root1, const AABB& bv1,
805 const OcTree* tree2,
806 const OcTree::OcTreeNode* root2, const AABB& bv2,
807 const Transform3s& tf1,
808 const Transform3s& tf2) const {
809 ✗ if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
810 ✗ if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
811 ✗ Box box1, box2;
812 ✗ Transform3s box1_tf, box2_tf;
813 ✗ constructBox(bv1, tf1, box1, box1_tf);
814 ✗ constructBox(bv2, tf2, box2, box2_tf);
815
816 ✗ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
817 ✗ box1.computeLocalAABB();
818 ✗ box2.computeLocalAABB();
819 }
820
821 ✗ Vec3s p1, p2, normal;
822 ✗ const Scalar distance = internal::ShapeShapeDistance<Box, Box>(
823 ✗ &box1, box1_tf, &box2, box2_tf, this->solver,
824 ✗ this->drequest->enable_signed_distance, p1, p2, normal);
825
826 ✗ this->dresult->update(distance, tree1, tree2,
827 ✗ (int)(root1 - tree1->getRoot()),
828 ✗ (int)(root2 - tree2->getRoot()), p1, p2, normal);
829
830 ✗ return drequest->isSatisfied(*dresult);
831 ✗ } else
832 ✗ return false;
833 }
834
835 ✗ if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
836 ✗ return false;
837
838 ✗ if (!tree2->nodeHasChildren(root2) ||
839 ✗ (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
840 ✗ for (unsigned int i = 0; i < 8; ++i) {
841 ✗ if (tree1->nodeChildExists(root1, i)) {
842 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
843 ✗ AABB child_bv;
844 ✗ computeChildBV(bv1, i, child_bv);
845
846 Scalar d;
847 ✗ AABB aabb1, aabb2;
848 ✗ convertBV(bv1, tf1, aabb1);
849 ✗ convertBV(bv2, tf2, aabb2);
850 ✗ d = aabb1.distance(aabb2);
851
852 ✗ if (d < dresult->min_distance) {
853 ✗ if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
854 tf1, tf2))
855 ✗ return true;
856 }
857 }
858 }
859 } else {
860 ✗ for (unsigned int i = 0; i < 8; ++i) {
861 ✗ if (tree2->nodeChildExists(root2, i)) {
862 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
863 ✗ AABB child_bv;
864 ✗ computeChildBV(bv2, i, child_bv);
865
866 Scalar d;
867 ✗ AABB aabb1, aabb2;
868 ✗ convertBV(bv1, tf1, aabb1);
869 ✗ convertBV(bv2, tf2, aabb2);
870 ✗ d = aabb1.distance(aabb2);
871
872 ✗ if (d < dresult->min_distance) {
873 ✗ if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
874 tf1, tf2))
875 ✗ return true;
876 }
877 }
878 }
879 }
880
881 ✗ return false;
882 }
883
884 ✗ bool OcTreeIntersectRecurse(const OcTree* tree1,
885 const OcTree::OcTreeNode* root1, const AABB& bv1,
886 const OcTree* tree2,
887 const OcTree::OcTreeNode* root2, const AABB& bv2,
888 const Transform3s& tf1,
889 const Transform3s& tf2) const {
890 // Empty OcTree is considered free.
891 ✗ if (!root1) return false;
892 ✗ if (!root2) return false;
893
894 /// stop when 1) bounding boxes of two objects not overlap; OR
895 /// 2) at least one of the nodes is free; OR
896 /// 2) (two uncertain nodes OR one node occupied and one node
897 /// uncertain) AND cost not required
898 ✗ if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
899 ✗ return false;
900 ✗ else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
901 ✗ return false;
902
903 bool bothAreLeaves =
904 ✗ (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
905 ✗ if (!bothAreLeaves || !crequest->enable_contact) {
906 ✗ OBB obb1, obb2;
907 ✗ convertBV(bv1, tf1, obb1);
908 ✗ convertBV(bv2, tf2, obb2);
909 Scalar sqrDistLowerBound;
910 ✗ if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
911 ✗ if (cresult->distance_lower_bound > 0 &&
912 ✗ sqrDistLowerBound <
913 ✗ cresult->distance_lower_bound * cresult->distance_lower_bound)
914 ✗ cresult->distance_lower_bound =
915 ✗ sqrt(sqrDistLowerBound) - crequest->security_margin;
916 ✗ return false;
917 }
918 ✗ if (crequest->enable_contact) { // Overlap
919 ✗ if (cresult->numContacts() < crequest->num_max_contacts)
920 ✗ cresult->addContact(
921 ✗ Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
922 ✗ static_cast<int>(root2 - tree2->getRoot())));
923 ✗ return crequest->isSatisfied(*cresult);
924 }
925 }
926
927 // Both node are leaves
928 ✗ if (bothAreLeaves) {
929 ✗ assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
930
931 ✗ Box box1, box2;
932 ✗ Transform3s box1_tf, box2_tf;
933 ✗ constructBox(bv1, tf1, box1, box1_tf);
934 ✗ constructBox(bv2, tf2, box2, box2_tf);
935
936 ✗ if (this->solver->gjk_initial_guess ==
937 GJKInitialGuess::BoundingVolumeGuess) {
938 ✗ box1.computeLocalAABB();
939 ✗ box2.computeLocalAABB();
940 }
941
942 // When reaching this point, `this->solver` has already been set up
943 // by the CollisionRequest `this->request`.
944 // The only thing we need to (and can) pass to `ShapeShapeDistance` is
945 // whether or not penetration information is should be computed in case of
946 // collision.
947 ✗ const bool compute_penetration = (this->crequest->enable_contact ||
948 ✗ (this->crequest->security_margin < 0));
949 ✗ Vec3s c1, c2, normal;
950 ✗ Scalar distance = internal::ShapeShapeDistance<Box, Box>(
951 ✗ &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
952 c2, normal);
953
954 ✗ const Scalar distToCollision = distance - this->crequest->security_margin;
955
956 ✗ internal::updateDistanceLowerBoundFromLeaf(
957 ✗ *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
958
959 ✗ if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
960 ✗ if (distToCollision <= this->crequest->collision_distance_threshold)
961 ✗ this->cresult->addContact(
962 ✗ Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
963 ✗ static_cast<int>(root2 - tree2->getRoot()), c1, c2,
964 normal, distance));
965 }
966
967 ✗ return crequest->isSatisfied(*cresult);
968 ✗ }
969
970 // Determine which tree to traverse first.
971 ✗ if (!tree2->nodeHasChildren(root2) ||
972 ✗ (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
973 ✗ for (unsigned int i = 0; i < 8; ++i) {
974 ✗ if (tree1->nodeChildExists(root1, i)) {
975 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
976 ✗ AABB child_bv;
977 ✗ computeChildBV(bv1, i, child_bv);
978
979 ✗ if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
980 tf1, tf2))
981 ✗ return true;
982 }
983 }
984 } else {
985 ✗ for (unsigned int i = 0; i < 8; ++i) {
986 ✗ if (tree2->nodeChildExists(root2, i)) {
987 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
988 ✗ AABB child_bv;
989 ✗ computeChildBV(bv2, i, child_bv);
990
991 ✗ if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
992 tf1, tf2))
993 ✗ return true;
994 }
995 }
996 }
997
998 ✗ return false;
999 }
1000 };
1001
1002 /// @addtogroup Traversal_For_Collision
1003 /// @{
1004
1005 /// @brief Traversal node for octree collision
1006 class COAL_DLLAPI OcTreeCollisionTraversalNode
1007 : public CollisionTraversalNodeBase {
1008 public:
1009 ✗ OcTreeCollisionTraversalNode(const CollisionRequest& request)
1010 ✗ : CollisionTraversalNodeBase(request) {
1011 ✗ model1 = NULL;
1012 ✗ model2 = NULL;
1013
1014 ✗ otsolver = NULL;
1015 ✗ }
1016
1017 ✗ bool BVDisjoints(unsigned, unsigned, Scalar&) const { return false; }
1018
1019 ✗ void leafCollides(unsigned, unsigned, Scalar& sqrDistLowerBound) const {
1020 ✗ otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
1021 ✗ sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1022 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1023 ✗ }
1024
1025 const OcTree* model1;
1026 const OcTree* model2;
1027
1028 Transform3s tf1, tf2;
1029
1030 const OcTreeSolver* otsolver;
1031 };
1032
1033 /// @brief Traversal node for shape-octree collision
1034 template <typename S>
1035 class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
1036 : public CollisionTraversalNodeBase {
1037 public:
1038 ✗ ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
1039 ✗ : CollisionTraversalNodeBase(request) {
1040 ✗ model1 = NULL;
1041 ✗ model2 = NULL;
1042
1043 ✗ otsolver = NULL;
1044 ✗ }
1045
1046 ✗ bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1047
1048 ✗ void leafCollides(unsigned int, unsigned int,
1049 Scalar& sqrDistLowerBound) const {
1050 ✗ otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
1051 ✗ sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1052 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1053 ✗ }
1054
1055 const S* model1;
1056 const OcTree* model2;
1057
1058 Transform3s tf1, tf2;
1059
1060 const OcTreeSolver* otsolver;
1061 };
1062
1063 /// @brief Traversal node for octree-shape collision
1064
1065 template <typename S>
1066 class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
1067 : public CollisionTraversalNodeBase {
1068 public:
1069 2000 OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
1070
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1071 2000 model1 = NULL;
1072 2000 model2 = NULL;
1073
1074 2000 otsolver = NULL;
1075 2000 }
1076
1077 ✗ bool BVDisjoints(unsigned int, unsigned int, coal::Scalar&) const {
1078 ✗ return false;
1079 }
1080
1081 2000 void leafCollides(unsigned int, unsigned int,
1082 Scalar& sqrDistLowerBound) const {
1083 2000 otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
1084 2000 sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1085 2000 sqrDistLowerBound *= sqrDistLowerBound;
1086 2000 }
1087
1088 const OcTree* model1;
1089 const S* model2;
1090
1091 Transform3s tf1, tf2;
1092
1093 const OcTreeSolver* otsolver;
1094 };
1095
1096 /// @brief Traversal node for mesh-octree collision
1097 template <typename BV>
1098 class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
1099 : public CollisionTraversalNodeBase {
1100 public:
1101 200 MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
1102
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 : CollisionTraversalNodeBase(request) {
1103 200 model1 = NULL;
1104 200 model2 = NULL;
1105
1106 200 otsolver = NULL;
1107 200 }
1108
1109 ✗ bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1110
1111 200 void leafCollides(unsigned int, unsigned int,
1112 Scalar& sqrDistLowerBound) const {
1113 200 otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
1114 200 sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1115 200 sqrDistLowerBound *= sqrDistLowerBound;
1116 200 }
1117
1118 const BVHModel<BV>* model1;
1119 const OcTree* model2;
1120
1121 Transform3s tf1, tf2;
1122
1123 const OcTreeSolver* otsolver;
1124 };
1125
1126 /// @brief Traversal node for octree-mesh collision
1127 template <typename BV>
1128 class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
1129 : public CollisionTraversalNodeBase {
1130 public:
1131 ✗ OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
1132 ✗ : CollisionTraversalNodeBase(request) {
1133 ✗ model1 = NULL;
1134 ✗ model2 = NULL;
1135
1136 ✗ otsolver = NULL;
1137 ✗ }
1138
1139 ✗ bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1140
1141 ✗ void leafCollides(unsigned int, unsigned int,
1142 Scalar& sqrDistLowerBound) const {
1143 ✗ otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
1144 ✗ sqrDistLowerBound = std::max((Scalar)0, result->distance_lower_bound);
1145 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1146 ✗ }
1147
1148 const OcTree* model1;
1149 const BVHModel<BV>* model2;
1150
1151 Transform3s tf1, tf2;
1152
1153 const OcTreeSolver* otsolver;
1154 };
1155
1156 /// @brief Traversal node for octree-height-field collision
1157 template <typename BV>
1158 class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
1159 : public CollisionTraversalNodeBase {
1160 public:
1161 2000 OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
1162
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1163 2000 model1 = NULL;
1164 2000 model2 = NULL;
1165
1166 2000 otsolver = NULL;
1167 2000 }
1168
1169 ✗ bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1170
1171 2000 void leafCollides(unsigned int, unsigned int,
1172 Scalar& sqrDistLowerBound) const {
1173 2000 otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
1174 2000 *result, sqrDistLowerBound);
1175 2000 }
1176
1177 const OcTree* model1;
1178 const HeightField<BV>* model2;
1179
1180 Transform3s tf1, tf2;
1181
1182 const OcTreeSolver* otsolver;
1183 };
1184
1185 /// @brief Traversal node for octree-height-field collision
1186 template <typename BV>
1187 class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
1188 : public CollisionTraversalNodeBase {
1189 public:
1190 2000 HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
1191
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1192 2000 model1 = NULL;
1193 2000 model2 = NULL;
1194
1195 2000 otsolver = NULL;
1196 2000 }
1197
1198 ✗ bool BVDisjoints(unsigned int, unsigned int, Scalar&) const { return false; }
1199
1200 2000 void leafCollides(unsigned int, unsigned int,
1201 Scalar& sqrDistLowerBound) const {
1202 2000 otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
1203 2000 *result, sqrDistLowerBound);
1204 2000 }
1205
1206 const HeightField<BV>* model1;
1207 const OcTree* model2;
1208
1209 Transform3s tf1, tf2;
1210
1211 const OcTreeSolver* otsolver;
1212 };
1213
1214 /// @}
1215
1216 /// @addtogroup Traversal_For_Distance
1217 /// @{
1218
1219 /// @brief Traversal node for octree distance
1220 class COAL_DLLAPI OcTreeDistanceTraversalNode
1221 : public DistanceTraversalNodeBase {
1222 public:
1223 ✗ OcTreeDistanceTraversalNode() {
1224 ✗ model1 = NULL;
1225 ✗ model2 = NULL;
1226
1227 ✗ otsolver = NULL;
1228 ✗ }
1229
1230 ✗ Scalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
1231
1232 bool BVDistanceLowerBound(unsigned, unsigned, Scalar&) const { return false; }
1233
1234 ✗ void leafComputeDistance(unsigned, unsigned int) const {
1235 ✗ otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
1236 ✗ }
1237
1238 const OcTree* model1;
1239 const OcTree* model2;
1240
1241 const OcTreeSolver* otsolver;
1242 };
1243
1244 /// @brief Traversal node for shape-octree distance
1245 template <typename Shape>
1246 class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
1247 : public DistanceTraversalNodeBase {
1248 public:
1249 ✗ ShapeOcTreeDistanceTraversalNode() {
1250 ✗ model1 = NULL;
1251 ✗ model2 = NULL;
1252
1253 ✗ otsolver = NULL;
1254 ✗ }
1255
1256 ✗ Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1257
1258 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1259 ✗ otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
1260 ✗ }
1261
1262 const Shape* model1;
1263 const OcTree* model2;
1264
1265 const OcTreeSolver* otsolver;
1266 };
1267
1268 /// @brief Traversal node for octree-shape distance
1269 template <typename Shape>
1270 class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
1271 : public DistanceTraversalNodeBase {
1272 public:
1273 ✗ OcTreeShapeDistanceTraversalNode() {
1274 ✗ model1 = NULL;
1275 ✗ model2 = NULL;
1276
1277 ✗ otsolver = NULL;
1278 ✗ }
1279
1280 ✗ Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1281
1282 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1283 ✗ otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
1284 ✗ }
1285
1286 const OcTree* model1;
1287 const Shape* model2;
1288
1289 const OcTreeSolver* otsolver;
1290 };
1291
1292 /// @brief Traversal node for mesh-octree distance
1293 template <typename BV>
1294 class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
1295 : public DistanceTraversalNodeBase {
1296 public:
1297 ✗ MeshOcTreeDistanceTraversalNode() {
1298 ✗ model1 = NULL;
1299 ✗ model2 = NULL;
1300
1301 ✗ otsolver = NULL;
1302 ✗ }
1303
1304 ✗ Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1305
1306 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1307 ✗ otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
1308 ✗ }
1309
1310 const BVHModel<BV>* model1;
1311 const OcTree* model2;
1312
1313 const OcTreeSolver* otsolver;
1314 };
1315
1316 /// @brief Traversal node for octree-mesh distance
1317 template <typename BV>
1318 class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
1319 : public DistanceTraversalNodeBase {
1320 public:
1321 ✗ OcTreeMeshDistanceTraversalNode() {
1322 ✗ model1 = NULL;
1323 ✗ model2 = NULL;
1324
1325 ✗ otsolver = NULL;
1326 ✗ }
1327
1328 ✗ Scalar BVDistanceLowerBound(unsigned int, unsigned int) const { return -1; }
1329
1330 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1331 ✗ otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
1332 ✗ }
1333
1334 const OcTree* model1;
1335 const BVHModel<BV>* model2;
1336
1337 const OcTreeSolver* otsolver;
1338 };
1339
1340 /// @}
1341
1342 } // namespace coal
1343
1344 /// @endcond
1345
1346 #endif
1347