GCC Code Coverage Report

Directory: ./
File: include/coal/internal/traversal_node_octree.h
Date: 2025-05-09 13:08:51
Exec Total Coverage
Lines: 250 571 43.8%
Branches: 250 898 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
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 OcTreeMeshIntersectRecurse(tree1, tree1->getRoot(), tree1->getRootBV(),
109 tree2, 0, tf1, tf2);
110 }
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_, CoalScalar& sqrDistLowerBound) const {
158 2000 crequest = &request_;
159 2000 cresult = &result_;
160
161
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 OcTreeHeightFieldIntersectRecurse(tree1, tree1->getRoot(),
162 tree1->getRootBV(), tree2, 0, tf1, tf2,
163 sqrDistLowerBound);
164 }
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 CoalScalar& sqrDistLowerBound) const {
173 2000 crequest = &request_;
174 2000 cresult = &result_;
175
176
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 HeightFieldOcTreeIntersectRecurse(tree1, 0, tree2, tree2->getRoot(),
177 tree2->getRootBV(), tf1, tf2,
178 sqrDistLowerBound);
179 }
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 }
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 CoalScalar 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 ✗ CoalScalar 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 CoalScalar 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 }
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 Triangle& 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 CoalScalar distance =
395 ✗ internal::ShapeShapeDistance<Box, TriangleP>(
396 ✗ &box, box_tf, &tri, tf2, this->solver,
397 ✗ this->drequest->enable_signed_distance, p1, p2, normal);
398
399 ✗ this->dresult->update(distance, tree1, tree2,
400 ✗ (int)(root1 - tree1->getRoot()),
401 static_cast<int>(primitive_id), p1, p2, normal);
402
403 ✗ return this->drequest->isSatisfied(*dresult);
404 ✗ } else
405 ✗ return false;
406 }
407
408 ✗ if (!tree1->isNodeOccupied(root1)) return false;
409
410 ✗ if (tree2->getBV(root2).isLeaf() ||
411 ✗ (tree1->nodeHasChildren(root1) &&
412 ✗ (bv1.size() > tree2->getBV(root2).bv.size()))) {
413 ✗ for (unsigned int i = 0; i < 8; ++i) {
414 ✗ if (tree1->nodeChildExists(root1, i)) {
415 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
416 ✗ AABB child_bv;
417 ✗ computeChildBV(bv1, i, child_bv);
418
419 CoalScalar d;
420 ✗ AABB aabb1, aabb2;
421 ✗ convertBV(child_bv, tf1, aabb1);
422 ✗ convertBV(tree2->getBV(root2).bv, tf2, aabb2);
423 ✗ d = aabb1.distance(aabb2);
424
425 ✗ if (d < dresult->min_distance) {
426 ✗ if (OcTreeMeshDistanceRecurse(tree1, child, child_bv, tree2, root2,
427 tf1, tf2))
428 ✗ return true;
429 }
430 }
431 }
432 } else {
433 CoalScalar d;
434 ✗ AABB aabb1, aabb2;
435 ✗ convertBV(bv1, tf1, aabb1);
436 ✗ unsigned int child = (unsigned int)tree2->getBV(root2).leftChild();
437 ✗ convertBV(tree2->getBV(child).bv, tf2, aabb2);
438 ✗ d = aabb1.distance(aabb2);
439
440 ✗ if (d < dresult->min_distance) {
441 ✗ if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
442 tf2))
443 ✗ return true;
444 }
445
446 ✗ child = (unsigned int)tree2->getBV(root2).rightChild();
447 ✗ convertBV(tree2->getBV(child).bv, tf2, aabb2);
448 ✗ d = aabb1.distance(aabb2);
449
450 ✗ if (d < dresult->min_distance) {
451 ✗ if (OcTreeMeshDistanceRecurse(tree1, root1, bv1, tree2, child, tf1,
452 tf2))
453 ✗ return true;
454 }
455 }
456
457 ✗ return false;
458 }
459
460 /// \return True if the request is satisfied.
461 template <typename BV>
462 57964 bool OcTreeMeshIntersectRecurse(const OcTree* tree1,
463 const OcTree::OcTreeNode* root1,
464 const AABB& bv1, const BVHModel<BV>* tree2,
465 unsigned int root2, const Transform3s& tf1,
466 const Transform3s& tf2) const {
467 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
468 // code in this if(!root1) did not output anything so the empty OcTree is
469 // considered free. Should an empty OcTree be considered free ?
470
471 // Empty OcTree is considered free.
472
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28982 times.
 57964 if (!root1) return false;
473 57964 BVNode<BV> const& bvn2 = tree2->getBV(root2);
474
475 /// stop when 1) bounding boxes of two objects not overlap; OR
476 /// 2) at least one of the nodes is free; OR
477 /// 2) (two uncertain nodes OR one node occupied and one node
478 /// uncertain) AND cost not required
479
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28982 times.
 57964 if (tree1->isNodeFree(root1))
480 ✗ return false;
481
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()))
482 ✗ return false;
483 else {
484
2/4
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28982 times.
✗ Branch 5 not taken.
 57964 OBB obb1, obb2;
485
1/2
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
 57964 convertBV(bv1, tf1, obb1);
486
1/2
✓ Branch 1 taken 28982 times.
✗ Branch 2 not taken.
 57964 convertBV(bvn2.bv, tf2, obb2);
487 CoalScalar sqrDistLowerBound;
488
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)) {
489 34860 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
490 sqrDistLowerBound);
491 34860 return false;
492 }
493 }
494
495 // Check if leaf collides.
496
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()) {
497
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 539 times.
 1078 assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
498
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 Box box;
499
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 Transform3s box_tf;
500
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 constructBox(bv1, tf1, box, box_tf);
501
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
 1078 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
502 ✗ box.computeLocalAABB();
503 }
504
505 1078 size_t primitive_id = static_cast<size_t>(bvn2.primitiveId());
506 1078 const Triangle& tri_id = (*(tree2->tri_indices))[primitive_id];
507
1/2
✓ Branch 4 taken 539 times.
✗ Branch 5 not taken.
 1078 const TriangleP tri((*(tree2->vertices))[tri_id[0]],
508 1078 (*(tree2->vertices))[tri_id[1]],
509 1078 (*(tree2->vertices))[tri_id[2]]);
510
511 // When reaching this point, `this->solver` has already been set up
512 // by the CollisionRequest `this->crequest`.
513 // The only thing we need to (and can) pass to `ShapeShapeDistance` is
514 // whether or not penetration information is should be computed in case of
515 // collision.
516
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 539 times.
 1078 const bool compute_penetration = this->crequest->enable_contact ||
