GCC Code Coverage Report

Directory: ./
File: src/broadphase/broadphase_dynamic_AABB_tree_array.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 168 363 46.3%
Branches: 137 466 29.4%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-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_array.h"
39
40 #ifdef COAL_HAVE_OCTOMAP
41 #include "coal/octree.h"
42 #endif
43 namespace coal {
44 namespace detail {
45 namespace dynamic_AABB_tree_array {
46
47 #if COAL_HAVE_OCTOMAP
48
49 //==============================================================================
50 ✗ bool collisionRecurse_(
51 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
52 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
53 const AABB& root2_bv, const Transform3s& tf2,
54 CollisionCallBackBase* callback) {
55 ✗ DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
56 ✗ nodes1 + root1_id;
57 ✗ if (!root2) {
58 ✗ if (root1->isLeaf()) {
59 ✗ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
60
61 ✗ if (!obj1->collisionGeometry()->isFree()) {
62 ✗ OBB obb1, obb2;
63 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
64 ✗ convertBV(root2_bv, tf2, obb2);
65
66 ✗ if (obb1.overlap(obb2)) {
67 ✗ Box* box = new Box();
68 ✗ Transform3s box_tf;
69 ✗ constructBox(root2_bv, tf2, *box, box_tf);
70
71 ✗ box->cost_density = tree2->getDefaultOccupancy();
72
73 ✗ CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
74 ✗ return (*callback)(obj1, &obj2);
75 }
76 }
77 } else {
78 ✗ if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
79 root2_bv, tf2, callback))
80 ✗ return true;
81 ✗ if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
82 root2_bv, tf2, callback))
83 ✗ return true;
84 }
85
86 ✗ return false;
87 ✗ } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
88 ✗ CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
89 ✗ if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
90 ✗ OBB obb1, obb2;
91 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
92 ✗ convertBV(root2_bv, tf2, obb2);
93
94 ✗ if (obb1.overlap(obb2)) {
95 ✗ Box* box = new Box();
96 ✗ Transform3s box_tf;
97 ✗ constructBox(root2_bv, tf2, *box, box_tf);
98
99 ✗ box->cost_density = Scalar(root2->getOccupancy());
100 ✗ box->threshold_occupied = tree2->getOccupancyThres();
101
102 ✗ CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
103 ✗ return (*callback)(obj1, &obj2);
104 ✗ } else
105 ✗ return false;
106 } else
107 ✗ return false;
108 }
109
110 ✗ OBB obb1, obb2;
111 ✗ convertBV(root1->bv, Transform3s::Identity(), obb1);
112 ✗ convertBV(root2_bv, tf2, obb2);
113
114 ✗ if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
115
116 ✗ if (!tree2->nodeHasChildren(root2) ||
117 ✗ (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
118 ✗ if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
119 tf2, callback))
120 ✗ return true;
121 ✗ if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
122 tf2, callback))
123 ✗ return true;
124 } else {
125 ✗ for (unsigned int i = 0; i < 8; ++i) {
126 ✗ if (tree2->nodeChildExists(root2, i)) {
127 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
128 ✗ AABB child_bv;
129 ✗ computeChildBV(root2_bv, i, child_bv);
130
131 ✗ if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
132 callback))
133 ✗ return true;
134 } else {
135 ✗ AABB child_bv;
136 ✗ computeChildBV(root2_bv, i, child_bv);
137 ✗ if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2,
138 callback))
139 ✗ return true;
140 }
141 }
142 }
143
144 ✗ return false;
145 }
146
147 //==============================================================================
148 ✗ bool distanceRecurse_(
149 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
150 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
151 const AABB& root2_bv, const Transform3s& tf2,
152 DistanceCallBackBase* callback, Scalar& min_dist) {
153 ✗ DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
154 ✗ nodes1 + root1_id;
155 ✗ if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
156 ✗ if (tree2->isNodeOccupied(root2)) {
157 ✗ Box* box = new Box();
158 ✗ Transform3s box_tf;
159 ✗ constructBox(root2_bv, tf2, *box, box_tf);
160 ✗ CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
161 ✗ return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
162 ✗ min_dist);
163 ✗ } else
164 ✗ return false;
165 }
166
167 ✗ if (!tree2->isNodeOccupied(root2)) return false;
168
169 ✗ if (!tree2->nodeHasChildren(root2) ||
170 ✗ (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
171 ✗ AABB aabb2;
172 ✗ convertBV(root2_bv, tf2, aabb2);
173
174 ✗ Scalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
175 ✗ Scalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
176
177 ✗ if (d2 < d1) {
178 ✗ if (d2 < min_dist) {
179 ✗ if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
180 tf2, callback, min_dist))
181 ✗ return true;
182 }
183
184 ✗ if (d1 < min_dist) {
185 ✗ if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
186 tf2, callback, min_dist))
187 ✗ return true;
188 }
189 } else {
190 ✗ if (d1 < min_dist) {
191 ✗ if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
192 tf2, callback, min_dist))
193 ✗ return true;
194 }
195
196 ✗ if (d2 < min_dist) {
197 ✗ if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
198 tf2, callback, min_dist))
199 ✗ return true;
200 }
201 }
202 } else {
203 ✗ for (unsigned int i = 0; i < 8; ++i) {
204 ✗ if (tree2->nodeChildExists(root2, i)) {
205 ✗ const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
206 ✗ AABB child_bv;
207 ✗ computeChildBV(root2_bv, i, child_bv);
208
209 ✗ AABB aabb2;
210 ✗ convertBV(child_bv, tf2, aabb2);
211 ✗ Scalar d = root1->bv.distance(aabb2);
212
213 ✗ if (d < min_dist) {
214 ✗ if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
215 callback, min_dist))
216 ✗ return true;
217 }
218 }
219 }
220 }
221
222 ✗ return false;
223 }
224
225 #endif
226
227 //==============================================================================
228 6437 bool collisionRecurse(
229 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
230 size_t root1_id,
231 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2,
232 size_t root2_id, CollisionCallBackBase* callback) {
233 6437 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
234 6437 nodes1 + root1_id;
235 6437 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 =
236 6437 nodes2 + root2_id;
237
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()) {
238
2/2
✓ Branch 1 taken 1662 times.
✓ Branch 2 taken 14 times.
 1676 if (!root1->bv.overlap(root2->bv)) return false;
239 28 return (*callback)(static_cast<CollisionObject*>(root1->data),
240 14 static_cast<CollisionObject*>(root2->data));
241 }
242
243
2/2
✓ Branch 1 taken 3537 times.
✓ Branch 2 taken 1224 times.
 4761 if (!root1->bv.overlap(root2->bv)) return false;
