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