GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_dynamic_AABB_tree.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 188 396 47.5%
Branches: 168 582 28.9%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** @author Jia Pan */
37
38 #include "coal/broadphase/broadphase_dynamic_AABB_tree.h"
39 #include "coal/tracy.hh"
40
41 #ifdef COAL_HAVE_OCTOMAP
42 #include "coal/octree.h"
43 #endif
44
45 #include "coal/BV/BV.h"
46 #include "coal/shape/geometric_shapes_utility.h"
47
48 #include <limits>
49
50 namespace coal {
51 namespace detail {
52
53 namespace dynamic_AABB_tree {
54
55 #if COAL_HAVE_OCTOMAP
56 //==============================================================================
57 ✗ bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
58 const OcTree* tree2, const OcTree::OcTreeNode* root2,
59 const AABB& root2_bv, const Transform3s& tf2,
60 CollisionCallBackBase* callback) {
61 ✗ if (!root2) {
62 ✗ if (root1->isLeaf()) {
63 ✗ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
64
65 ✗ if (!obj1->collisionGeometry()->isFree()) {
66 ✗ OBB obb1, obb2;
67 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
68 ✗ convertBV(root2_bv, tf2, obb2);
69
70 ✗ if (obb1.overlap(obb2)) {
71 ✗ Box* box = new Box();
72 ✗ Transform3s box_tf;
73 ✗ constructBox(root2_bv, tf2, *box, box_tf);
74
75 ✗ box->cost_density = tree2->getDefaultOccupancy();
76
77 ✗ CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
78 ✗ return (*callback)(obj1, &obj2);
79 }
80 }
81 } else {
82 ✗ if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2,
83 callback))
84 ✗ return true;
85 ✗ if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2,
86 callback))
87 ✗ return true;
88 }
89
90 ✗ return false;
91 ✗ } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
92 ✗ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
93
94 ✗ if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
95 ✗ OBB obb1, obb2;
96 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
97 ✗ convertBV(root2_bv, tf2, obb2);
98
99 ✗ if (obb1.overlap(obb2)) {
100 ✗ Box* box = new Box();
101 ✗ Transform3s box_tf;
102 ✗ constructBox(root2_bv, tf2, *box, box_tf);
103
104 ✗ box->cost_density = Scalar(root2->getOccupancy());
105 ✗ box->threshold_occupied = tree2->getOccupancyThres();
106
107 ✗ CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
108 ✗ return (*callback)(obj1, &obj2);
109 ✗ } else
110 ✗ return false;
111 } else
112 ✗ return false;
113 }
114
115 ✗ OBB obb1, obb2;
116 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
117 ✗ convertBV(root2_bv, tf2, obb2);
118
119 ✗ if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
120
121 ✗ if (!tree2->nodeHasChildren(root2) ||
122 ✗ (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
123 ✗ if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
124 callback))
125 ✗ return true;
126 ✗ if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
127 callback))
128 ✗ return true;
129 } else {
130 ✗ for (unsigned int i = 0; i < 8; ++i) {
131 ✗ if (tree2->nodeChildExists(root2, i)) {
132 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
133 ✗ AABB child_bv;
134 ✗ computeChildBV(root2_bv, i, child_bv);
135
136 ✗ if (collisionRecurse_(root1, tree2, child, child_bv, tf2, callback))
137 ✗ return true;
138 } else {
139 ✗ AABB child_bv;
140 ✗ computeChildBV(root2_bv, i, child_bv);
141 ✗ if (collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, callback))
142 ✗ return true;
143 }
144 }
145 }
146 ✗ return false;
147 }
148
149 //==============================================================================
150 ✗ bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
151 const OcTree* tree2, const OcTree::OcTreeNode* root2,
152 const AABB& root2_bv, const Transform3s& tf2,
153 DistanceCallBackBase* callback, Scalar& min_dist) {
154 ✗ if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
155 ✗ if (tree2->isNodeOccupied(root2)) {
156 ✗ Box* box = new Box();
157 ✗ Transform3s box_tf;
158 ✗ constructBox(root2_bv, tf2, *box, box_tf);
159 ✗ CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
160 ✗ return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
161 ✗ min_dist);
162 ✗ } else
163 ✗ return false;
164 }
165
166 ✗ if (!tree2->isNodeOccupied(root2)) return false;
167
168 ✗ if (!tree2->nodeHasChildren(root2) ||
169 ✗ (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
170 ✗ AABB aabb2;
171 ✗ convertBV(root2_bv, tf2, aabb2);
172
173 ✗ Scalar d1 = aabb2.distance(root1->children[0]->bv);
174 ✗ Scalar d2 = aabb2.distance(root1->children[1]->bv);
175
176 ✗ if (d2 < d1) {
177 ✗ if (d2 < min_dist) {
178 ✗ if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
179 callback, min_dist))
180 ✗ return true;
181 }
182
183 ✗ if (d1 < min_dist) {
184 ✗ if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
185 callback, min_dist))
186 ✗ return true;
187 }
188 } else {
189 ✗ if (d1 < min_dist) {
190 ✗ if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
191 callback, min_dist))
192 ✗ return true;
193 }
194
195 ✗ if (d2 < min_dist) {
196 ✗ if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
197 callback, min_dist))
198 ✗ return true;
199 }
200 }
201 } else {
202 ✗ for (unsigned int i = 0; i < 8; ++i) {
203 ✗ if (tree2->nodeChildExists(root2, i)) {
204 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
205 ✗ AABB child_bv;
206 ✗ computeChildBV(root2_bv, i, child_bv);
207
208 ✗ AABB aabb2;
209 ✗ convertBV(child_bv, tf2, aabb2);
210 ✗ Scalar d = root1->bv.distance(aabb2);
211
212 ✗ if (d < min_dist) {
213 ✗ if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback,
214 min_dist))
215 ✗ return true;
216 }
217 }
218 }
219 }
220
221 ✗ return false;
222 }
223
224 //==============================================================================
225 ✗ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
226 const OcTree* tree2, const OcTree::OcTreeNode* root2,
227 const AABB& root2_bv, const Transform3s& tf2,
228 CollisionCallBackBase* callback) {
229 ✗ if (tf2.rotation().isIdentity())
230 ✗ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
231 ✗ callback);
232 else // has rotation
233 ✗ return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, callback);
234 }
235
236 //==============================================================================
237 ✗ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
238 const OcTree* tree2, const OcTree::OcTreeNode* root2,
239 const AABB& root2_bv, const Transform3s& tf2,
240 DistanceCallBackBase* callback, Scalar& min_dist) {
241 ✗ if (tf2.rotation().isIdentity())
242 ✗ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
243 ✗ callback, min_dist);
244 else
245 ✗ return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, callback,
246 ✗ min_dist);
247 }
248
249 #endif
250
251 //==============================================================================
252 3900 bool leafCollide(CollisionObject* o1, CollisionObject* o2,
253 CollisionCallBackBase* callback) {
254 3900 if ((o1->getNodeType() == GEOM_HALFSPACE ||
255
3/6
✓ Branch 0 taken 3900 times.
✗ Branch 1 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3900 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 3900 times.
 3900 o1->getNodeType() == GEOM_PLANE) &&
256 ✗ (o2->getNodeType() == GEOM_HALFSPACE ||
257 ✗ o2->getNodeType() == GEOM_PLANE)) {
258 // Halfspace-plane / Halfspace-plane collision, there is no need to check
259 // AABBs. We can directly use the callback.
260 ✗ return (*callback)(o1, o2);
261 }
262
263 3900 bool overlap = false;
264
3/6
✓ Branch 1 taken 3900 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 3900 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 3900 times.
 3900 if (o1->getNodeType() == GEOM_HALFSPACE || o1->getNodeType() == GEOM_PLANE) {
265 ✗ if (o1->getNodeType() == GEOM_HALFSPACE) {
266 const auto& halfspace =
267 ✗ static_cast<const Halfspace&>(*(o1->collisionGeometryPtr()));
268 ✗ overlap = o2->getAABB().overlap(transform(halfspace, o1->getTransform()));
269 } else {
270 const auto& plane =
271 ✗ static_cast<const Plane&>(*(o1->collisionGeometryPtr()));
272 ✗ overlap = o2->getAABB().overlap(transform(plane, o1->getTransform()));
273 }
274 } //
275
2/4
✓ Branch 1 taken 3900 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3900 times.
 7800 else if (o2->getNodeType() == GEOM_HALFSPACE ||
276
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3900 times.
 3900 o2->getNodeType() == GEOM_PLANE) {
277 ✗ if (o2->getNodeType() == GEOM_HALFSPACE) {
278 const auto& halfspace =
279 ✗ static_cast<const Halfspace&>(*(o2->collisionGeometryPtr()));
280 ✗ overlap = o1->getAABB().overlap(transform(halfspace, o2->getTransform()));
281 } else {
282 const auto& plane =
283 ✗ static_cast<const Plane&>(*(o2->collisionGeometryPtr()));
284 ✗ overlap = o1->getAABB().overlap(transform(plane, o2->getTransform()));
285 }
286 } //
287 else {
288 3900 overlap = o1->getAABB().overlap(o2->getAABB());
289 }
290
291
2/2
✓ Branch 0 taken 53 times.
✓ Branch 1 taken 3847 times.
 3900 if (overlap) {
292 53 return (*callback)(o1, o2);
293 }
294 3847 return false;
295 }
296
297 //==============================================================================
298 73155 bool nodeCollide(DynamicAABBTreeCollisionManager::DynamicAABBNode* node1,
299 DynamicAABBTreeCollisionManager::DynamicAABBNode* node2) {
300 // This function assumes that at least node1 or node2 is not a leaf of the
301 // tree.
302
2/2
✓ Branch 1 taken 693 times.
✓ Branch 2 taken 72462 times.
 73155 if (node1->isLeaf()) {
303 693 CollisionObject* o1 = static_cast<CollisionObject*>(node1->data);
304
2/4
✓ Branch 1 taken 693 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 693 times.
 1386 if (o1->getNodeType() == GEOM_HALFSPACE ||
305
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 693 times.
 693 o1->getNodeType() == GEOM_PLANE) {
306 ✗ if (o1->getNodeType() == GEOM_HALFSPACE) {
307 const auto& halfspace =
308 ✗ static_cast<const Halfspace&>(*(o1->collisionGeometryPtr()));
309 ✗ return node2->bv.overlap(transform(halfspace, o1->getTransform()));
310 }
311 const auto& plane =
312 ✗ static_cast<const Plane&>(*(o1->collisionGeometryPtr()));
313 ✗ return node2->bv.overlap(transform(plane, o1->getTransform()));
314 }
315 }
316
317
2/2
✓ Branch 1 taken 68873 times.
✓ Branch 2 taken 4282 times.
 73155 if (node2->isLeaf()) {
318 68873 CollisionObject* o2 = static_cast<CollisionObject*>(node2->data);
319
2/4
✓ Branch 1 taken 68873 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 68873 times.
 137746 if (o2->getNodeType() == GEOM_HALFSPACE ||
320
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 68873 times.
 68873 o2->getNodeType() == GEOM_PLANE) {
321 ✗ if (o2->getNodeType() == GEOM_HALFSPACE) {
322 const auto& halfspace =
323 ✗ static_cast<const Halfspace&>(*(o2->collisionGeometryPtr()));
324 ✗ return node1->bv.overlap(transform(halfspace, o2->getTransform()));
325 }
326 const auto& plane =
327 ✗ static_cast<const Plane&>(*(o2->collisionGeometryPtr()));
328 ✗ return node1->bv.overlap(transform(plane, o2->getTransform()));
329 }
330 }
331
332 73155 return node1->bv.overlap(node2->bv);
333 }
334
335 //==============================================================================
336 6437 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
337 DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
338 CollisionCallBackBase* callback) {
339
6/6
✓ Branch 1 taken 2369 times.
✓ Branch 2 taken 4068 times.
✓ Branch 4 taken 1676 times.
✓ Branch 5 taken 693 times.
✓ Branch 6 taken 1676 times.
✓ Branch 7 taken 4761 times.
 6437 if (root1->isLeaf() && root2->isLeaf()) {
340 1676 CollisionObject* o1 = static_cast<CollisionObject*>(root1->data);
341 1676 CollisionObject* o2 = static_cast<CollisionObject*>(root2->data);
342 1676 return leafCollide(o1, o2, callback);
343 }
344
345
2/2
✓ Branch 1 taken 3537 times.
✓ Branch 2 taken 1224 times.
 4761 if (!nodeCollide(root1, root2)) {
346 3537 return false;
347 }
348
349
4/4
✓ Branch 1 taken 1190 times.
✓ Branch 2 taken 34 times.
✓ Branch 3 taken 595 times.
✓ Branch 4 taken 629 times.
 2414 if (root2->isLeaf() ||
350
4/4
✓ Branch 1 taken 1167 times.
✓ Branch 2 taken 23 times.
✓ Branch 5 taken 561 times.
✓ Branch 6 taken 606 times.
 1190 (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
351
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 593 times.
 595 if (collisionRecurse(root1->children[0], root2, callback)) return true;
352
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 592 times.
 593 if (collisionRecurse(root1->children[1], root2, callback)) return true;
353 } else {
354
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 628 times.
 629 if (collisionRecurse(root1, root2->children[0], callback)) return true;
355
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 628 times.
 628 if (collisionRecurse(root1, root2->children[1], callback)) return true;
356 }
357 1220 return false;
358 }
359
360 //==============================================================================
361 70618 bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
362 CollisionObject* query, CollisionCallBackBase* callback) {
363
2/2
✓ Branch 1 taken 2224 times.
✓ Branch 2 taken 68394 times.
 70618 if (root->isLeaf()) {
364 2224 CollisionObject* leaf = static_cast<CollisionObject*>(root->data);
365
1/2
✓ Branch 1 taken 2224 times.
✗ Branch 2 not taken.
 2224 return leafCollide(leaf, query, callback);
366 }
367
368 // Create a temporary node, attached to no tree.
369 // This allows to reuse the `nodeCollide` function, which only checks for
370 // overlap between the AABBs of two nodes.
371
1/2
✓ Branch 1 taken 68394 times.
✗ Branch 2 not taken.
 68394 DynamicAABBTreeCollisionManager::DynamicAABBNode query_node;
372 68394 query_node.data = query;
373
1/2
✓ Branch 2 taken 68394 times.
✗ Branch 3 not taken.
 68394 query_node.bv = query->getAABB();
374 68394 query_node.parent = nullptr;
375 68394 query_node.children[1] = nullptr;
376
3/4
✓ Branch 1 taken 68394 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 36839 times.
✓ Branch 4 taken 31555 times.
 68394 if (!nodeCollide(root, &query_node)) {
377 36839 return false;
378 }
379
380 size_t select_res =
381
1/2
✓ Branch 2 taken 31555 times.
✗ Branch 3 not taken.
 31555 select(query->getAABB(), *(root->children[0]), *(root->children[1]));
382
383
3/4
✓ Branch 1 taken 31555 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
✓ Branch 4 taken 31538 times.
 31555 if (collisionRecurse(root->children[select_res], query, callback))
384 17 return true;
385
386
3/4
✓ Branch 1 taken 31538 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 31537 times.
 31538 if (collisionRecurse(root->children[1 - select_res], query, callback))
387 1 return true;
388
389 31537 return false;
390 }
391
392 //==============================================================================
393 8098 bool selfCollisionRecurse(
394 DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
395 CollisionCallBackBase* callback) {
396
2/2
✓ Branch 1 taken 4070 times.
✓ Branch 2 taken 4028 times.
 8098 if (root->isLeaf()) return false;
397
398
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 4012 times.
 4028 if (selfCollisionRecurse(root->children[0], callback)) return true;
399
400
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 3992 times.
 4012 if (selfCollisionRecurse(root->children[1], callback)) return true;
401
402
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3986 times.
 3992 if (collisionRecurse(root->children[0], root->children[1], callback))
403 6 return true;
404
405 3986 return false;
406 }
407
408 //==============================================================================
409 18584 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
410 DynamicAABBTreeCollisionManager::DynamicAABBNode* root2,
411 DistanceCallBackBase* callback, Scalar& min_dist) {
412
6/6
✓ Branch 1 taken 4025 times.
✓ Branch 2 taken 14559 times.
✓ Branch 4 taken 2681 times.
✓ Branch 5 taken 1344 times.
✓ Branch 6 taken 2681 times.
✓ Branch 7 taken 15903 times.
 18584 if (root1->isLeaf() && root2->isLeaf()) {
413 2681 CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
414 2681 CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
415 2681 return (*callback)(root1_obj, root2_obj, min_dist);
416 }
417
418
4/4
✓ Branch 1 taken 13584 times.
✓ Branch 2 taken 2319 times.
✓ Branch 3 taken 7790 times.
✓ Branch 4 taken 8113 times.
 29487 if (root2->isLeaf() ||
419
4/4
✓ Branch 1 taken 12240 times.
✓ Branch 2 taken 1344 times.
✓ Branch 5 taken 5471 times.
✓ Branch 6 taken 6769 times.
 13584 (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
420 7790 Scalar d1 = root2->bv.distance(root1->children[0]->bv);
421 7790 Scalar d2 = root2->bv.distance(root1->children[1]->bv);
422
423
2/2
✓ Branch 0 taken 5112 times.
✓ Branch 1 taken 2678 times.
 7790 if (d2 < d1) {
424
2/2
✓ Branch 0 taken 3034 times.
✓ Branch 1 taken 2078 times.
 5112 if (d2 < min_dist) {
425
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3034 times.
 3034 if (distanceRecurse(root1->children[1], root2, callback, min_dist))
426 ✗ return true;
427 }
428
429
2/2
✓ Branch 0 taken 724 times.
✓ Branch 1 taken 4388 times.
 5112 if (d1 < min_dist) {
430
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 724 times.
 724 if (distanceRecurse(root1->children[0], root2, callback, min_dist))
431 ✗ return true;
432 }
433 } else {
434
2/2
✓ Branch 0 taken 2066 times.
✓ Branch 1 taken 612 times.
 2678 if (d1 < min_dist) {
435
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2066 times.
 2066 if (distanceRecurse(root1->children[0], root2, callback, min_dist))
436 ✗ return true;
437 }
438
439
2/2
✓ Branch 0 taken 199 times.
✓ Branch 1 taken 2479 times.
 2678 if (d2 < min_dist) {
440
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 199 times.
 199 if (distanceRecurse(root1->children[1], root2, callback, min_dist))
441 ✗ return true;
442 }
443 }
444 } else {
445 8113 Scalar d1 = root1->bv.distance(root2->children[0]->bv);
446 8113 Scalar d2 = root1->bv.distance(root2->children[1]->bv);
447
448
2/2
✓ Branch 0 taken 3056 times.
✓ Branch 1 taken 5057 times.
 8113 if (d2 < d1) {
449
2/2
✓ Branch 0 taken 2585 times.
✓ Branch 1 taken 471 times.
 3056 if (d2 < min_dist) {
450
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2585 times.
 2585 if (distanceRecurse(root1, root2->children[1], callback, min_dist))
451 ✗ return true;
452 }
453
454
2/2
✓ Branch 0 taken 134 times.
✓ Branch 1 taken 2922 times.
 3056 if (d1 < min_dist) {
455
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 134 times.
 134 if (distanceRecurse(root1, root2->children[0], callback, min_dist))
456 ✗ return true;
457 }
458 } else {
459
2/2
✓ Branch 0 taken 3016 times.
✓ Branch 1 taken 2041 times.
 5057 if (d1 < min_dist) {
460
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3016 times.
 3016 if (distanceRecurse(root1, root2->children[0], callback, min_dist))
461 ✗ return true;
462 }
463
464
2/2
✓ Branch 0 taken 850 times.
✓ Branch 1 taken 4207 times.
 5057 if (d2 < min_dist) {
465
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 850 times.
 850 if (distanceRecurse(root1, root2->children[1], callback, min_dist))
466 ✗ return true;
467 }
468 }
469 }
470
471 15903 return false;
472 }
473
474 //==============================================================================
475 27019 bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
476 CollisionObject* query, DistanceCallBackBase* callback,
477 Scalar& min_dist) {
478
2/2
✓ Branch 1 taken 9225 times.
✓ Branch 2 taken 17794 times.
 27019 if (root->isLeaf()) {
479 9225 CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
480 9225 return (*callback)(root_obj, query, min_dist);
481 }
482
483 17794 Scalar d1 = query->getAABB().distance(root->children[0]->bv);
484 17794 Scalar d2 = query->getAABB().distance(root->children[1]->bv);
485
486
2/2
✓ Branch 0 taken 8905 times.
✓ Branch 1 taken 8889 times.
 17794 if (d2 < d1) {
487
2/2
✓ Branch 0 taken 8601 times.
✓ Branch 1 taken 304 times.
 8905 if (d2 < min_dist) {
488
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8601 times.
 8601 if (distanceRecurse(root->children[1], query, callback, min_dist))
489 ✗ return true;
490 }
491
492
2/2
✓ Branch 0 taken 4591 times.
✓ Branch 1 taken 4314 times.
 8905 if (d1 < min_dist) {
493
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4591 times.
 4591 if (distanceRecurse(root->children[0], query, callback, min_dist))
494 ✗ return true;
495 }
496 } else {
497
2/2
✓ Branch 0 taken 8611 times.
✓ Branch 1 taken 278 times.
 8889 if (d1 < min_dist) {
498
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8611 times.
 8611 if (distanceRecurse(root->children[0], query, callback, min_dist))
499 ✗ return true;
500 }
501
502
2/2
✓ Branch 0 taken 5056 times.
✓ Branch 1 taken 3833 times.
 8889 if (d2 < min_dist) {
503
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5056 times.
 5056 if (distanceRecurse(root->children[1], query, callback, min_dist))
504 ✗ return true;
505 }
506 }
507
508 17794 return false;
509 }
510
511 //==============================================================================
512 11972 bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root,
513 DistanceCallBackBase* callback, Scalar& min_dist) {
514
2/2
✓ Branch 1 taken 5996 times.
✓ Branch 2 taken 5976 times.
 11972 if (root->isLeaf()) return false;
515
516
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5976 times.
 5976 if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true;
517
518
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5976 times.
 5976 if (selfDistanceRecurse(root->children[1], callback, min_dist)) return true;
519
520
2/2
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 5968 times.
 5976 if (distanceRecurse(root->children[0], root->children[1], callback, min_dist))
521 8 return true;
522
523 5968 return false;
524 }
525
526 } // namespace dynamic_AABB_tree
527
528 } // namespace detail
529
530 //==============================================================================
531 104 DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() {
532 104 tree_topdown_balance_threshold = &dtree.bu_threshold;
533 104 tree_topdown_level = &dtree.topdown_level;
534 104 max_tree_nonbalanced_level = 10;
535 104 tree_incremental_balance_pass = 10;
536 104 *tree_topdown_balance_threshold = 2;
537 104 *tree_topdown_level = 0;
538 104 tree_init_level = 0;
539 104 setup_ = false;
540
541 // from experiment, this is the optimal setting
542 104 octree_as_geometry_collide = true;
543 104 octree_as_geometry_distance = false;
544 104 }
545
546 //==============================================================================
547 102 void DynamicAABBTreeCollisionManager::registerObjects(
548 const std::vector<CollisionObject*>& other_objs) {
549
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 86 times.
 102 if (other_objs.empty()) return;
550
551
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 86 times.
 86 if (size() > 0) {
552 ✗ BroadPhaseCollisionManager::registerObjects(other_objs);
553 } else {
554
1/2
✓ Branch 3 taken 86 times.
✗ Branch 4 not taken.
 86 std::vector<DynamicAABBNode*> leaves(other_objs.size());
555
1/2
✓ Branch 2 taken 86 times.
✗ Branch 3 not taken.
 86 table.rehash(other_objs.size());
556
2/2
✓ Branch 1 taken 25342 times.
✓ Branch 2 taken 86 times.
 25428 for (size_t i = 0, size = other_objs.size(); i < size; ++i) {
557 DynamicAABBNode* node =
558
2/4
✓ Branch 1 taken 25342 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25342 times.
✗ Branch 5 not taken.
 25342 new DynamicAABBNode; // node will be managed by the dtree
559
1/2
✓ Branch 3 taken 25342 times.
✗ Branch 4 not taken.
 25342 node->bv = other_objs[i]->getAABB();
560 25342 node->parent = nullptr;
561 25342 node->children[1] = nullptr;
562 25342 node->data = other_objs[i];
563
1/2
✓ Branch 2 taken 25342 times.
✗ Branch 3 not taken.
 25342 table[other_objs[i]] = node;
564 25342 leaves[i] = node;
565 }
566
567
1/2
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
 86 dtree.init(leaves, tree_init_level);
568
569 86 setup_ = true;
570 86 }
571 }
572
573 //==============================================================================
574 4 void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) {
575 4 DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
576 4 table[obj] = node;
577 4 }
578
579 //==============================================================================
580 ✗ void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) {
581 ✗ DynamicAABBNode* node = table[obj];
582 ✗ table.erase(obj);
583 ✗ dtree.remove(node);
584 }
585
586 //==============================================================================
587 137 void DynamicAABBTreeCollisionManager::setup() {
588
2/2
✓ Branch 0 taken 51 times.
✓ Branch 1 taken 86 times.
 137 if (!setup_) {
589 51 size_t num = dtree.size();
590
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 35 times.
 51 if (num == 0) {
591 16 setup_ = true;
592 16 return;
593 }
594
595 35 size_t height = dtree.getMaxHeight();
596
597 35 if (((Scalar)height - std::log((Scalar)num) / std::log(2.0)) <
598
1/2
✓ Branch 0 taken 35 times.
✗ Branch 1 not taken.
 35 max_tree_nonbalanced_level)
599 35 dtree.balanceIncremental(tree_incremental_balance_pass);
600 else
601 ✗ dtree.balanceTopdown();
602
603 35 setup_ = true;
604 }
605 }
606
607 //==============================================================================
608 34 void DynamicAABBTreeCollisionManager::update() {
609
2/2
✓ Branch 4 taken 2704 times.
✓ Branch 5 taken 34 times.
 2738 for (auto it = table.cbegin(); it != table.cend(); ++it) {
610 2704 CollisionObject* obj = it->first;
611 2704 DynamicAABBNode* node = it->second;
612
1/2
✓ Branch 2 taken 2704 times.
✗ Branch 3 not taken.
 2704 node->bv = obj->getAABB();
613
2/4
✓ Branch 1 taken 2704 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2704 times.
 2704 if (node->bv.volume() <= 0.)
614 ✗ COAL_THROW_PRETTY("The bounding volume has a negative volume.",
615 std::invalid_argument)
616 }
617
618 34 dtree.refit();
619 34 setup_ = false;
620
621 34 setup();
622 34 }
623
624 //==============================================================================
625 ✗ void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) {
626 ✗ const auto it = table.find(updated_obj);
627 ✗ if (it != table.end()) {
628 ✗ DynamicAABBNode* node = it->second;
629 ✗ if (!(node->bv == updated_obj->getAABB()))
630 ✗ dtree.update(node, updated_obj->getAABB());
631 }
632 ✗ setup_ = false;
633 }
634
635 //==============================================================================
636 ✗ void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) {
637 ✗ update_(updated_obj);
638 ✗ setup();
639 }
640
641 //==============================================================================
642 ✗ void DynamicAABBTreeCollisionManager::update(
643 const std::vector<CollisionObject*>& updated_objs) {
644 ✗ for (size_t i = 0, size = updated_objs.size(); i < size; ++i)
645 ✗ update_(updated_objs[i]);
646 ✗ setup();
647 }
648
649 //==============================================================================
650 ✗ void DynamicAABBTreeCollisionManager::clear() {
651 ✗ dtree.clear();
652 ✗ table.clear();
653 }
654
655 //==============================================================================
656 ✗ void DynamicAABBTreeCollisionManager::getObjects(
657 std::vector<CollisionObject*>& objs) const {
658 ✗ objs.resize(this->size());
659 ✗ std::transform(
660 table.begin(), table.end(), objs.begin(),
661 ✗ std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
662 }
663
664 //==============================================================================
665 7645 void DynamicAABBTreeCollisionManager::collide(
666 CollisionObject* obj, CollisionCallBackBase* callback) const {
667 COAL_TRACY_ZONE_SCOPED_N(
668 "coal::DynamicAABBTreeCollisionManager::collide(CollisionObject*, "
669 "CollisionCallBackBase*)");
670 7645 callback->init();
671
2/2
✓ Branch 1 taken 120 times.
✓ Branch 2 taken 7525 times.
 7645 if (size() == 0) return;
672
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7525 times.
 7525 switch (obj->collisionGeometry()->getNodeType()) {
673 #if COAL_HAVE_OCTOMAP
674 ✗ case GEOM_OCTREE: {
675 ✗ if (!octree_as_geometry_collide) {
676 const OcTree* octree =
677 ✗ static_cast<const OcTree*>(obj->collisionGeometryPtr());
678 ✗ detail::dynamic_AABB_tree::collisionRecurse(
679 ✗ dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(),
680 obj->getTransform(), callback);
681 } else
682 ✗ detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj,
683 callback);
684 ✗ } break;
685 #endif
686 7525 default:
687 7525 detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj,
688 callback);
689 }
690 }
691
692 //==============================================================================
693 160 void DynamicAABBTreeCollisionManager::distance(
694 CollisionObject* obj, DistanceCallBackBase* callback) const {
695 COAL_TRACY_ZONE_SCOPED_N(
696 "coal::DynamicAABBTreeCollisionManager::distance(CollisionObject*, "
697 "DistanceCallBackBase*)");
698
1/2
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
 160 callback->init();
699
2/4
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 160 times.
 160 if (size() == 0) return;
700 160 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
701
2/4
✓ Branch 3 taken 160 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 160 times.
 160 switch (obj->collisionGeometry()->getNodeType()) {
702 #if COAL_HAVE_OCTOMAP
703 ✗ case GEOM_OCTREE: {
704 ✗ if (!octree_as_geometry_distance) {
705 const OcTree* octree =
706 ✗ static_cast<const OcTree*>(obj->collisionGeometryPtr());
707 ✗ detail::dynamic_AABB_tree::distanceRecurse(
708 ✗ dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(),
709 obj->getTransform(), callback, min_dist);
710 } else
711 ✗ detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj,
712 callback, min_dist);
713 ✗ } break;
714 #endif
715 160 default:
716
1/2
✓ Branch 2 taken 160 times.
✗ Branch 3 not taken.
 160 detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback,
717 min_dist);
718 }
719 }
720
721 //==============================================================================
722 74 void DynamicAABBTreeCollisionManager::collide(
723 CollisionCallBackBase* callback) const {
724 COAL_TRACY_ZONE_SCOPED_N(
725 "coal::DynamicAABBTreeCollisionManager::collide(CollisionCallBackBase*)");
726 74 callback->init();
727
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 58 times.
 74 if (size() == 0) return;
728 58 detail::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), callback);
729 }
730
731 //==============================================================================
732 20 void DynamicAABBTreeCollisionManager::distance(
733 DistanceCallBackBase* callback) const {
734 COAL_TRACY_ZONE_SCOPED_N(
735 "coal::DynamicAABBTreeCollisionManager::distance(DistanceCallBackBase*)");
736
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 callback->init();
737
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 20 times.
 20 if (size() == 0) return;
738 20 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
739
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback,
740 min_dist);
741 }
742
743 //==============================================================================
744 ✗ void DynamicAABBTreeCollisionManager::collide(
745 BroadPhaseCollisionManager* other_manager_,
746 CollisionCallBackBase* callback) const {
747 COAL_TRACY_ZONE_SCOPED_N(
748 "coal::DynamicAABBTreeCollisionManager::collide("
749 "BroadPhaseCollisionManager*, CollisionCallBackBase*)");
750 ✗ callback->init();
751 ✗ DynamicAABBTreeCollisionManager* other_manager =
752 static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
753 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
754 ✗ detail::dynamic_AABB_tree::collisionRecurse(
755 ✗ dtree.getRoot(), other_manager->dtree.getRoot(), callback);
756 }
757
758 //==============================================================================
759 ✗ void DynamicAABBTreeCollisionManager::distance(
760 BroadPhaseCollisionManager* other_manager_,
761 DistanceCallBackBase* callback) const {
762 COAL_TRACY_ZONE_SCOPED_N(
763 "coal::DynamicAABBTreeCollisionManager::distance("
764 "BroadPhaseCollisionManager*, DistanceCallBackBase*)");
765 ✗ callback->init();
766 ✗ DynamicAABBTreeCollisionManager* other_manager =
767 static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
768 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
769 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
770 ✗ detail::dynamic_AABB_tree::distanceRecurse(
771 ✗ dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist);
772 }
773
774 //==============================================================================
775 ✗ bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); }
776
777 //==============================================================================
778 7986 size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); }
779
780 //==============================================================================
781 ✗ const detail::HierarchyTree<AABB>& DynamicAABBTreeCollisionManager::getTree()
782 const {
783 ✗ return dtree;
784 }
785
786 ✗ detail::HierarchyTree<AABB>& DynamicAABBTreeCollisionManager::getTree() {
787 ✗ return dtree;
788 }
789
790 } // namespace coal
791