244
245
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() ||
246
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()))) {
247
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 593 times.
 595 if (collisionRecurse(nodes1, root1->children[0], nodes2, root2_id,
248 callback))
249 2 return true;
250
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 592 times.
 593 if (collisionRecurse(nodes1, root1->children[1], nodes2, root2_id,
251 callback))
252 1 return true;
253 } else {
254
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 628 times.
 629 if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[0],
255 callback))
256 1 return true;
257
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 628 times.
 628 if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[1],
258 callback))
259 ✗ return true;
260 }
261 1220 return false;
262 }
263
264 //==============================================================================
265 70616 inline COAL_DLLAPI bool collisionRecurse(
266 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
267 size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) {
268 70616 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
269
2/2
✓ Branch 2 taken 39024 times.
✓ Branch 3 taken 31592 times.
 70616 if (!root->bv.overlap(query->getAABB())) return false;
270
271
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 31554 times.
 31592 if (root->isLeaf()) {
272 38 return (*callback)(static_cast<CollisionObject*>(root->data), query);
273 }
274
275 31554 size_t select_res = implementation_array::select(
276 31554 query->getAABB(), root->children[0], root->children[1], nodes);
277
278
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 31538 times.
 31554 if (collisionRecurse(nodes, root->children[select_res], query, callback))
279 16 return true;
280
281
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 31537 times.
 31538 if (collisionRecurse(nodes, root->children[1 - select_res], query, callback))
282 1 return true;
283
284 31537 return false;
285 }
286
287 //==============================================================================
288 8098 bool selfCollisionRecurse(
289 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
290 size_t root_id, CollisionCallBackBase* callback) {
291 8098 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
292
2/2
✓ Branch 1 taken 4070 times.
✓ Branch 2 taken 4028 times.
 8098 if (root->isLeaf()) return false;
293
294
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 4012 times.
 4028 if (selfCollisionRecurse(nodes, root->children[0], callback)) return true;
295
296
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 3992 times.
 4012 if (selfCollisionRecurse(nodes, root->children[1], callback)) return true;
297
298
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 3986 times.
 3992 if (collisionRecurse(nodes, root->children[0], nodes, root->children[1],
299 callback))
300 6 return true;
301
302 3986 return false;
303 }
304
305 //==============================================================================
306 18576 bool distanceRecurse(
307 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
308 size_t root1_id,
309 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes2,
310 size_t root2_id, DistanceCallBackBase* callback, Scalar& min_dist) {
311 18576 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root1 =
312 18576 nodes1 + root1_id;
313 18576 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root2 =
314 18576 nodes2 + root2_id;
315
6/6
✓ Branch 1 taken 4017 times.
✓ Branch 2 taken 14559 times.
✓ Branch 4 taken 2673 times.
✓ Branch 5 taken 1344 times.
✓ Branch 6 taken 2673 times.
✓ Branch 7 taken 15903 times.
 18576 if (root1->isLeaf() && root2->isLeaf()) {
316 2673 CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
317 2673 CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
318 2673 return (*callback)(root1_obj, root2_obj, min_dist);
319 }
320
321
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() ||
322
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()))) {
323 7790 Scalar d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
324 7790 Scalar d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
325
326
2/2
✓ Branch 0 taken 5112 times.
✓ Branch 1 taken 2678 times.
 7790 if (d2 < d1) {
327
2/2
✓ Branch 0 taken 3034 times.
✓ Branch 1 taken 2078 times.
 5112 if (d2 < min_dist) {
328
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3034 times.
 3034 if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id,
329 callback, min_dist))
330 ✗ return true;
331 }
332
333
2/2
✓ Branch 0 taken 724 times.
✓ Branch 1 taken 4388 times.
 5112 if (d1 < min_dist) {
334
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 724 times.
 724 if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id,
335 callback, min_dist))
336 ✗ return true;
337 }
338 } else {
339
2/2
✓ Branch 0 taken 2066 times.
✓ Branch 1 taken 612 times.
 2678 if (d1 < min_dist) {
340
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2066 times.
 2066 if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id,
341 callback, min_dist))
342 ✗ return true;
343 }
344
345
2/2
✓ Branch 0 taken 199 times.
✓ Branch 1 taken 2479 times.
 2678 if (d2 < min_dist) {
346
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 199 times.
 199 if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id,
347 callback, min_dist))
348 ✗ return true;
349 }
350 }
351 } else {
352 8113 Scalar d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
353 8113 Scalar d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
354
355
2/2
✓ Branch 0 taken 3056 times.
✓ Branch 1 taken 5057 times.
 8113 if (d2 < d1) {
356
2/2
✓ Branch 0 taken 2585 times.
✓ Branch 1 taken 471 times.
 3056 if (d2 < min_dist) {
357
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2585 times.
 2585 if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1],
358 callback, min_dist))
359 ✗ return true;
360 }
361
362
2/2
✓ Branch 0 taken 134 times.
✓ Branch 1 taken 2922 times.
 3056 if (d1 < min_dist) {
363
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 134 times.
 134 if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0],
