Directory: | ./ |
---|---|
File: | src/broadphase/broadphase_dynamic_AABB_tree_array.cpp |
Date: | 2025-04-01 09:23:31 |
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 |