517 ✗ (this->crequest->security_margin < 0);
518
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;
519 2156 const CoalScalar distance = internal::ShapeShapeDistance<Box, TriangleP>(
520
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 &box, box_tf, &tri, tf2, this->solver, compute_penetration, c1, c2,
521 normal);
522 1078 const CoalScalar distToCollision =
523 1078 distance - this->crequest->security_margin;
524
525 1078 internal::updateDistanceLowerBoundFromLeaf(
526
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
527
528
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 if (cresult->numContacts() < crequest->num_max_contacts) {
529
2/2
✓ Branch 0 taken 53 times.
✓ Branch 1 taken 486 times.
 1078 if (distToCollision <= crequest->collision_distance_threshold) {
530
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 106 cresult->addContact(Contact(
531
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()),
532 static_cast<int>(primitive_id), c1, c2, normal, distance));
533 }
534 }
535
1/2
✓ Branch 1 taken 539 times.
✗ Branch 2 not taken.
 1078 return crequest->isSatisfied(*cresult);
536 }
537
538 // Determine which tree to traverse first.
539
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() ||
540
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()))) {
541
2/2
✓ Branch 0 taken 42996 times.
✓ Branch 1 taken 4790 times.
 95572 for (unsigned int i = 0; i < 8; ++i) {
542
2/2
✓ Branch 1 taken 18305 times.
✓ Branch 2 taken 24691 times.
 85992 if (tree1->nodeChildExists(root1, i)) {
543
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
544
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 AABB child_bv;
545
1/2
✓ Branch 1 taken 18305 times.
✗ Branch 2 not taken.
 36610 computeChildBV(bv1, i, child_bv);
546
547
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,
548 tf1, tf2))
549 1620 return true;
550 }
551 }
552 } else {
553
2/2
✓ Branch 1 taken 249 times.
✓ Branch 2 taken 5164 times.
 10826 if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
554 10826 (unsigned int)bvn2.leftChild(), tf1, tf2))
555 498 return true;
556
557
2/2
✓ Branch 1 taken 147 times.
✓ Branch 2 taken 5017 times.
 10328 if (OcTreeMeshIntersectRecurse(tree1, root1, bv1, tree2,
558 10328 (unsigned int)bvn2.rightChild(), tf1, tf2))
559 294 return true;
560 }
561
562 19614 return false;
563 }
564
565 /// \return True if the request is satisfied.
566 template <typename BV>
567 56584 bool OcTreeHeightFieldIntersectRecurse(
568 const OcTree* tree1, const OcTree::OcTreeNode* root1, const AABB& bv1,
569 const HeightField<BV>* tree2, unsigned int root2, const Transform3s& tf1,
570 const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
571 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
572 // code in this if(!root1) did not output anything so the empty OcTree is
573 // considered free. Should an empty OcTree be considered free ?
574
575 // Empty OcTree is considered free.
576
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
 56584 if (!root1) return false;
577 56584 HFNode<BV> const& bvn2 = tree2->getBV(root2);
578
579 /// stop when 1) bounding boxes of two objects not overlap; OR
580 /// 2) at least one of the nodes is free; OR
581 /// 2) (two uncertain nodes OR one node occupied and one node
582 /// uncertain) AND cost not required
583
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
 56584 if (tree1->isNodeFree(root1))
584 ✗ return false;
585
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()))
586 ✗ return false;
587 else {
588
2/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
 56584 OBB obb1, obb2;
589
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bv1, tf1, obb1);
590
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bvn2.bv, tf2, obb2);
591 CoalScalar sqrDistLowerBound_;
592
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_)) {
593
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
 35600 if (sqrDistLowerBound_ < sqrDistLowerBound)
594 ✗ sqrDistLowerBound = sqrDistLowerBound_;
595 35600 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
596 sqrDistLowerBound);
597 35600 return false;
598 }
599 }
600
601 // Check if leaf collides.
602
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()) {
603
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(tree1->isNodeOccupied(root1)); // it isn't free nor uncertain.
604
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Box box;
605
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Transform3s box_tf;
606
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 constructBox(bv1, tf1, box, box_tf);
607
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
608 ✗ box.computeLocalAABB();
609 }
610
611 typedef Convex<Triangle> ConvexTriangle;
612
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ConvexTriangle convex1, convex2;
613 int convex1_active_faces, convex2_active_faces;
614
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 details::buildConvexTriangles(bvn2, *tree2, convex1, convex1_active_faces,
615 convex2, convex2_active_faces);
616
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
617 ✗ convex1.computeLocalAABB();
618 ✗ convex2.computeLocalAABB();
619 }
620
621
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;
622 CoalScalar distance;
623 bool hfield_witness_is_on_bin_side;
624
625 64 bool collision = details::shapeDistance<Triangle, Box, 0>(
626
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 solver, *crequest, convex1, convex1_active_faces, convex2,
627 convex2_active_faces, tf2, box, box_tf, distance, c2, c1, normal,
628 normal_top, hfield_witness_is_on_bin_side);
629
630 32 CoalScalar distToCollision =
631
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 distance - crequest->security_margin * (normal_top.dot(normal));
632
633
1/2
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
 32 if (distToCollision <= crequest->collision_distance_threshold) {
634 32 sqrDistLowerBound = 0;
635
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 if (crequest->num_max_contacts > cresult->numContacts()) {
636
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) &&
637 ✗ (collision || !hfield_witness_is_on_bin_side)) {
638
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 cresult->addContact(
639
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()),
640 (int)Contact::NONE, c1, c2, -normal, distance));
641 }
642 }
643 } else
644 ✗ sqrDistLowerBound = distToCollision * distToCollision;
645
646 // const Vec3s c1 = contact_point - distance * 0.5 * normal;
647 // const Vec3s c2 = contact_point + distance * 0.5 * normal;
648
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 internal::updateDistanceLowerBoundFromLeaf(
649
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 *crequest, *cresult, distToCollision, c1, c2, -normal);
650
651
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);
652
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 return crequest->isSatisfied(*cresult);
653 }
654
655 // Determine which tree to traverse first.
656
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() ||
657
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()))) {
658
2/2
✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
 182828 for (unsigned int i = 0; i < 8; ++i) {
659
2/2
✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
 162778 if (tree1->nodeChildExists(root1, i)) {
660
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
661
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 AABB child_bv;
662
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 computeChildBV(bv1, i, child_bv);
663
664
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,
665 root2, tf1, tf2,
666 sqrDistLowerBound))
667 492 return true;
668 }
669 }
670 } else {
671
2/2
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
 410 if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
672 410 (unsigned int)bvn2.leftChild(), tf1,
673 tf2, sqrDistLowerBound))
674 288 return true;
675
676
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 122 if (OcTreeHeightFieldIntersectRecurse(tree1, root1, bv1, tree2,
677 122 (unsigned int)bvn2.rightChild(),
678 tf1, tf2, sqrDistLowerBound))
679 122 return true;
680 }
681
682 20050 return false;
683 }
684
685 /// \return True if the request is satisfied.
686 template <typename BV>
687 56584 bool HeightFieldOcTreeIntersectRecurse(
688 const HeightField<BV>* tree1, unsigned int root1, const OcTree* tree2,
689 const OcTree::OcTreeNode* root2, const AABB& bv2, const Transform3s& tf1,
690 const Transform3s& tf2, CoalScalar& sqrDistLowerBound) const {
691 // FIXME(jmirabel) I do not understand why the BVHModel was traversed. The
692 // code in this if(!root1) did not output anything so the empty OcTree is
693 // considered free. Should an empty OcTree be considered free ?
694
695 // Empty OcTree is considered free.
696
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 28292 times.
 56584 if (!root2) return false;
697 56584 HFNode<BV> const& bvn1 = tree1->getBV(root1);
698
699 /// stop when 1) bounding boxes of two objects not overlap; OR
700 /// 2) at least one of the nodes is free; OR
701 /// 2) (two uncertain nodes OR one node occupied and one node
702 /// uncertain) AND cost not required
703
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 28292 times.
 56584 if (tree2->isNodeFree(root2))
704 ✗ return false;
705
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()))
706 ✗ return false;
707 else {
708
2/4
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28292 times.
✗ Branch 5 not taken.
 56584 OBB obb1, obb2;
709
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bvn1.bv, tf1, obb1);
710
1/2
✓ Branch 1 taken 28292 times.
✗ Branch 2 not taken.
 56584 convertBV(bv2, tf2, obb2);