364 callback, min_dist))
365 ✗ return true;
366 }
367 } else {
368
2/2
✓ Branch 0 taken 3016 times.
✓ Branch 1 taken 2041 times.
 5057 if (d1 < min_dist) {
369
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3016 times.
 3016 if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0],
370 callback, min_dist))
371 ✗ return true;
372 }
373
374
2/2
✓ Branch 0 taken 850 times.
✓ Branch 1 taken 4207 times.
 5057 if (d2 < min_dist) {
375
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 850 times.
 850 if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1],
376 callback, min_dist))
377 ✗ return true;
378 }
379 }
380 }
381
382 15903 return false;
383 }
384
385 //==============================================================================
386 27019 bool distanceRecurse(
387 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
388 size_t root_id, CollisionObject* query, DistanceCallBackBase* callback,
389 Scalar& min_dist) {
390 27019 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
391
2/2
✓ Branch 1 taken 9225 times.
✓ Branch 2 taken 17794 times.
 27019 if (root->isLeaf()) {
392 9225 CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
393 9225 return (*callback)(root_obj, query, min_dist);
394 }
395
396 17794 Scalar d1 = query->getAABB().distance((nodes + root->children[0])->bv);
397 17794 Scalar d2 = query->getAABB().distance((nodes + root->children[1])->bv);
398
399
2/2
✓ Branch 0 taken 8905 times.
✓ Branch 1 taken 8889 times.
 17794 if (d2 < d1) {
400
2/2
✓ Branch 0 taken 8601 times.
✓ Branch 1 taken 304 times.
 8905 if (d2 < min_dist) {
401
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8601 times.
 8601 if (distanceRecurse(nodes, root->children[1], query, callback, min_dist))
402 ✗ return true;
403 }
404
405
2/2
✓ Branch 0 taken 4591 times.
✓ Branch 1 taken 4314 times.
 8905 if (d1 < min_dist) {
406
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4591 times.
 4591 if (distanceRecurse(nodes, root->children[0], query, callback, min_dist))
407 ✗ return true;
408 }
409 } else {
410
2/2
✓ Branch 0 taken 8611 times.
✓ Branch 1 taken 278 times.
 8889 if (d1 < min_dist) {
411
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 8611 times.
 8611 if (distanceRecurse(nodes, root->children[0], query, callback, min_dist))
412 ✗ return true;
413 }
414
415
2/2
✓ Branch 0 taken 5056 times.
✓ Branch 1 taken 3833 times.
 8889 if (d2 < min_dist) {
416
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5056 times.
 5056 if (distanceRecurse(nodes, root->children[1], query, callback, min_dist))
417 ✗ return true;
418 }
419 }
420
421 17794 return false;
422 }
423
424 //==============================================================================
425 11948 bool selfDistanceRecurse(
426 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes,
427 size_t root_id, DistanceCallBackBase* callback, Scalar& min_dist) {
428 11948 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* root = nodes + root_id;
429
2/2
✓ Branch 1 taken 5980 times.
✓ Branch 2 taken 5968 times.
 11948 if (root->isLeaf()) return false;
430
431
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
 5968 if (selfDistanceRecurse(nodes, root->children[0], callback, min_dist))
432 ✗ return true;
433
434
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
 5968 if (selfDistanceRecurse(nodes, root->children[1], callback, min_dist))
435 ✗ return true;
436
437
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5968 times.
 5968 if (distanceRecurse(nodes, root->children[0], nodes, root->children[1],
438 callback, min_dist))
439 ✗ return true;
440
441 5968 return false;
442 }
443
444 #if COAL_HAVE_OCTOMAP
445
446 //==============================================================================
447 ✗ bool collisionRecurse(
448 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
449 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
450 const AABB& root2_bv, const Transform3s& tf2,
451 CollisionCallBackBase* callback) {
452 ✗ if (tf2.rotation().isIdentity())
453 ✗ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
454 ✗ tf2.translation(), callback);
455 else
456 ✗ return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
457 ✗ callback);
458 }
459
460 //==============================================================================
461 ✗ bool distanceRecurse(
462 DynamicAABBTreeArrayCollisionManager::DynamicAABBNode* nodes1,
463 size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
464 const AABB& root2_bv, const Transform3s& tf2,
465 DistanceCallBackBase* callback, Scalar& min_dist) {
466 ✗ if (tf2.rotation().isIdentity())
467 ✗ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
468 ✗ tf2.translation(), callback, min_dist);
469 else
470 ✗ return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
471 ✗ callback, min_dist);
472 }
473
474 #endif
475
476 } // namespace dynamic_AABB_tree_array
477
478 } // namespace detail
479
480 //==============================================================================
481
1/2
✓ Branch 2 taken 102 times.
✗ Branch 3 not taken.
 102 DynamicAABBTreeArrayCollisionManager::DynamicAABBTreeArrayCollisionManager() {
482 102 tree_topdown_balance_threshold = &dtree.bu_threshold;
483 102 tree_topdown_level = &dtree.topdown_level;
484 102 max_tree_nonbalanced_level = 10;
485 102 tree_incremental_balance_pass = 10;
486 102 *tree_topdown_balance_threshold = 2;
487 102 *tree_topdown_level = 0;
488 102 tree_init_level = 0;
489 102 setup_ = false;
490
491 // from experiment, this is the optimal setting
492 102 octree_as_geometry_collide = true;
493 102 octree_as_geometry_distance = false;
494 102 }
495
496 //==============================================================================
497 102 void DynamicAABBTreeArrayCollisionManager::registerObjects(
498 const std::vector<CollisionObject*>& other_objs) {
499
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 86 times.
 102 if (other_objs.empty()) return;
500
501
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 86 times.
 86 if (size() > 0) {
502 ✗ BroadPhaseCollisionManager::registerObjects(other_objs);
503 } else {
504
4/6
✓ Branch 1 taken 86 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 25342 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 25342 times.
✓ Branch 9 taken 86 times.
 25428 DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
505 86 table.rehash(other_objs.size());
506
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) {
507 25342 leaves[i].bv = other_objs[i]->getAABB();
508 25342 leaves[i].parent = dtree.NULL_NODE;
509 25342 leaves[i].children[1] = dtree.NULL_NODE;
510 25342 leaves[i].data = other_objs[i];
511 25342 table[other_objs[i]] = i;
512 }
513
514 86 int n_leaves = (int)other_objs.size();
515
516 86 dtree.init(leaves, n_leaves, tree_init_level);
517
518 86 setup_ = true;
519 }
520 }
521
522 //==============================================================================
523 ✗ void DynamicAABBTreeArrayCollisionManager::registerObject(
524 CollisionObject* obj) {
525 ✗ size_t node = dtree.insert(obj->getAABB(), obj);
526 ✗ table[obj] = node;
527 }
528
529 //==============================================================================
530 ✗ void DynamicAABBTreeArrayCollisionManager::unregisterObject(
531 CollisionObject* obj) {
532 ✗ size_t node = table[obj];
533 ✗ table.erase(obj);
534 ✗ dtree.remove(node);
535 }
536
537 //==============================================================================
538 128 void DynamicAABBTreeArrayCollisionManager::setup() {
539
2/2
✓ Branch 0 taken 42 times.
✓ Branch 1 taken 86 times.
 128 if (!setup_) {
540 42 int num = (int)dtree.size();
541
2/2
✓ Branch 0 taken 16 times.
✓ Branch 1 taken 26 times.
 42 if (num == 0) {
542 16 setup_ = true;
543 16 return;
544 }
545
546 26 int height = (int)dtree.getMaxHeight();
547
548 26 if ((Scalar)height - std::log((Scalar)num) / std::log(2.0) <
549
1/2
✓ Branch 0 taken 26 times.
✗ Branch 1 not taken.
 26 max_tree_nonbalanced_level)
550 26 dtree.balanceIncremental(tree_incremental_balance_pass);
551 else
552 ✗ dtree.balanceTopdown();
553
554 26 setup_ = true;
555 }
556 }
557
558 //==============================================================================
559 26 void DynamicAABBTreeArrayCollisionManager::update() {
560
2/2
✓ Branch 4 taken 2688 times.
✓ Branch 5 taken 26 times.
 2714 for (auto it = table.cbegin(), end = table.cend(); it != end; ++it) {
561 2688 const CollisionObject* obj = it->first;
562 2688 size_t node = it->second;
563
1/2
✓ Branch 3 taken 2688 times.
✗ Branch 4 not taken.
 2688 dtree.getNodes()[node].bv = obj->getAABB();
564 }
565
566 26 dtree.refit();
567 26 setup_ = false;
568
569 26 setup();
570 26 }
571
572 //==============================================================================
573 ✗ void DynamicAABBTreeArrayCollisionManager::update_(
574 CollisionObject* updated_obj) {
575 ✗ const auto it = table.find(updated_obj);
576 ✗ if (it != table.end()) {
577 ✗ size_t node = it->second;
578 ✗ if (!(dtree.getNodes()[node].bv == updated_obj->getAABB()))
579 ✗ dtree.update(node, updated_obj->getAABB());
580 }
581 ✗ setup_ = false;
582 }
583
584 //==============================================================================
585 ✗ void DynamicAABBTreeArrayCollisionManager::update(
586 CollisionObject* updated_obj) {
587 ✗ update_(updated_obj);
588 ✗ setup();
589 }
590
591 //==============================================================================
592 ✗ void DynamicAABBTreeArrayCollisionManager::update(
593 const std::vector<CollisionObject*>& updated_objs) {
594 ✗ for (size_t i = 0, size = updated_objs.size(); i < size; ++i)
595 ✗ update_(updated_objs[i]);
596 ✗ setup();
597 }
598
599 //==============================================================================
600 ✗ void DynamicAABBTreeArrayCollisionManager::clear() {
601 ✗ dtree.clear();
602 ✗ table.clear();
603 }
604
605 //==============================================================================
606 ✗ void DynamicAABBTreeArrayCollisionManager::getObjects(
607 std::vector<CollisionObject*>& objs) const {
608 ✗ objs.resize(this->size());
609 ✗ std::transform(
610 table.begin(), table.end(), objs.begin(),
611 ✗ std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
612 }
613
614 //==============================================================================
615 7644 void DynamicAABBTreeArrayCollisionManager::collide(
616 CollisionObject* obj, CollisionCallBackBase* callback) const {
617 7644 callback->init();
618
2/2
✓ Branch 1 taken 120 times.
✓ Branch 2 taken 7524 times.
 7644 if (size() == 0) return;
619
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7524 times.
 7524 switch (obj->collisionGeometry()->getNodeType()) {
620 #if COAL_HAVE_OCTOMAP
621 ✗ case GEOM_OCTREE: {
622 ✗ if (!octree_as_geometry_collide) {
623 const OcTree* octree =
624 ✗ static_cast<const OcTree*>(obj->collisionGeometryPtr());
625 ✗ detail::dynamic_AABB_tree_array::collisionRecurse(
626 ✗ dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(),
627 ✗ octree->getRootBV(), obj->getTransform(), callback);
628 } else
629 ✗ detail::dynamic_AABB_tree_array::collisionRecurse(
630 dtree.getNodes(), dtree.getRoot(), obj, callback);
631 ✗ } break;
632 #endif
633 7524 default:
634 7524 detail::dynamic_AABB_tree_array::collisionRecurse(
635 dtree.getNodes(), dtree.getRoot(), obj, callback);
636 }
637 }
638
639 //==============================================================================
640 160 void DynamicAABBTreeArrayCollisionManager::distance(
641 CollisionObject* obj, DistanceCallBackBase* callback) const {
642
1/2
✓ Branch 1 taken 160 times.
✗ Branch 2 not taken.
 160 callback->init();
643
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;
644 160 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
645
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()) {
646 #if COAL_HAVE_OCTOMAP
647 ✗ case GEOM_OCTREE: {
648 ✗ if (!octree_as_geometry_distance) {
649 const OcTree* octree =
650 ✗ static_cast<const OcTree*>(obj->collisionGeometryPtr());
651 ✗ detail::dynamic_AABB_tree_array::distanceRecurse(
652 ✗ dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(),
653 ✗ octree->getRootBV(), obj->getTransform(), callback, min_dist);
654 } else
655 ✗ detail::dynamic_AABB_tree_array::distanceRecurse(
656 dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist);
657 ✗ } break;
658 #endif
659 160 default:
660
1/2
✓ Branch 3 taken 160 times.
✗ Branch 4 not taken.
 160 detail::dynamic_AABB_tree_array::distanceRecurse(
661 dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist);
662 }
663 }
664
665 //==============================================================================
666 74 void DynamicAABBTreeArrayCollisionManager::collide(
667 CollisionCallBackBase* callback) const {
668 74 callback->init();
669
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 58 times.
 74 if (size() == 0) return;
670 58 detail::dynamic_AABB_tree_array::selfCollisionRecurse(
671 dtree.getNodes(), dtree.getRoot(), callback);
672 }
673
674 //==============================================================================
675 12 void DynamicAABBTreeArrayCollisionManager::distance(
676 DistanceCallBackBase* callback) const {
677
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 callback->init();
678
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 12 times.
 12 if (size() == 0) return;
679 12 Scalar min_dist = (std::numeric_limits<Scalar>::max)();
680
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 detail::dynamic_AABB_tree_array::selfDistanceRecurse(
681 dtree.getNodes(), dtree.getRoot(), callback, min_dist);
682 }
683
684 //==============================================================================
685 ✗ void DynamicAABBTreeArrayCollisionManager::collide(
686 BroadPhaseCollisionManager* other_manager_,
687 CollisionCallBackBase* callback) const {
688 ✗ callback->init();
689 ✗ DynamicAABBTreeArrayCollisionManager* other_manager =
690 static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
691 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
692 ✗ detail::dynamic_AABB_tree_array::collisionRecurse(
693 dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(),
694 other_manager->dtree.getRoot(), callback);
695 }
696
697 //==============================================================================
698 ✗ void DynamicAABBTreeArrayCollisionManager::distance(
699 BroadPhaseCollisionManager* other_manager_,
700 DistanceCallBackBase* callback) const {
701 ✗ callback->init();
702 ✗ DynamicAABBTreeArrayCollisionManager* other_manager =
703 static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
704 ✗ if ((size() == 0) || (other_manager->size() == 0)) return;
705 ✗ Scalar min_dist = (std::numeric_limits<Scalar>::max)();
706 ✗ detail::dynamic_AABB_tree_array::distanceRecurse(
707 dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(),
708 other_manager->dtree.getRoot(), callback, min_dist);
709 }
710
711 //==============================================================================
712 ✗ bool DynamicAABBTreeArrayCollisionManager::empty() const {
713 ✗ return dtree.empty();
714 }
715
716 //==============================================================================
717 7976 size_t DynamicAABBTreeArrayCollisionManager::size() const {
718 7976 return dtree.size();
719 }
720
721 //==============================================================================
722 const detail::implementation_array::HierarchyTree<AABB>&
723 ✗ DynamicAABBTreeArrayCollisionManager::getTree() const {
724 ✗ return dtree;
725 }
726
727 } // namespace coal
728