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 "hpp/fcl/broadphase/broadphase_dynamic_AABB_tree.h" |
39 |
|
|
|
40 |
|
|
#include <limits> |
41 |
|
|
|
42 |
|
|
#if HPP_FCL_HAVE_OCTOMAP |
43 |
|
|
#include "hpp/fcl/octree.h" |
44 |
|
|
#endif |
45 |
|
|
|
46 |
|
|
#include "hpp/fcl/BV/BV.h" |
47 |
|
|
#include "hpp/fcl/shape/geometric_shapes_utility.h" |
48 |
|
|
|
49 |
|
|
namespace hpp { |
50 |
|
|
namespace fcl { |
51 |
|
|
namespace detail { |
52 |
|
|
|
53 |
|
|
namespace dynamic_AABB_tree { |
54 |
|
|
|
55 |
|
|
#if HPP_FCL_HAVE_OCTOMAP |
56 |
|
|
//============================================================================== |
57 |
|
|
bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, |
58 |
|
|
const OcTree* tree2, const OcTree::OcTreeNode* root2, |
59 |
|
|
const AABB& root2_bv, const Transform3f& 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, Transform3f::Identity(), obb1); |
68 |
|
|
convertBV(root2_bv, tf2, obb2); |
69 |
|
|
|
70 |
|
|
if (obb1.overlap(obb2)) { |
71 |
|
|
Box* box = new Box(); |
72 |
|
|
Transform3f 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, Transform3f::Identity(), obb1); |
97 |
|
|
convertBV(root2_bv, tf2, obb2); |
98 |
|
|
|
99 |
|
|
if (obb1.overlap(obb2)) { |
100 |
|
|
Box* box = new Box(); |
101 |
|
|
Transform3f box_tf; |
102 |
|
|
constructBox(root2_bv, tf2, *box, box_tf); |
103 |
|
|
|
104 |
|
|
box->cost_density = 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, Transform3f::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 Transform3f& tf2, |
153 |
|
|
DistanceCallBackBase* callback, FCL_REAL& min_dist) { |
154 |
|
|
if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) { |
155 |
|
|
if (tree2->isNodeOccupied(root2)) { |
156 |
|
|
Box* box = new Box(); |
157 |
|
|
Transform3f 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 |
|
|
FCL_REAL d1 = aabb2.distance(root1->children[0]->bv); |
174 |
|
|
FCL_REAL 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 |
|
|
FCL_REAL 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 Transform3f& 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 Transform3f& tf2, |
240 |
|
|
DistanceCallBackBase* callback, FCL_REAL& 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 |
|
11679 |
bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, |
253 |
|
|
DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, |
254 |
|
|
CollisionCallBackBase* callback) { |
255 |
✓✓✓✓ ✓✓ |
11679 |
if (root1->isLeaf() && root2->isLeaf()) { |
256 |
✓✓ |
2013 |
if (!root1->bv.overlap(root2->bv)) return false; |
257 |
|
12 |
return (*callback)(static_cast<CollisionObject*>(root1->data), |
258 |
|
12 |
static_cast<CollisionObject*>(root2->data)); |
259 |
|
|
} |
260 |
|
|
|
261 |
✓✓ |
9666 |
if (!root1->bv.overlap(root2->bv)) return false; |
262 |
|
|
|
263 |
✓✓✓✓
|
7026 |
if (root2->isLeaf() || |
264 |
✓✓✓✓
|
3468 |
(!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { |
265 |
✗✓ |
1766 |
if (collisionRecurse(root1->children[0], root2, callback)) return true; |
266 |
✗✓ |
1766 |
if (collisionRecurse(root1->children[1], root2, callback)) return true; |
267 |
|
|
} else { |
268 |
✗✓ |
1792 |
if (collisionRecurse(root1, root2->children[0], callback)) return true; |
269 |
✗✓ |
1792 |
if (collisionRecurse(root1, root2->children[1], callback)) return true; |
270 |
|
|
} |
271 |
|
3558 |
return false; |
272 |
|
|
} |
273 |
|
|
|
274 |
|
|
//============================================================================== |
275 |
|
74018 |
bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, |
276 |
|
|
CollisionObject* query, CollisionCallBackBase* callback) { |
277 |
✓✓ |
74018 |
if (root->isLeaf()) { |
278 |
✓✓ |
2341 |
if (!root->bv.overlap(query->getAABB())) return false; |
279 |
|
21 |
return (*callback)(static_cast<CollisionObject*>(root->data), query); |
280 |
|
|
} |
281 |
|
|
|
282 |
✓✓ |
71677 |
if (!root->bv.overlap(query->getAABB())) return false; |
283 |
|
|
|
284 |
|
|
size_t select_res = |
285 |
|
33247 |
select(query->getAABB(), *(root->children[0]), *(root->children[1])); |
286 |
|
|
|
287 |
✓✓ |
33247 |
if (collisionRecurse(root->children[select_res], query, callback)) |
288 |
|
1 |
return true; |
289 |
|
|
|
290 |
✗✓ |
33246 |
if (collisionRecurse(root->children[1 - select_res], query, callback)) |
291 |
|
|
return true; |
292 |
|
|
|
293 |
|
33246 |
return false; |
294 |
|
|
} |
295 |
|
|
|
296 |
|
|
//============================================================================== |
297 |
|
9213 |
bool selfCollisionRecurse( |
298 |
|
|
DynamicAABBTreeCollisionManager::DynamicAABBNode* root, |
299 |
|
|
CollisionCallBackBase* callback) { |
300 |
✓✓ |
9213 |
if (root->isLeaf()) return false; |
301 |
|
|
|
302 |
✓✓ |
4580 |
if (selfCollisionRecurse(root->children[0], callback)) return true; |
303 |
|
|
|
304 |
✓✓ |
4575 |
if (selfCollisionRecurse(root->children[1], callback)) return true; |
305 |
|
|
|
306 |
✓✓ |
4563 |
if (collisionRecurse(root->children[0], root->children[1], callback)) |
307 |
|
2 |
return true; |
308 |
|
|
|
309 |
|
4561 |
return false; |
310 |
|
|
} |
311 |
|
|
|
312 |
|
|
//============================================================================== |
313 |
|
19246 |
bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1, |
314 |
|
|
DynamicAABBTreeCollisionManager::DynamicAABBNode* root2, |
315 |
|
|
DistanceCallBackBase* callback, FCL_REAL& min_dist) { |
316 |
✓✓✓✓ ✓✓ |
19246 |
if (root1->isLeaf() && root2->isLeaf()) { |
317 |
|
3022 |
CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data); |
318 |
|
3022 |
CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data); |
319 |
|
3022 |
return (*callback)(root1_obj, root2_obj, min_dist); |
320 |
|
|
} |
321 |
|
|
|
322 |
✓✓✓✓
|
29952 |
if (root2->isLeaf() || |
323 |
✓✓✓✓
|
13728 |
(!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) { |
324 |
|
8006 |
FCL_REAL d1 = root2->bv.distance(root1->children[0]->bv); |
325 |
|
8006 |
FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv); |
326 |
|
|
|
327 |
✓✓ |
8006 |
if (d2 < d1) { |
328 |
✓✓ |
5178 |
if (d2 < min_dist) { |
329 |
✗✓ |
3218 |
if (distanceRecurse(root1->children[1], root2, callback, min_dist)) |
330 |
|
|
return true; |
331 |
|
|
} |
332 |
|
|
|
333 |
✓✓ |
5178 |
if (d1 < min_dist) { |
334 |
✗✓ |
724 |
if (distanceRecurse(root1->children[0], root2, callback, min_dist)) |
335 |
|
|
return true; |
336 |
|
|
} |
337 |
|
|
} else { |
338 |
✓✓ |
2828 |
if (d1 < min_dist) { |
339 |
✗✓ |
2224 |
if (distanceRecurse(root1->children[0], root2, callback, min_dist)) |
340 |
|
|
return true; |
341 |
|
|
} |
342 |
|
|
|
343 |
✓✓ |
2828 |
if (d2 < min_dist) { |
344 |
✗✓ |
222 |
if (distanceRecurse(root1->children[1], root2, callback, min_dist)) |
345 |
|
|
return true; |
346 |
|
|
} |
347 |
|
|
} |
348 |
|
|
} else { |
349 |
|
8218 |
FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv); |
350 |
|
8218 |
FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv); |
351 |
|
|
|
352 |
✓✓ |
8218 |
if (d2 < d1) { |
353 |
✓✓ |
3130 |
if (d2 < min_dist) { |
354 |
✗✓ |
2674 |
if (distanceRecurse(root1, root2->children[1], callback, min_dist)) |
355 |
|
|
return true; |
356 |
|
|
} |
357 |
|
|
|
358 |
✓✓ |
3130 |
if (d1 < min_dist) { |
359 |
✗✓ |
218 |
if (distanceRecurse(root1, root2->children[0], callback, min_dist)) |
360 |
|
|
return true; |
361 |
|
|
} |
362 |
|
|
} else { |
363 |
✓✓ |
5088 |
if (d1 < min_dist) { |
364 |
✗✓ |
3140 |
if (distanceRecurse(root1, root2->children[0], callback, min_dist)) |
365 |
|
|
return true; |
366 |
|
|
} |
367 |
|
|
|
368 |
✓✓ |
5088 |
if (d2 < min_dist) { |
369 |
✗✓ |
850 |
if (distanceRecurse(root1, root2->children[1], callback, min_dist)) |
370 |
|
|
return true; |
371 |
|
|
} |
372 |
|
|
} |
373 |
|
|
} |
374 |
|
|
|
375 |
|
16224 |
return false; |
376 |
|
|
} |
377 |
|
|
|
378 |
|
|
//============================================================================== |
379 |
|
5522 |
bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, |
380 |
|
|
CollisionObject* query, DistanceCallBackBase* callback, |
381 |
|
|
FCL_REAL& min_dist) { |
382 |
✓✓ |
5522 |
if (root->isLeaf()) { |
383 |
|
1250 |
CollisionObject* root_obj = static_cast<CollisionObject*>(root->data); |
384 |
|
1250 |
return (*callback)(root_obj, query, min_dist); |
385 |
|
|
} |
386 |
|
|
|
387 |
|
4272 |
FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv); |
388 |
|
4272 |
FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv); |
389 |
|
|
|
390 |
✓✓ |
4272 |
if (d2 < d1) { |
391 |
✓✓ |
2041 |
if (d2 < min_dist) { |
392 |
✗✓ |
1874 |
if (distanceRecurse(root->children[1], query, callback, min_dist)) |
393 |
|
|
return true; |
394 |
|
|
} |
395 |
|
|
|
396 |
✓✓ |
2041 |
if (d1 < min_dist) { |
397 |
✗✓ |
569 |
if (distanceRecurse(root->children[0], query, callback, min_dist)) |
398 |
|
|
return true; |
399 |
|
|
} |
400 |
|
|
} else { |
401 |
✓✓ |
2231 |
if (d1 < min_dist) { |
402 |
✗✓ |
2098 |
if (distanceRecurse(root->children[0], query, callback, min_dist)) |
403 |
|
|
return true; |
404 |
|
|
} |
405 |
|
|
|
406 |
✓✓ |
2231 |
if (d2 < min_dist) { |
407 |
✗✓ |
821 |
if (distanceRecurse(root->children[1], query, callback, min_dist)) |
408 |
|
|
return true; |
409 |
|
|
} |
410 |
|
|
} |
411 |
|
|
|
412 |
|
4272 |
return false; |
413 |
|
|
} |
414 |
|
|
|
415 |
|
|
//============================================================================== |
416 |
|
11972 |
bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root, |
417 |
|
|
DistanceCallBackBase* callback, FCL_REAL& min_dist) { |
418 |
✓✓ |
11972 |
if (root->isLeaf()) return false; |
419 |
|
|
|
420 |
✗✓ |
5976 |
if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true; |
421 |
|
|
|
422 |
✗✓ |
5976 |
if (selfDistanceRecurse(root->children[1], callback, min_dist)) return true; |
423 |
|
|
|
424 |
✓✓ |
5976 |
if (distanceRecurse(root->children[0], root->children[1], callback, min_dist)) |
425 |
|
8 |
return true; |
426 |
|
|
|
427 |
|
5968 |
return false; |
428 |
|
|
} |
429 |
|
|
|
430 |
|
|
} // namespace dynamic_AABB_tree |
431 |
|
|
|
432 |
|
|
} // namespace detail |
433 |
|
|
|
434 |
|
|
//============================================================================== |
435 |
|
104 |
DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager() { |
436 |
|
104 |
tree_topdown_balance_threshold = &dtree.bu_threshold; |
437 |
|
104 |
tree_topdown_level = &dtree.topdown_level; |
438 |
|
104 |
max_tree_nonbalanced_level = 10; |
439 |
|
104 |
tree_incremental_balance_pass = 10; |
440 |
|
104 |
*tree_topdown_balance_threshold = 2; |
441 |
|
104 |
*tree_topdown_level = 0; |
442 |
|
104 |
tree_init_level = 0; |
443 |
|
104 |
setup_ = false; |
444 |
|
|
|
445 |
|
|
// from experiment, this is the optimal setting |
446 |
|
104 |
octree_as_geometry_collide = true; |
447 |
|
104 |
octree_as_geometry_distance = false; |
448 |
|
104 |
} |
449 |
|
|
|
450 |
|
|
//============================================================================== |
451 |
|
102 |
void DynamicAABBTreeCollisionManager::registerObjects( |
452 |
|
|
const std::vector<CollisionObject*>& other_objs) { |
453 |
✓✓ |
102 |
if (other_objs.empty()) return; |
454 |
|
|
|
455 |
✗✓ |
86 |
if (size() > 0) { |
456 |
|
|
BroadPhaseCollisionManager::registerObjects(other_objs); |
457 |
|
|
} else { |
458 |
✓✗ |
86 |
std::vector<DynamicAABBNode*> leaves(other_objs.size()); |
459 |
✓✗ |
86 |
table.rehash(other_objs.size()); |
460 |
✓✓ |
25428 |
for (size_t i = 0, size = other_objs.size(); i < size; ++i) { |
461 |
|
|
DynamicAABBNode* node = |
462 |
✓✗✓✗
|
25342 |
new DynamicAABBNode; // node will be managed by the dtree |
463 |
✓✗ |
25342 |
node->bv = other_objs[i]->getAABB(); |
464 |
|
25342 |
node->parent = nullptr; |
465 |
|
25342 |
node->children[1] = nullptr; |
466 |
|
25342 |
node->data = other_objs[i]; |
467 |
✓✗ |
25342 |
table[other_objs[i]] = node; |
468 |
|
25342 |
leaves[i] = node; |
469 |
|
|
} |
470 |
|
|
|
471 |
✓✗ |
86 |
dtree.init(leaves, tree_init_level); |
472 |
|
|
|
473 |
|
86 |
setup_ = true; |
474 |
|
|
} |
475 |
|
|
} |
476 |
|
|
|
477 |
|
|
//============================================================================== |
478 |
|
4 |
void DynamicAABBTreeCollisionManager::registerObject(CollisionObject* obj) { |
479 |
|
4 |
DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj); |
480 |
|
4 |
table[obj] = node; |
481 |
|
4 |
} |
482 |
|
|
|
483 |
|
|
//============================================================================== |
484 |
|
|
void DynamicAABBTreeCollisionManager::unregisterObject(CollisionObject* obj) { |
485 |
|
|
DynamicAABBNode* node = table[obj]; |
486 |
|
|
table.erase(obj); |
487 |
|
|
dtree.remove(node); |
488 |
|
|
} |
489 |
|
|
|
490 |
|
|
//============================================================================== |
491 |
|
137 |
void DynamicAABBTreeCollisionManager::setup() { |
492 |
✓✓ |
137 |
if (!setup_) { |
493 |
|
51 |
size_t num = dtree.size(); |
494 |
✓✓ |
51 |
if (num == 0) { |
495 |
|
16 |
setup_ = true; |
496 |
|
16 |
return; |
497 |
|
|
} |
498 |
|
|
|
499 |
|
35 |
size_t height = dtree.getMaxHeight(); |
500 |
|
|
|
501 |
|
35 |
if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) < |
502 |
✓✗ |
35 |
max_tree_nonbalanced_level) |
503 |
|
35 |
dtree.balanceIncremental(tree_incremental_balance_pass); |
504 |
|
|
else |
505 |
|
|
dtree.balanceTopdown(); |
506 |
|
|
|
507 |
|
35 |
setup_ = true; |
508 |
|
|
} |
509 |
|
|
} |
510 |
|
|
|
511 |
|
|
//============================================================================== |
512 |
|
34 |
void DynamicAABBTreeCollisionManager::update() { |
513 |
✓✓ |
2738 |
for (auto it = table.cbegin(); it != table.cend(); ++it) { |
514 |
|
2704 |
CollisionObject* obj = it->first; |
515 |
|
2704 |
DynamicAABBNode* node = it->second; |
516 |
✓✗ |
2704 |
node->bv = obj->getAABB(); |
517 |
✓✗✗✓
|
2704 |
if (node->bv.volume() <= 0.) |
518 |
|
|
throw std::invalid_argument("The bounding volume has a negative volume."); |
519 |
|
|
} |
520 |
|
|
|
521 |
|
34 |
dtree.refit(); |
522 |
|
34 |
setup_ = false; |
523 |
|
|
|
524 |
|
34 |
setup(); |
525 |
|
34 |
} |
526 |
|
|
|
527 |
|
|
//============================================================================== |
528 |
|
|
void DynamicAABBTreeCollisionManager::update_(CollisionObject* updated_obj) { |
529 |
|
|
const auto it = table.find(updated_obj); |
530 |
|
|
if (it != table.end()) { |
531 |
|
|
DynamicAABBNode* node = it->second; |
532 |
|
|
if (!(node->bv == updated_obj->getAABB())) |
533 |
|
|
dtree.update(node, updated_obj->getAABB()); |
534 |
|
|
} |
535 |
|
|
setup_ = false; |
536 |
|
|
} |
537 |
|
|
|
538 |
|
|
//============================================================================== |
539 |
|
|
void DynamicAABBTreeCollisionManager::update(CollisionObject* updated_obj) { |
540 |
|
|
update_(updated_obj); |
541 |
|
|
setup(); |
542 |
|
|
} |
543 |
|
|
|
544 |
|
|
//============================================================================== |
545 |
|
|
void DynamicAABBTreeCollisionManager::update( |
546 |
|
|
const std::vector<CollisionObject*>& updated_objs) { |
547 |
|
|
for (size_t i = 0, size = updated_objs.size(); i < size; ++i) |
548 |
|
|
update_(updated_objs[i]); |
549 |
|
|
setup(); |
550 |
|
|
} |
551 |
|
|
|
552 |
|
|
//============================================================================== |
553 |
|
|
void DynamicAABBTreeCollisionManager::clear() { |
554 |
|
|
dtree.clear(); |
555 |
|
|
table.clear(); |
556 |
|
|
} |
557 |
|
|
|
558 |
|
|
//============================================================================== |
559 |
|
|
void DynamicAABBTreeCollisionManager::getObjects( |
560 |
|
|
std::vector<CollisionObject*>& objs) const { |
561 |
|
|
objs.resize(this->size()); |
562 |
|
|
std::transform( |
563 |
|
|
table.begin(), table.end(), objs.begin(), |
564 |
|
|
std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1)); |
565 |
|
|
} |
566 |
|
|
|
567 |
|
|
//============================================================================== |
568 |
|
7645 |
void DynamicAABBTreeCollisionManager::collide( |
569 |
|
|
CollisionObject* obj, CollisionCallBackBase* callback) const { |
570 |
|
7645 |
callback->init(); |
571 |
✓✓ |
7645 |
if (size() == 0) return; |
572 |
✗✓ |
7525 |
switch (obj->collisionGeometry()->getNodeType()) { |
573 |
|
|
#if HPP_FCL_HAVE_OCTOMAP |
574 |
|
|
case GEOM_OCTREE: { |
575 |
|
|
if (!octree_as_geometry_collide) { |
576 |
|
|
const OcTree* octree = |
577 |
|
|
static_cast<const OcTree*>(obj->collisionGeometryPtr()); |
578 |
|
|
detail::dynamic_AABB_tree::collisionRecurse( |
579 |
|
|
dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), |
580 |
|
|
obj->getTransform(), callback); |
581 |
|
|
} else |
582 |
|
|
detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, |
583 |
|
|
callback); |
584 |
|
|
} break; |
585 |
|
|
#endif |
586 |
|
7525 |
default: |
587 |
|
7525 |
detail::dynamic_AABB_tree::collisionRecurse(dtree.getRoot(), obj, |
588 |
|
|
callback); |
589 |
|
|
} |
590 |
|
|
} |
591 |
|
|
|
592 |
|
|
//============================================================================== |
593 |
|
160 |
void DynamicAABBTreeCollisionManager::distance( |
594 |
|
|
CollisionObject* obj, DistanceCallBackBase* callback) const { |
595 |
✓✗ |
160 |
callback->init(); |
596 |
✓✗✗✓
|
160 |
if (size() == 0) return; |
597 |
|
160 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
598 |
✓✗✗✓
|
160 |
switch (obj->collisionGeometry()->getNodeType()) { |
599 |
|
|
#if HPP_FCL_HAVE_OCTOMAP |
600 |
|
|
case GEOM_OCTREE: { |
601 |
|
|
if (!octree_as_geometry_distance) { |
602 |
|
|
const OcTree* octree = |
603 |
|
|
static_cast<const OcTree*>(obj->collisionGeometryPtr()); |
604 |
|
|
detail::dynamic_AABB_tree::distanceRecurse( |
605 |
|
|
dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), |
606 |
|
|
obj->getTransform(), callback, min_dist); |
607 |
|
|
} else |
608 |
|
|
detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, |
609 |
|
|
callback, min_dist); |
610 |
|
|
} break; |
611 |
|
|
#endif |
612 |
|
160 |
default: |
613 |
✓✗ |
160 |
detail::dynamic_AABB_tree::distanceRecurse(dtree.getRoot(), obj, callback, |
614 |
|
|
min_dist); |
615 |
|
|
} |
616 |
|
|
} |
617 |
|
|
|
618 |
|
|
//============================================================================== |
619 |
|
74 |
void DynamicAABBTreeCollisionManager::collide( |
620 |
|
|
CollisionCallBackBase* callback) const { |
621 |
|
74 |
callback->init(); |
622 |
✓✓ |
74 |
if (size() == 0) return; |
623 |
|
58 |
detail::dynamic_AABB_tree::selfCollisionRecurse(dtree.getRoot(), callback); |
624 |
|
|
} |
625 |
|
|
|
626 |
|
|
//============================================================================== |
627 |
|
20 |
void DynamicAABBTreeCollisionManager::distance( |
628 |
|
|
DistanceCallBackBase* callback) const { |
629 |
✓✗ |
20 |
callback->init(); |
630 |
✓✗✗✓
|
20 |
if (size() == 0) return; |
631 |
|
20 |
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
632 |
✓✗ |
20 |
detail::dynamic_AABB_tree::selfDistanceRecurse(dtree.getRoot(), callback, |
633 |
|
|
min_dist); |
634 |
|
|
} |
635 |
|
|
|
636 |
|
|
//============================================================================== |
637 |
|
|
void DynamicAABBTreeCollisionManager::collide( |
638 |
|
|
BroadPhaseCollisionManager* other_manager_, |
639 |
|
|
CollisionCallBackBase* callback) const { |
640 |
|
|
callback->init(); |
641 |
|
|
DynamicAABBTreeCollisionManager* other_manager = |
642 |
|
|
static_cast<DynamicAABBTreeCollisionManager*>(other_manager_); |
643 |
|
|
if ((size() == 0) || (other_manager->size() == 0)) return; |
644 |
|
|
detail::dynamic_AABB_tree::collisionRecurse( |
645 |
|
|
dtree.getRoot(), other_manager->dtree.getRoot(), callback); |
646 |
|
|
} |
647 |
|
|
|
648 |
|
|
//============================================================================== |
649 |
|
|
void DynamicAABBTreeCollisionManager::distance( |
650 |
|
|
BroadPhaseCollisionManager* other_manager_, |
651 |
|
|
DistanceCallBackBase* callback) const { |
652 |
|
|
callback->init(); |
653 |
|
|
DynamicAABBTreeCollisionManager* other_manager = |
654 |
|
|
static_cast<DynamicAABBTreeCollisionManager*>(other_manager_); |
655 |
|
|
if ((size() == 0) || (other_manager->size() == 0)) return; |
656 |
|
|
FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)(); |
657 |
|
|
detail::dynamic_AABB_tree::distanceRecurse( |
658 |
|
|
dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist); |
659 |
|
|
} |
660 |
|
|
|
661 |
|
|
//============================================================================== |
662 |
|
|
bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); } |
663 |
|
|
|
664 |
|
|
//============================================================================== |
665 |
|
7986 |
size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); } |
666 |
|
|
|
667 |
|
|
//============================================================================== |
668 |
|
|
const detail::HierarchyTree<AABB>& DynamicAABBTreeCollisionManager::getTree() |
669 |
|
|
const { |
670 |
|
|
return dtree; |
671 |
|
|
} |
672 |
|
|
|
673 |
|
|
detail::HierarchyTree<AABB>& DynamicAABBTreeCollisionManager::getTree() { |
674 |
|
|
return dtree; |
675 |
|
|
} |
676 |
|
|
|
677 |
|
|
} // namespace fcl |
678 |
|
|
} // namespace hpp |