711 CoalScalar sqrDistLowerBound_;
712
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_)) {
713
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 17800 times.
 35600 if (sqrDistLowerBound_ < sqrDistLowerBound)
714 ✗ sqrDistLowerBound = sqrDistLowerBound_;
715 35600 internal::updateDistanceLowerBoundFromBV(*crequest, *cresult,
716 sqrDistLowerBound);
717 35600 return false;
718 }
719 }
720
721 // Check if leaf collides.
722
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()) {
723
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 16 times.
 32 assert(tree2->isNodeOccupied(root2)); // it isn't free nor uncertain.
724
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Box box;
725
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 Transform3s box_tf;
726
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 constructBox(bv2, tf2, box, box_tf);
727
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
728 ✗ box.computeLocalAABB();
729 }
730
731 typedef Convex<Triangle> ConvexTriangle;
732
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 ConvexTriangle convex1, convex2;
733 int convex1_active_faces, convex2_active_faces;
734
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 details::buildConvexTriangles(bvn1, *tree1, convex1, convex1_active_faces,
735 convex2, convex2_active_faces);
736
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 32 if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
737 ✗ convex1.computeLocalAABB();
738 ✗ convex2.computeLocalAABB();
739 }
740
741
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;
742 CoalScalar distance;
743 bool hfield_witness_is_on_bin_side;
744
745 64 bool collision = details::shapeDistance<Triangle, Box, 0>(
746
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 solver, *crequest, convex1, convex1_active_faces, convex2,
747 convex2_active_faces, tf1, box, box_tf, distance, c1, c2, normal,
748 normal_top, hfield_witness_is_on_bin_side);
749
750 32 CoalScalar distToCollision =
751
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 distance - crequest->security_margin * (normal_top.dot(normal));
752
753
1/2
✓ Branch 0 taken 16 times.
✗ Branch 1 not taken.
 32 if (distToCollision <= crequest->collision_distance_threshold) {
754 32 sqrDistLowerBound = 0;
755
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 if (crequest->num_max_contacts > cresult->numContacts()) {
756
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) &&
757 ✗ (collision || !hfield_witness_is_on_bin_side)) {
758
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 cresult->addContact(Contact(tree1, tree2, (int)Contact::NONE,
759
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,
760 normal, distance));
761 }
762 }
763 } else
764 ✗ sqrDistLowerBound = distToCollision * distToCollision;
765
766 // const Vec3s c1 = contact_point - distance * 0.5 * normal;
767 // const Vec3s c2 = contact_point + distance * 0.5 * normal;
768 32 internal::updateDistanceLowerBoundFromLeaf(
769
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 *crequest, *cresult, distToCollision, c1, c2, normal);
770
771
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);
772
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 32 return crequest->isSatisfied(*cresult);
773 }
774
775 // Determine which tree to traverse first.
776
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() ||
777
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()))) {
778
2/2
✓ Branch 0 taken 81389 times.
✓ Branch 1 taken 10025 times.
 182828 for (unsigned int i = 0; i < 8; ++i) {
779
2/2
✓ Branch 1 taken 27026 times.
✓ Branch 2 taken 54363 times.
 162778 if (tree2->nodeChildExists(root2, i)) {
780
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
781
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 AABB child_bv;
782
1/2
✓ Branch 1 taken 27026 times.
✗ Branch 2 not taken.
 54052 computeChildBV(bv2, i, child_bv);
783
784
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,
785 child_bv, tf1, tf2,
786 sqrDistLowerBound))
787 492 return true;
788 }
789 }
790 } else {
791
2/2
✓ Branch 1 taken 144 times.
✓ Branch 2 taken 61 times.
 410 if (HeightFieldOcTreeIntersectRecurse(
792 410 tree1, (unsigned int)bvn1.leftChild(), tree2, root2, bv2, tf1,
793 tf2, sqrDistLowerBound))
794 288 return true;
795
796
1/2
✓ Branch 1 taken 61 times.
✗ Branch 2 not taken.
 122 if (HeightFieldOcTreeIntersectRecurse(
797 122 tree1, (unsigned int)bvn1.rightChild(), tree2, root2, bv2, tf1,
798 tf2, sqrDistLowerBound))
799 122 return true;
800 }
801
802 20050 return false;
803 }
804
805 ✗ bool OcTreeDistanceRecurse(const OcTree* tree1,
806 const OcTree::OcTreeNode* root1, const AABB& bv1,
807 const OcTree* tree2,
808 const OcTree::OcTreeNode* root2, const AABB& bv2,
809 const Transform3s& tf1,
810 const Transform3s& tf2) const {
811 ✗ if (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2)) {
812 ✗ if (tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2)) {
813 ✗ Box box1, box2;
814 ✗ Transform3s box1_tf, box2_tf;
815 ✗ constructBox(bv1, tf1, box1, box1_tf);
816 ✗ constructBox(bv2, tf2, box2, box2_tf);
817
818 ✗ if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
819 ✗ box1.computeLocalAABB();
820 ✗ box2.computeLocalAABB();
821 }
822
823 ✗ Vec3s p1, p2, normal;
824 ✗ const CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
825 ✗ &box1, box1_tf, &box2, box2_tf, this->solver,
826 ✗ this->drequest->enable_signed_distance, p1, p2, normal);
827
828 ✗ this->dresult->update(distance, tree1, tree2,
829 ✗ (int)(root1 - tree1->getRoot()),
830 ✗ (int)(root2 - tree2->getRoot()), p1, p2, normal);
831
832 ✗ return drequest->isSatisfied(*dresult);
833 ✗ } else
834 ✗ return false;
835 }
836
837 ✗ if (!tree1->isNodeOccupied(root1) || !tree2->isNodeOccupied(root2))
838 ✗ return false;
839
840 ✗ if (!tree2->nodeHasChildren(root2) ||
841 ✗ (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
842 ✗ for (unsigned int i = 0; i < 8; ++i) {
843 ✗ if (tree1->nodeChildExists(root1, i)) {
844 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
845 ✗ AABB child_bv;
846 ✗ computeChildBV(bv1, i, child_bv);
847
848 CoalScalar d;
849 ✗ AABB aabb1, aabb2;
850 ✗ convertBV(bv1, tf1, aabb1);
851 ✗ convertBV(bv2, tf2, aabb2);
852 ✗ d = aabb1.distance(aabb2);
853
854 ✗ if (d < dresult->min_distance) {
855 ✗ if (OcTreeDistanceRecurse(tree1, child, child_bv, tree2, root2, bv2,
856 tf1, tf2))
857 ✗ return true;
858 }
859 }
860 }
861 } else {
862 ✗ for (unsigned int i = 0; i < 8; ++i) {
863 ✗ if (tree2->nodeChildExists(root2, i)) {
864 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
865 ✗ AABB child_bv;
866 ✗ computeChildBV(bv2, i, child_bv);
867
868 CoalScalar d;
869 ✗ AABB aabb1, aabb2;
870 ✗ convertBV(bv1, tf1, aabb1);
871 ✗ convertBV(bv2, tf2, aabb2);
872 ✗ d = aabb1.distance(aabb2);
873
874 ✗ if (d < dresult->min_distance) {
875 ✗ if (OcTreeDistanceRecurse(tree1, root1, bv1, tree2, child, child_bv,
876 tf1, tf2))
877 ✗ return true;
878 }
879 }
880 }
881 }
882
883 ✗ return false;
884 }
885
886 ✗ bool OcTreeIntersectRecurse(const OcTree* tree1,
887 const OcTree::OcTreeNode* root1, const AABB& bv1,
888 const OcTree* tree2,
889 const OcTree::OcTreeNode* root2, const AABB& bv2,
890 const Transform3s& tf1,
891 const Transform3s& tf2) const {
892 // Empty OcTree is considered free.
893 ✗ if (!root1) return false;
894 ✗ if (!root2) return false;
895
896 /// stop when 1) bounding boxes of two objects not overlap; OR
897 /// 2) at least one of the nodes is free; OR
898 /// 2) (two uncertain nodes OR one node occupied and one node
899 /// uncertain) AND cost not required
900 ✗ if (tree1->isNodeFree(root1) || tree2->isNodeFree(root2))
901 ✗ return false;
902 ✗ else if ((tree1->isNodeUncertain(root1) || tree2->isNodeUncertain(root2)))
903 ✗ return false;
904
905 bool bothAreLeaves =
906 ✗ (!tree1->nodeHasChildren(root1) && !tree2->nodeHasChildren(root2));
907 ✗ if (!bothAreLeaves || !crequest->enable_contact) {
908 ✗ OBB obb1, obb2;
909 ✗ convertBV(bv1, tf1, obb1);
910 ✗ convertBV(bv2, tf2, obb2);
911 CoalScalar sqrDistLowerBound;
912 ✗ if (!obb1.overlap(obb2, *crequest, sqrDistLowerBound)) {
913 ✗ if (cresult->distance_lower_bound > 0 &&
914 ✗ sqrDistLowerBound <
915 ✗ cresult->distance_lower_bound * cresult->distance_lower_bound)
916 ✗ cresult->distance_lower_bound =
917 ✗ sqrt(sqrDistLowerBound) - crequest->security_margin;
918 ✗ return false;
919 }
920 ✗ if (crequest->enable_contact) { // Overlap
921 ✗ if (cresult->numContacts() < crequest->num_max_contacts)
922 ✗ cresult->addContact(
923 ✗ Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
924 ✗ static_cast<int>(root2 - tree2->getRoot())));
925 ✗ return crequest->isSatisfied(*cresult);
926 }
927 }
928
929 // Both node are leaves
930 ✗ if (bothAreLeaves) {
931 ✗ assert(tree1->isNodeOccupied(root1) && tree2->isNodeOccupied(root2));
932
933 ✗ Box box1, box2;
934 ✗ Transform3s box1_tf, box2_tf;
935 ✗ constructBox(bv1, tf1, box1, box1_tf);
936 ✗ constructBox(bv2, tf2, box2, box2_tf);
937
938 ✗ if (this->solver->gjk_initial_guess ==
939 GJKInitialGuess::BoundingVolumeGuess) {
940 ✗ box1.computeLocalAABB();
941 ✗ box2.computeLocalAABB();
942 }
943
944 // When reaching this point, `this->solver` has already been set up
945 // by the CollisionRequest `this->request`.
946 // The only thing we need to (and can) pass to `ShapeShapeDistance` is
947 // whether or not penetration information is should be computed in case of
948 // collision.
949 ✗ const bool compute_penetration = (this->crequest->enable_contact ||
950 ✗ (this->crequest->security_margin < 0));
951 ✗ Vec3s c1, c2, normal;
952 ✗ CoalScalar distance = internal::ShapeShapeDistance<Box, Box>(
953 ✗ &box1, box1_tf, &box2, box2_tf, this->solver, compute_penetration, c1,
954 c2, normal);
955
956 ✗ const CoalScalar distToCollision =
957 ✗ distance - this->crequest->security_margin;
958
959 ✗ internal::updateDistanceLowerBoundFromLeaf(
960 ✗ *(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
961
962 ✗ if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
963 ✗ if (distToCollision <= this->crequest->collision_distance_threshold)
964 ✗ this->cresult->addContact(
965 ✗ Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
966 ✗ static_cast<int>(root2 - tree2->getRoot()), c1, c2,
967 normal, distance));
968 }
969
970 ✗ return crequest->isSatisfied(*cresult);
971 }
972
973 // Determine which tree to traverse first.
974 ✗ if (!tree2->nodeHasChildren(root2) ||
975 ✗ (tree1->nodeHasChildren(root1) && (bv1.size() > bv2.size()))) {
976 ✗ for (unsigned int i = 0; i < 8; ++i) {
977 ✗ if (tree1->nodeChildExists(root1, i)) {
978 ✗ const OcTree::OcTreeNode* child = tree1->getNodeChild(root1, i);
979 ✗ AABB child_bv;
980 ✗ computeChildBV(bv1, i, child_bv);
981
982 ✗ if (OcTreeIntersectRecurse(tree1, child, child_bv, tree2, root2, bv2,
983 tf1, tf2))
984 ✗ return true;
985 }
986 }
987 } else {
988 ✗ for (unsigned int i = 0; i < 8; ++i) {
989 ✗ if (tree2->nodeChildExists(root2, i)) {
990 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
991 ✗ AABB child_bv;
992 ✗ computeChildBV(bv2, i, child_bv);
993
994 ✗ if (OcTreeIntersectRecurse(tree1, root1, bv1, tree2, child, child_bv,
995 tf1, tf2))
996 ✗ return true;
997 }
998 }
999 }
1000
1001 ✗ return false;
1002 }
1003 };
1004
1005 /// @addtogroup Traversal_For_Collision
1006 /// @{
1007
1008 /// @brief Traversal node for octree collision
1009 class COAL_DLLAPI OcTreeCollisionTraversalNode
1010 : public CollisionTraversalNodeBase {
1011 public:
1012 ✗ OcTreeCollisionTraversalNode(const CollisionRequest& request)
1013 ✗ : CollisionTraversalNodeBase(request) {
1014 ✗ model1 = NULL;
1015 ✗ model2 = NULL;
1016
1017 ✗ otsolver = NULL;
1018 }
1019
1020 ✗ bool BVDisjoints(unsigned, unsigned, CoalScalar&) const { return false; }
1021
1022 ✗ void leafCollides(unsigned, unsigned, CoalScalar& sqrDistLowerBound) const {
1023 ✗ otsolver->OcTreeIntersect(model1, model2, tf1, tf2, request, *result);
1024 ✗ sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1025 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1026 }
1027
1028 const OcTree* model1;
1029 const OcTree* model2;
1030
1031 Transform3s tf1, tf2;
1032
1033 const OcTreeSolver* otsolver;
1034 };
1035
1036 /// @brief Traversal node for shape-octree collision
1037 template <typename S>
1038 class COAL_DLLAPI ShapeOcTreeCollisionTraversalNode
1039 : public CollisionTraversalNodeBase {
1040 public:
1041 ✗ ShapeOcTreeCollisionTraversalNode(const CollisionRequest& request)
1042 ✗ : CollisionTraversalNodeBase(request) {
1043 ✗ model1 = NULL;
1044 ✗ model2 = NULL;
1045
1046 ✗ otsolver = NULL;
1047 }
1048
1049 ✗ bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1050 ✗ return false;
1051 }
1052
1053 ✗ void leafCollides(unsigned int, unsigned int,
1054 CoalScalar& sqrDistLowerBound) const {
1055 ✗ otsolver->OcTreeShapeIntersect(model2, *model1, tf2, tf1, request, *result);
1056 ✗ sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1057 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1058 }
1059
1060 const S* model1;
1061 const OcTree* model2;
1062
1063 Transform3s tf1, tf2;
1064
1065 const OcTreeSolver* otsolver;
1066 };
1067
1068 /// @brief Traversal node for octree-shape collision
1069
1070 template <typename S>
1071 class COAL_DLLAPI OcTreeShapeCollisionTraversalNode
1072 : public CollisionTraversalNodeBase {
1073 public:
1074 2000 OcTreeShapeCollisionTraversalNode(const CollisionRequest& request)
1075
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1076 2000 model1 = NULL;
1077 2000 model2 = NULL;
1078
1079 2000 otsolver = NULL;
1080 }
1081
1082 ✗ bool BVDisjoints(unsigned int, unsigned int, coal::CoalScalar&) const {
1083 ✗ return false;
1084 }
1085
1086 2000 void leafCollides(unsigned int, unsigned int,
1087 CoalScalar& sqrDistLowerBound) const {
1088 2000 otsolver->OcTreeShapeIntersect(model1, *model2, tf1, tf2, request, *result);
1089 2000 sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1090 2000 sqrDistLowerBound *= sqrDistLowerBound;
1091 }
1092
1093 const OcTree* model1;
1094 const S* model2;
1095
1096 Transform3s tf1, tf2;
1097
1098 const OcTreeSolver* otsolver;
1099 };
1100
1101 /// @brief Traversal node for mesh-octree collision
1102 template <typename BV>
1103 class COAL_DLLAPI MeshOcTreeCollisionTraversalNode
1104 : public CollisionTraversalNodeBase {
1105 public:
1106 200 MeshOcTreeCollisionTraversalNode(const CollisionRequest& request)
1107
2/4
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
 200 : CollisionTraversalNodeBase(request) {
1108 200 model1 = NULL;
1109 200 model2 = NULL;
1110
1111 200 otsolver = NULL;
1112 }
1113
1114 ✗ bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1115 ✗ return false;
1116 }
1117
1118 200 void leafCollides(unsigned int, unsigned int,
1119 CoalScalar& sqrDistLowerBound) const {
1120 200 otsolver->OcTreeMeshIntersect(model2, model1, tf2, tf1, request, *result);
1121 200 sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1122 200 sqrDistLowerBound *= sqrDistLowerBound;
1123 }
1124
1125 const BVHModel<BV>* model1;
1126 const OcTree* model2;
1127
1128 Transform3s tf1, tf2;
1129
1130 const OcTreeSolver* otsolver;
1131 };
1132
1133 /// @brief Traversal node for octree-mesh collision
1134 template <typename BV>
1135 class COAL_DLLAPI OcTreeMeshCollisionTraversalNode
1136 : public CollisionTraversalNodeBase {
1137 public:
1138 ✗ OcTreeMeshCollisionTraversalNode(const CollisionRequest& request)
1139 ✗ : CollisionTraversalNodeBase(request) {
1140 ✗ model1 = NULL;
1141 ✗ model2 = NULL;
1142
1143 ✗ otsolver = NULL;
1144 }
1145
1146 ✗ bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1147 ✗ return false;
1148 }
1149
1150 ✗ void leafCollides(unsigned int, unsigned int,
1151 CoalScalar& sqrDistLowerBound) const {
1152 ✗ otsolver->OcTreeMeshIntersect(model1, model2, tf1, tf2, request, *result);
1153 ✗ sqrDistLowerBound = std::max((CoalScalar)0, result->distance_lower_bound);
1154 ✗ sqrDistLowerBound *= sqrDistLowerBound;
1155 }
1156
1157 const OcTree* model1;
1158 const BVHModel<BV>* model2;
1159
1160 Transform3s tf1, tf2;
1161
1162 const OcTreeSolver* otsolver;
1163 };
1164
1165 /// @brief Traversal node for octree-height-field collision
1166 template <typename BV>
1167 class COAL_DLLAPI OcTreeHeightFieldCollisionTraversalNode
1168 : public CollisionTraversalNodeBase {
1169 public:
1170 2000 OcTreeHeightFieldCollisionTraversalNode(const CollisionRequest& request)
1171
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1172 2000 model1 = NULL;
1173 2000 model2 = NULL;
1174
1175 2000 otsolver = NULL;
1176 }
1177
1178 ✗ bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1179 ✗ return false;
1180 }
1181
1182 2000 void leafCollides(unsigned int, unsigned int,
1183 CoalScalar& sqrDistLowerBound) const {
1184 2000 otsolver->OcTreeHeightFieldIntersect(model1, model2, tf1, tf2, request,
1185 2000 *result, sqrDistLowerBound);
1186 }
1187
1188 const OcTree* model1;
1189 const HeightField<BV>* model2;
1190
1191 Transform3s tf1, tf2;
1192
1193 const OcTreeSolver* otsolver;
1194 };
1195
1196 /// @brief Traversal node for octree-height-field collision
1197 template <typename BV>
1198 class COAL_DLLAPI HeightFieldOcTreeCollisionTraversalNode
1199 : public CollisionTraversalNodeBase {
1200 public:
1201 2000 HeightFieldOcTreeCollisionTraversalNode(const CollisionRequest& request)
1202
2/4
✓ Branch 2 taken 1000 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1000 times.
✗ Branch 6 not taken.
 2000 : CollisionTraversalNodeBase(request) {
1203 2000 model1 = NULL;
1204 2000 model2 = NULL;
1205
1206 2000 otsolver = NULL;
1207 }
1208
1209 ✗ bool BVDisjoints(unsigned int, unsigned int, CoalScalar&) const {
1210 ✗ return false;
1211 }
1212
1213 2000 void leafCollides(unsigned int, unsigned int,
1214 CoalScalar& sqrDistLowerBound) const {
1215 2000 otsolver->HeightFieldOcTreeIntersect(model1, model2, tf1, tf2, request,
1216 2000 *result, sqrDistLowerBound);
1217 }
1218
1219 const HeightField<BV>* model1;
1220 const OcTree* model2;
1221
1222 Transform3s tf1, tf2;
1223
1224 const OcTreeSolver* otsolver;
1225 };
1226
1227 /// @}
1228
1229 /// @addtogroup Traversal_For_Distance
1230 /// @{
1231
1232 /// @brief Traversal node for octree distance
1233 class COAL_DLLAPI OcTreeDistanceTraversalNode
1234 : public DistanceTraversalNodeBase {
1235 public:
1236 ✗ OcTreeDistanceTraversalNode() {
1237 ✗ model1 = NULL;
1238 ✗ model2 = NULL;
1239
1240 ✗ otsolver = NULL;
1241 }
1242
1243 ✗ CoalScalar BVDistanceLowerBound(unsigned, unsigned) const { return -1; }
1244
1245 bool BVDistanceLowerBound(unsigned, unsigned, CoalScalar&) const {
1246 return false;
1247 }
1248
1249 ✗ void leafComputeDistance(unsigned, unsigned int) const {
1250 ✗ otsolver->OcTreeDistance(model1, model2, tf1, tf2, request, *result);
1251 }
1252
1253 const OcTree* model1;
1254 const OcTree* model2;
1255
1256 const OcTreeSolver* otsolver;
1257 };
1258
1259 /// @brief Traversal node for shape-octree distance
1260 template <typename Shape>
1261 class COAL_DLLAPI ShapeOcTreeDistanceTraversalNode
1262 : public DistanceTraversalNodeBase {
1263 public:
1264 ✗ ShapeOcTreeDistanceTraversalNode() {
1265 ✗ model1 = NULL;
1266 ✗ model2 = NULL;
1267
1268 ✗ otsolver = NULL;
1269 }
1270
1271 ✗ CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1272 ✗ return -1;
1273 }
1274
1275 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1276 ✗ otsolver->OcTreeShapeDistance(model2, *model1, tf2, tf1, request, *result);
1277 }
1278
1279 const Shape* model1;
1280 const OcTree* model2;
1281
1282 const OcTreeSolver* otsolver;
1283 };
1284
1285 /// @brief Traversal node for octree-shape distance
1286 template <typename Shape>
1287 class COAL_DLLAPI OcTreeShapeDistanceTraversalNode
1288 : public DistanceTraversalNodeBase {
1289 public:
1290 ✗ OcTreeShapeDistanceTraversalNode() {
1291 ✗ model1 = NULL;
1292 ✗ model2 = NULL;
1293
1294 ✗ otsolver = NULL;
1295 }
1296
1297 ✗ CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1298 ✗ return -1;
1299 }
1300
1301 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1302 ✗ otsolver->OcTreeShapeDistance(model1, *model2, tf1, tf2, request, *result);
1303 }
1304
1305 const OcTree* model1;
1306 const Shape* model2;
1307
1308 const OcTreeSolver* otsolver;
1309 };
1310
1311 /// @brief Traversal node for mesh-octree distance
1312 template <typename BV>
1313 class COAL_DLLAPI MeshOcTreeDistanceTraversalNode
1314 : public DistanceTraversalNodeBase {
1315 public:
1316 ✗ MeshOcTreeDistanceTraversalNode() {
1317 ✗ model1 = NULL;
1318 ✗ model2 = NULL;
1319
1320 ✗ otsolver = NULL;
1321 }
1322
1323 ✗ CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1324 ✗ return -1;
1325 }
1326
1327 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1328 ✗ otsolver->OcTreeMeshDistance(model2, model1, tf2, tf1, request, *result);
1329 }
1330
1331 const BVHModel<BV>* model1;
1332 const OcTree* model2;
1333
1334 const OcTreeSolver* otsolver;
1335 };
1336
1337 /// @brief Traversal node for octree-mesh distance
1338 template <typename BV>
1339 class COAL_DLLAPI OcTreeMeshDistanceTraversalNode
1340 : public DistanceTraversalNodeBase {
1341 public:
1342 ✗ OcTreeMeshDistanceTraversalNode() {
1343 ✗ model1 = NULL;
1344 ✗ model2 = NULL;
1345
1346 ✗ otsolver = NULL;
1347 }
1348
1349 ✗ CoalScalar BVDistanceLowerBound(unsigned int, unsigned int) const {
1350 ✗ return -1;
1351 }
1352
1353 ✗ void leafComputeDistance(unsigned int, unsigned int) const {
1354 ✗ otsolver->OcTreeMeshDistance(model1, model2, tf1, tf2, request, *result);
1355 }
1356
1357 const OcTree* model1;
1358 const BVHModel<BV>* model2;
1359
1360 const OcTreeSolver* otsolver;
1361 };
1362
1363 /// @}
1364
1365 } // namespace coal
1366
1367 /// @endcond
1368
1369 #endif
1370