broadphase_dynamic_AABB_tree_array-inl.h
Go to the documentation of this file.
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 
38 #ifndef FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
39 #define FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H
40 
42 
43 #if FCL_HAVE_OCTOMAP
45 #endif
46 
47 namespace fcl
48 {
49 
50 //==============================================================================
51 extern template
53 
54 namespace detail
55 {
56 
57 namespace dynamic_AABB_tree_array
58 {
59 
60 #if FCL_HAVE_OCTOMAP
61 
62 //==============================================================================
63 template <typename S>
64 FCL_EXPORT
65 bool collisionRecurse_(
67  size_t root1_id,
68  const OcTree<S>* tree2,
69  const typename OcTree<S>::OcTreeNode* root2,
70  const AABB<S>& root2_bv,
71  const Transform3<S>& tf2,
72  void* cdata,
73  CollisionCallBack<S> callback)
74 {
75  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
76  if(!root2)
77  {
78  if(root1->isLeaf())
79  {
80  CollisionObject<S>* obj1 = static_cast<CollisionObject<S>*>(root1->data);
81 
82  if(!obj1->isFree())
83  {
84  OBB<S> obb1, obb2;
85  convertBV(root1->bv, Transform3<S>::Identity(), obb1);
86  convertBV(root2_bv, tf2, obb2);
87 
88  if(obb1.overlap(obb2))
89  {
90  Box<S>* box = new Box<S>();
91  Transform3<S> box_tf;
92  constructBox(root2_bv, tf2, *box, box_tf);
93 
94  box->cost_density = tree2->getDefaultOccupancy();
95 
96  CollisionObject<S> obj2(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
97  return callback(obj1, &obj2, cdata);
98  }
99  }
100  }
101  else
102  {
103  if(collisionRecurse_<S>(nodes1, root1->children[0], tree2, nullptr, root2_bv, tf2, cdata, callback))
104  return true;
105  if(collisionRecurse_<S>(nodes1, root1->children[1], tree2, nullptr, root2_bv, tf2, cdata, callback))
106  return true;
107  }
108 
109  return false;
110  }
111  else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
112  {
113  CollisionObject<S>* obj1 = static_cast<CollisionObject<S>*>(root1->data);
114  if(!tree2->isNodeFree(root2) && !obj1->isFree())
115  {
116  OBB<S> obb1, obb2;
117  convertBV(root1->bv, Transform3<S>::Identity(), obb1);
118  convertBV(root2_bv, tf2, obb2);
119 
120  if(obb1.overlap(obb2))
121  {
122  Box<S>* box = new Box<S>();
123  Transform3<S> box_tf;
124  constructBox(root2_bv, tf2, *box, box_tf);
125 
126  box->cost_density = root2->getOccupancy();
127  box->threshold_occupied = tree2->getOccupancyThres();
128 
129  CollisionObject<S> obj2(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
130  return callback(obj1, &obj2, cdata);
131  }
132  else return false;
133  }
134  else return false;
135  }
136 
137  OBB<S> obb1, obb2;
138  convertBV(root1->bv, Transform3<S>::Identity(), obb1);
139  convertBV(root2_bv, tf2, obb2);
140 
141  if(tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
142 
143  if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
144  {
145  if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback))
146  return true;
147  if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback))
148  return true;
149  }
150  else
151  {
152  for(unsigned int i = 0; i < 8; ++i)
153  {
154  if(tree2->nodeChildExists(root2, i))
155  {
156  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
157  AABB<S> child_bv;
158  computeChildBV(root2_bv, i, child_bv);
159 
160  if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback))
161  return true;
162  }
163  else
164  {
165  AABB<S> child_bv;
166  computeChildBV(root2_bv, i, child_bv);
167  if(collisionRecurse_<S>(nodes1, root1_id, tree2, nullptr, child_bv, tf2, cdata, callback))
168  return true;
169  }
170  }
171  }
172 
173  return false;
174 }
175 
176 //==============================================================================
177 template <typename S, typename Derived>
178 FCL_EXPORT
179 bool collisionRecurse_(
181  size_t root1_id,
182  const OcTree<S>* tree2,
183  const typename OcTree<S>::OcTreeNode* root2,
184  const AABB<S>& root2_bv,
185  const Eigen::MatrixBase<Derived>& translation2,
186  void* cdata,
187  CollisionCallBack<S> callback)
188 {
189  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
190  if(!root2)
191  {
192  if(root1->isLeaf())
193  {
194  CollisionObject<S>* obj1 = static_cast<CollisionObject<S>*>(root1->data);
195 
196  if(!obj1->isFree())
197  {
198  const AABB<S>& root_bv_t = translate(root2_bv, translation2);
199  if(root1->bv.overlap(root_bv_t))
200  {
201  Box<S>* box = new Box<S>();
202  Transform3<S> box_tf;
204  tf2.translation() = translation2;
205  constructBox(root2_bv, tf2, *box, box_tf);
206 
207  box->cost_density = tree2->getDefaultOccupancy();
208 
209  CollisionObject<S> obj2(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
210  return callback(obj1, &obj2, cdata);
211  }
212  }
213  }
214  else
215  {
216  if(collisionRecurse_<S>(nodes1, root1->children[0], tree2, nullptr, root2_bv, translation2, cdata, callback))
217  return true;
218  if(collisionRecurse_<S>(nodes1, root1->children[1], tree2, nullptr, root2_bv, translation2, cdata, callback))
219  return true;
220  }
221 
222  return false;
223  }
224  else if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
225  {
226  CollisionObject<S>* obj1 = static_cast<CollisionObject<S>*>(root1->data);
227  if(!tree2->isNodeFree(root2) && !obj1->isFree())
228  {
229  const AABB<S>& root_bv_t = translate(root2_bv, translation2);
230  if(root1->bv.overlap(root_bv_t))
231  {
232  Box<S>* box = new Box<S>();
233  Transform3<S> box_tf;
235  tf2.translation() = translation2;
236  constructBox(root2_bv, tf2, *box, box_tf);
237 
238  box->cost_density = root2->getOccupancy();
239  box->threshold_occupied = tree2->getOccupancyThres();
240 
241  CollisionObject<S> obj2(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
242  return callback(obj1, &obj2, cdata);
243  }
244  else return false;
245  }
246  else return false;
247  }
248 
249  const AABB<S>& root_bv_t = translate(root2_bv, translation2);
250  if(tree2->isNodeFree(root2) || !root1->bv.overlap(root_bv_t)) return false;
251 
252  if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
253  {
254  if(collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback))
255  return true;
256  if(collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback))
257  return true;
258  }
259  else
260  {
261  for(unsigned int i = 0; i < 8; ++i)
262  {
263  if(tree2->nodeChildExists(root2, i))
264  {
265  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
266  AABB<S> child_bv;
267  computeChildBV(root2_bv, i, child_bv);
268 
269  if(collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback))
270  return true;
271  }
272  else
273  {
274  AABB<S> child_bv;
275  computeChildBV(root2_bv, i, child_bv);
276  if(collisionRecurse_<S>(nodes1, root1_id, tree2, nullptr, child_bv, translation2, cdata, callback))
277  return true;
278  }
279  }
280  }
281 
282  return false;
283 }
284 
285 //==============================================================================
286 template <typename S>
287 FCL_EXPORT
288 bool distanceRecurse_(typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes1, size_t root1_id, const OcTree<S>* tree2, const typename OcTree<S>::OcTreeNode* root2, const AABB<S>& root2_bv, const Transform3<S>& tf2, void* cdata, DistanceCallBack<S> callback, S& min_dist)
289 {
290  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
291  if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
292  {
293  if(tree2->isNodeOccupied(root2))
294  {
295  Box<S>* box = new Box<S>();
296  Transform3<S> box_tf;
297  constructBox(root2_bv, tf2, *box, box_tf);
298  CollisionObject<S> obj(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
299  return callback(static_cast<CollisionObject<S>*>(root1->data), &obj, cdata, min_dist);
300  }
301  else return false;
302  }
303 
304  if(!tree2->isNodeOccupied(root2)) return false;
305 
306  if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
307  {
308  AABB<S> aabb2;
309  convertBV(root2_bv, tf2, aabb2);
310 
311  S d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
312  S d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
313 
314  if(d2 < d1)
315  {
316  if(d2 < min_dist)
317  {
318  if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
319  return true;
320  }
321 
322  if(d1 < min_dist)
323  {
324  if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
325  return true;
326  }
327  }
328  else
329  {
330  if(d1 < min_dist)
331  {
332  if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
333  return true;
334  }
335 
336  if(d2 < min_dist)
337  {
338  if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, tf2, cdata, callback, min_dist))
339  return true;
340  }
341  }
342  }
343  else
344  {
345  for(unsigned int i = 0; i < 8; ++i)
346  {
347  if(tree2->nodeChildExists(root2, i))
348  {
349  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
350  AABB<S> child_bv;
351  computeChildBV(root2_bv, i, child_bv);
352 
353  AABB<S> aabb2;
354  convertBV(child_bv, tf2, aabb2);
355  S d = root1->bv.distance(aabb2);
356 
357  if(d < min_dist)
358  {
359  if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2, cdata, callback, min_dist))
360  return true;
361  }
362  }
363  }
364  }
365 
366  return false;
367 }
368 
369 //==============================================================================
370 template <typename S, typename Derived>
371 FCL_EXPORT
372 bool distanceRecurse_(
374  size_t root1_id,
375  const OcTree<S>* tree2,
376  const typename OcTree<S>::OcTreeNode* root2,
377  const AABB<S>& root2_bv,
378  const Eigen::MatrixBase<Derived>& translation2,
379  void* cdata,
380  DistanceCallBack<S> callback,
381  S& min_dist)
382 {
383  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
384  if(root1->isLeaf() && !tree2->nodeHasChildren(root2))
385  {
386  if(tree2->isNodeOccupied(root2))
387  {
388  Box<S>* box = new Box<S>();
389  Transform3<S> box_tf;
391  tf2.translation() = translation2;
392  constructBox(root2_bv, tf2, *box, box_tf);
393  CollisionObject<S> obj(std::shared_ptr<CollisionGeometry<S>>(box), box_tf);
394  return callback(static_cast<CollisionObject<S>*>(root1->data), &obj, cdata, min_dist);
395  }
396  else return false;
397  }
398 
399  if(!tree2->isNodeOccupied(root2)) return false;
400 
401  if(!tree2->nodeHasChildren(root2) || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
402  {
403  const AABB<S>& aabb2 = translate(root2_bv, translation2);
404 
405  S d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
406  S d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
407 
408  if(d2 < d1)
409  {
410  if(d2 < min_dist)
411  {
412  if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist))
413  return true;
414  }
415 
416  if(d1 < min_dist)
417  {
418  if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist))
419  return true;
420  }
421  }
422  else
423  {
424  if(d1 < min_dist)
425  {
426  if(distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv, translation2, cdata, callback, min_dist))
427  return true;
428  }
429 
430  if(d2 < min_dist)
431  {
432  if(distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv, translation2, cdata, callback, min_dist))
433  return true;
434  }
435  }
436  }
437  else
438  {
439  for(unsigned int i = 0; i < 8; ++i)
440  {
441  if(tree2->nodeChildExists(root2, i))
442  {
443  const typename OcTree<S>::OcTreeNode* child = tree2->getNodeChild(root2, i);
444  AABB<S> child_bv;
445  computeChildBV(root2_bv, i, child_bv);
446 
447  const AABB<S>& aabb2 = translate(child_bv, translation2);
448  S d = root1->bv.distance(aabb2);
449 
450  if(d < min_dist)
451  {
452  if(distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, translation2, cdata, callback, min_dist))
453  return true;
454  }
455  }
456  }
457  }
458 
459  return false;
460 }
461 
462 
463 #endif
464 
465 //==============================================================================
466 template <typename S>
467 FCL_EXPORT
469  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes2, size_t root2_id,
470  void* cdata, CollisionCallBack<S> callback)
471 {
472  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
473  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root2 = nodes2 + root2_id;
474  if(root1->isLeaf() && root2->isLeaf())
475  {
476  if(!root1->bv.overlap(root2->bv)) return false;
477  return callback(static_cast<CollisionObject<S>*>(root1->data), static_cast<CollisionObject<S>*>(root2->data), cdata);
478  }
479 
480  if(!root1->bv.overlap(root2->bv)) return false;
481 
482  if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
483  {
484  if(collisionRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback))
485  return true;
486  if(collisionRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback))
487  return true;
488  }
489  else
490  {
491  if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback))
492  return true;
493  if(collisionRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback))
494  return true;
495  }
496  return false;
497 }
498 
499 //==============================================================================
500 template <typename S>
501 FCL_EXPORT
503 {
504  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root = nodes + root_id;
505  if(root->isLeaf())
506  {
507  if(!root->bv.overlap(query->getAABB())) return false;
508  return callback(static_cast<CollisionObject<S>*>(root->data), query, cdata);
509  }
510 
511  if(!root->bv.overlap(query->getAABB())) return false;
512 
513  int select_res = implementation_array::select(query->getAABB(), root->children[0], root->children[1], nodes);
514 
515  if(collisionRecurse(nodes, root->children[select_res], query, cdata, callback))
516  return true;
517 
518  if(collisionRecurse(nodes, root->children[1-select_res], query, cdata, callback))
519  return true;
520 
521  return false;
522 }
523 
524 //==============================================================================
525 template <typename S>
526 FCL_EXPORT
528 {
529  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root = nodes + root_id;
530  if(root->isLeaf()) return false;
531 
532  if(selfCollisionRecurse(nodes, root->children[0], cdata, callback))
533  return true;
534 
535  if(selfCollisionRecurse(nodes, root->children[1], cdata, callback))
536  return true;
537 
538  if(collisionRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback))
539  return true;
540 
541  return false;
542 }
543 
544 //==============================================================================
545 template <typename S>
546 FCL_EXPORT
548  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes2, size_t root2_id,
549  void* cdata, DistanceCallBack<S> callback, S& min_dist)
550 {
551  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root1 = nodes1 + root1_id;
552  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root2 = nodes2 + root2_id;
553  if(root1->isLeaf() && root2->isLeaf())
554  {
555  CollisionObject<S>* root1_obj = static_cast<CollisionObject<S>*>(root1->data);
556  CollisionObject<S>* root2_obj = static_cast<CollisionObject<S>*>(root2->data);
557  return callback(root1_obj, root2_obj, cdata, min_dist);
558  }
559 
560  if(root2->isLeaf() || (!root1->isLeaf() && (root1->bv.size() > root2->bv.size())))
561  {
562  S d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
563  S d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
564 
565  if(d2 < d1)
566  {
567  if(d2 < min_dist)
568  {
569  if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist))
570  return true;
571  }
572 
573  if(d1 < min_dist)
574  {
575  if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist))
576  return true;
577  }
578  }
579  else
580  {
581  if(d1 < min_dist)
582  {
583  if(distanceRecurse(nodes1, root1->children[0], nodes2, root2_id, cdata, callback, min_dist))
584  return true;
585  }
586 
587  if(d2 < min_dist)
588  {
589  if(distanceRecurse(nodes1, root1->children[1], nodes2, root2_id, cdata, callback, min_dist))
590  return true;
591  }
592  }
593  }
594  else
595  {
596  S d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
597  S d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
598 
599  if(d2 < d1)
600  {
601  if(d2 < min_dist)
602  {
603  if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist))
604  return true;
605  }
606 
607  if(d1 < min_dist)
608  {
609  if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist))
610  return true;
611  }
612  }
613  else
614  {
615  if(d1 < min_dist)
616  {
617  if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[0], cdata, callback, min_dist))
618  return true;
619  }
620 
621  if(d2 < min_dist)
622  {
623  if(distanceRecurse(nodes1, root1_id, nodes2, root2->children[1], cdata, callback, min_dist))
624  return true;
625  }
626  }
627  }
628 
629  return false;
630 }
631 
632 //==============================================================================
633 template <typename S>
634 FCL_EXPORT
635 bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes, size_t root_id, CollisionObject<S>* query, void* cdata, DistanceCallBack<S> callback, S& min_dist)
636 {
637  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root = nodes + root_id;
638  if(root->isLeaf())
639  {
640  CollisionObject<S>* root_obj = static_cast<CollisionObject<S>*>(root->data);
641  return callback(root_obj, query, cdata, min_dist);
642  }
643 
644  S d1 = query->getAABB().distance((nodes + root->children[0])->bv);
645  S d2 = query->getAABB().distance((nodes + root->children[1])->bv);
646 
647  if(d2 < d1)
648  {
649  if(d2 < min_dist)
650  {
651  if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist))
652  return true;
653  }
654 
655  if(d1 < min_dist)
656  {
657  if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist))
658  return true;
659  }
660  }
661  else
662  {
663  if(d1 < min_dist)
664  {
665  if(distanceRecurse(nodes, root->children[0], query, cdata, callback, min_dist))
666  return true;
667  }
668 
669  if(d2 < min_dist)
670  {
671  if(distanceRecurse(nodes, root->children[1], query, cdata, callback, min_dist))
672  return true;
673  }
674  }
675 
676  return false;
677 }
678 
679 //==============================================================================
680 template <typename S>
681 FCL_EXPORT
682 bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes, size_t root_id, void* cdata, DistanceCallBack<S> callback, S& min_dist)
683 {
684  typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* root = nodes + root_id;
685  if(root->isLeaf()) return false;
686 
687  if(selfDistanceRecurse(nodes, root->children[0], cdata, callback, min_dist))
688  return true;
689 
690  if(selfDistanceRecurse(nodes, root->children[1], cdata, callback, min_dist))
691  return true;
692 
693  if(distanceRecurse(nodes, root->children[0], nodes, root->children[1], cdata, callback, min_dist))
694  return true;
695 
696  return false;
697 }
698 
699 
700 #if FCL_HAVE_OCTOMAP
701 
702 //==============================================================================
703 template <typename S>
704 FCL_EXPORT
705 bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes1, size_t root1_id, const OcTree<S>* tree2, const typename OcTree<S>::OcTreeNode* root2, const AABB<S>& root2_bv, const Transform3<S>& tf2, void* cdata, CollisionCallBack<S> callback)
706 {
707  if(tf2.linear().isIdentity())
708  return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback);
709  else
710  return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback);
711 }
712 
713 //==============================================================================
714 template <typename S>
715 FCL_EXPORT
716 bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array<S>::DynamicAABBNode* nodes1, size_t root1_id, const OcTree<S>* tree2, const typename OcTree<S>::OcTreeNode* root2, const AABB<S>& root2_bv, const Transform3<S>& tf2, void* cdata, DistanceCallBack<S> callback, S& min_dist)
717 {
718  if(tf2.linear().isIdentity())
719  return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2.translation(), cdata, callback, min_dist);
720  else
721  return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2, cdata, callback, min_dist);
722 }
723 
724 #endif
725 
726 } // namespace dynamic_AABB_tree_array
727 
728 } // namespace detail
729 
730 //==============================================================================
731 template <typename S>
732 FCL_EXPORT
734  : tree_topdown_balance_threshold(dtree.bu_threshold),
735  tree_topdown_level(dtree.topdown_level)
736 {
740  tree_topdown_level = 0;
741  tree_init_level = 0;
742  setup_ = false;
743 
744  // from experiment, this is the optimal setting
747 }
748 
749 //==============================================================================
750 template <typename S>
751 FCL_EXPORT
753  const std::vector<CollisionObject<S>*>& other_objs)
754 {
755  if(other_objs.empty()) return;
756 
757  if(size() > 0)
758  {
760  }
761  else
762  {
763  DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
764  table.rehash(other_objs.size());
765  for(size_t i = 0, size = other_objs.size(); i < size; ++i)
766  {
767  leaves[i].bv = other_objs[i]->getAABB();
768  leaves[i].parent = dtree.NULL_NODE;
769  leaves[i].children[1] = dtree.NULL_NODE;
770  leaves[i].data = other_objs[i];
771  table[other_objs[i]] = i;
772  }
773 
774  int n_leaves = other_objs.size();
775 
776  dtree.init(leaves, n_leaves, tree_init_level);
777 
778  setup_ = true;
779  }
780 }
781 
782 //==============================================================================
783 template <typename S>
784 FCL_EXPORT
786 {
787  size_t node = dtree.insert(obj->getAABB(), obj);
788  table[obj] = node;
789 }
790 
791 //==============================================================================
792 template <typename S>
793 FCL_EXPORT
795 {
796  size_t node = table[obj];
797  table.erase(obj);
798  dtree.remove(node);
799 }
800 
801 //==============================================================================
802 template <typename S>
803 FCL_EXPORT
805 {
806  if(!setup_)
807  {
808  int num = dtree.size();
809  if(num == 0)
810  {
811  setup_ = true;
812  return;
813  }
814 
815  int height = dtree.getMaxHeight();
816 
817 
818  if(height - std::log((S)num) / std::log(2.0) < max_tree_nonbalanced_level)
819  dtree.balanceIncremental(tree_incremental_balance_pass);
820  else
821  dtree.balanceTopdown();
822 
823  setup_ = true;
824  }
825 }
826 
827 //==============================================================================
828 template <typename S>
829 FCL_EXPORT
831 {
832  for(auto it = table.cbegin(), end = table.cend(); it != end; ++it)
833  {
834  const CollisionObject<S>* obj = it->first;
835  size_t node = it->second;
836  dtree.getNodes()[node].bv = obj->getAABB();
837  }
838 
839  dtree.refit();
840  setup_ = false;
841 
842  setup();
843 }
844 
845 //==============================================================================
846 template <typename S>
847 FCL_EXPORT
849 {
850  const auto it = table.find(updated_obj);
851  if(it != table.end())
852  {
853  size_t node = it->second;
854  if(!dtree.getNodes()[node].bv.equal(updated_obj->getAABB()))
855  dtree.update(node, updated_obj->getAABB());
856  }
857  setup_ = false;
858 }
859 
860 //==============================================================================
861 template <typename S>
862 FCL_EXPORT
864 {
865  update_(updated_obj);
866  setup();
867 }
868 
869 //==============================================================================
870 template <typename S>
871 FCL_EXPORT
873 {
874  for(size_t i = 0, size = updated_objs.size(); i < size; ++i)
875  update_(updated_objs[i]);
876  setup();
877 }
878 
879 //==============================================================================
880 template <typename S>
881 FCL_EXPORT
883 {
884  dtree.clear();
885  table.clear();
886 }
887 
888 //==============================================================================
889 template <typename S>
890 FCL_EXPORT
892 {
893  objs.resize(this->size());
894  std::transform(table.begin(), table.end(), objs.begin(), std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
895 }
896 
897 //==============================================================================
898 template <typename S>
899 FCL_EXPORT
901 {
902  if(size() == 0) return;
903  switch(obj->collisionGeometry()->getNodeType())
904  {
905 #if FCL_HAVE_OCTOMAP
906  case GEOM_OCTREE:
907  {
908  if(!octree_as_geometry_collide)
909  {
910  const OcTree<S>* octree = static_cast<const OcTree<S>*>(obj->collisionGeometry().get());
911  detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback);
912  }
913  else
914  detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
915  }
916  break;
917 #endif
918  default:
919  detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
920  }
921 }
922 
923 //==============================================================================
924 template <typename S>
925 FCL_EXPORT
927 {
928  if(size() == 0) return;
929  S min_dist = std::numeric_limits<S>::max();
930  switch(obj->collisionGeometry()->getNodeType())
931  {
932 #if FCL_HAVE_OCTOMAP
933  case GEOM_OCTREE:
934  {
935  if(!octree_as_geometry_distance)
936  {
937  const OcTree<S>* octree = static_cast<const OcTree<S>*>(obj->collisionGeometry().get());
938  detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(), obj->getTransform(), cdata, callback, min_dist);
939  }
940  else
941  detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
942  }
943  break;
944 #endif
945  default:
946  detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
947  }
948 }
949 
950 //==============================================================================
951 template <typename S>
952 FCL_EXPORT
954 {
955  if(size() == 0) return;
956  detail::dynamic_AABB_tree_array::selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
957 }
958 
959 //==============================================================================
960 template <typename S>
961 FCL_EXPORT
963 {
964  if(size() == 0) return;
965  S min_dist = std::numeric_limits<S>::max();
966  detail::dynamic_AABB_tree_array::selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
967 }
968 
969 //==============================================================================
970 template <typename S>
971 FCL_EXPORT
973 {
974  DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
975  if((size() == 0) || (other_manager->size() == 0)) return;
976  detail::dynamic_AABB_tree_array::collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
977 }
978 
979 //==============================================================================
980 template <typename S>
981 FCL_EXPORT
983 {
984  DynamicAABBTreeCollisionManager_Array* other_manager = static_cast<DynamicAABBTreeCollisionManager_Array*>(other_manager_);
985  if((size() == 0) || (other_manager->size() == 0)) return;
986  S min_dist = std::numeric_limits<S>::max();
987  detail::dynamic_AABB_tree_array::distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
988 }
989 
990 //==============================================================================
991 template <typename S>
992 FCL_EXPORT
994 {
995  return dtree.empty();
996 }
997 
998 //==============================================================================
999 template <typename S>
1000 FCL_EXPORT
1002 {
1003  return dtree.size();
1004 }
1005 
1006 //==============================================================================
1007 template <typename S>
1008 FCL_EXPORT
1011 {
1012  return dtree;
1013 }
1014 
1015 } // namespace fcl
1016 
1017 #endif
fcl::detail::dynamic_AABB_tree_array::selfDistanceRecurse
FCL_EXPORT bool selfDistanceRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes, size_t root_id, void *cdata, DistanceCallBack< S > callback, S &min_dist)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:682
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::convertBV
FCL_EXPORT void convertBV(const BV1 &bv1, const Transform3< typename BV1::S > &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: math/bv/utility-inl.h:973
fcl::detail::implementation_array::NodeBase::parent
size_t parent
Definition: node_base_array.h:59
fcl::DynamicAABBTreeCollisionManager_Array::DynamicAABBTreeCollisionManager_Array
DynamicAABBTreeCollisionManager_Array()
Definition: broadphase_dynamic_AABB_tree_array-inl.h:733
obj2
CollisionObject< S > * obj2
Definition: broadphase_SaP.h:211
fcl::DynamicAABBTreeCollisionManager_Array::tree_topdown_balance_threshold
int & tree_topdown_balance_threshold
Definition: broadphase_dynamic_AABB_tree_array.h:64
fcl::detail::implementation_array::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree_array.h:61
fcl::GEOM_OCTREE
@ GEOM_OCTREE
Definition: collision_geometry.h:54
fcl::DynamicAABBTreeCollisionManager_Array< double >
template class FCL_EXPORT DynamicAABBTreeCollisionManager_Array< double >
fcl::detail::dynamic_AABB_tree_array::distanceRecurse
FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes, size_t root_id, CollisionObject< S > *query, void *cdata, DistanceCallBack< S > callback, S &min_dist)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:635
fcl::DynamicAABBTreeCollisionManager_Array::tree_incremental_balance_pass
int tree_incremental_balance_pass
Definition: broadphase_dynamic_AABB_tree_array.h:63
fcl::DynamicAABBTreeCollisionManager_Array::tree_topdown_level
int & tree_topdown_level
Definition: broadphase_dynamic_AABB_tree_array.h:65
fcl::DynamicAABBTreeCollisionManager_Array::collide
void collide(CollisionObject< S > *obj, void *cdata, CollisionCallBack< S > callback) const
perform collision test between one object and all the objects belonging to the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:900
fcl::DynamicAABBTreeCollisionManager_Array::size
size_t size() const
the number of objects managed by the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:1001
fcl::BroadPhaseCollisionManager::registerObjects
virtual void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
Definition: broadphase_collision_manager-inl.h:68
fcl::CollisionGeometry< S_ >::cost_density
S_ cost_density
collision cost for unit volume
Definition: collision_geometry.h:102
fcl::translate
AABB< S > translate(const AABB< S > &aabb, const Eigen::MatrixBase< Derived > &t)
translate the center of AABB by t
Definition: AABB-inl.h:345
obj1
CollisionObject< S > * obj1
Definition: broadphase_SaP.h:210
max
static T max(T x, T y)
Definition: svm.cpp:52
fcl::detail::dynamic_AABB_tree_array::selfCollisionRecurse
FCL_EXPORT bool selfCollisionRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes, size_t root_id, void *cdata, CollisionCallBack< S > callback)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:527
fcl::transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
fcl::DynamicAABBTreeCollisionManager_Array::getObjects
void getObjects(std::vector< CollisionObject< S > * > &objs) const
return the objects managed by the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:891
fcl::AABB< S >
obj
CollisionObject< S > * obj
object
Definition: broadphase_SaP.h:164
fcl::DynamicAABBTreeCollisionManager_Array::distance
void distance(CollisionObject< S > *obj, void *cdata, DistanceCallBack< S > callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:926
fcl::DynamicAABBTreeCollisionManager_Array::registerObject
void registerObject(CollisionObject< S > *obj)
add one object to the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:785
fcl::DistanceCallBack
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata, S &dist) DistanceCallBack
Callback for distance between two objects, Return value is whether can stop now, also return the mini...
Definition: broadphase_collision_manager.h:60
fcl::DynamicAABBTreeCollisionManager_Array::registerObjects
void registerObjects(const std::vector< CollisionObject< S > * > &other_objs)
add objects to the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:752
fcl::CollisionCallBack
bool(*)(CollisionObject< S > *o1, CollisionObject< S > *o2, void *cdata) CollisionCallBack
Callback for collision between two objects. Return value is whether can stop now.
Definition: broadphase_collision_manager.h:53
fcl::DynamicAABBTreeCollisionManager_Array::update
void update()
update the condition of manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:830
fcl::DynamicAABBTreeCollisionManager_Array::getTree
const detail::implementation_array::HierarchyTree< AABB< S > > & getTree() const
Definition: broadphase_dynamic_AABB_tree_array-inl.h:1010
fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_geometry.h:58
fcl::OBB< S >
fcl::detail::implementation_array::NodeBase
Definition: node_base_array.h:53
fcl::DynamicAABBTreeCollisionManager_Array::max_tree_nonbalanced_level
int max_tree_nonbalanced_level
Definition: broadphase_dynamic_AABB_tree_array.h:62
fcl::detail::select
size_t select(const NodeBase< BV > &query, const NodeBase< BV > &node1, const NodeBase< BV > &node2)
select from node1 and node2 which is close to a given query. 0 for node1 and 1 for node2
Definition: hierarchy_tree-inl.h:1079
fcl::DynamicAABBTreeCollisionManager_Array::octree_as_geometry_collide
bool octree_as_geometry_collide
Definition: broadphase_dynamic_AABB_tree_array.h:68
fcl::DynamicAABBTreeCollisionManager_Array::unregisterObject
void unregisterObject(CollisionObject< S > *obj)
remove one object from the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:794
broadphase_dynamic_AABB_tree_array.h
fcl::DynamicAABBTreeCollisionManager_Array::setup
void setup()
initialize the manager, related with the specific type of manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:804
fcl::detail::dynamic_AABB_tree_array::collisionRecurse
FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes1, size_t root1_id, typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes2, size_t root2_id, void *cdata, CollisionCallBack< S > callback)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:468
fcl::detail::dynamic_AABB_tree_array::distanceRecurse
FCL_EXPORT bool distanceRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes1, size_t root1_id, typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes2, size_t root2_id, void *cdata, DistanceCallBack< S > callback, S &min_dist)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:547
fcl::DynamicAABBTreeCollisionManager_Array::tree_init_level
int tree_init_level
Definition: broadphase_dynamic_AABB_tree_array.h:66
fcl::DynamicAABBTreeCollisionManager_Array
Definition: broadphase_dynamic_AABB_tree_array.h:55
fcl::DynamicAABBTreeCollisionManager_Array::setup_
bool setup_
Definition: broadphase_dynamic_AABB_tree_array.h:130
fcl::constructBox
template void constructBox(const OBB< double > &bv, Box< double > &box, Transform3< double > &tf)
fcl::detail::implementation_array::NodeBase::data
void * data
Definition: node_base_array.h:66
fcl::Box
Center at zero point, axis aligned box.
Definition: box.h:50
fcl::CollisionObject::getAABB
const AABB< S > & getAABB() const
get the AABB in world space
Definition: collision_object-inl.h:111
fcl::detail::implementation_array::NodeBase::bv
BV bv
Definition: node_base_array.h:55
fcl::detail::implementation_array::NodeBase::children
size_t children[2]
Definition: node_base_array.h:65
fcl::AABB::distance
S distance(const AABB< S > &other, Vector3< S > *P, Vector3< S > *Q) const
Distance between two AABBs; P and Q, should not be nullptr, return the nearest points.
Definition: AABB-inl.h:237
fcl::DynamicAABBTreeCollisionManager_Array::octree_as_geometry_distance
bool octree_as_geometry_distance
Definition: broadphase_dynamic_AABB_tree_array.h:69
fcl::AABB::size
S size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Definition: AABB-inl.h:216
fcl::DynamicAABBTreeCollisionManager_Array::update_
void update_(CollisionObject< S > *updated_obj)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:848
fcl::detail::dynamic_AABB_tree_array::collisionRecurse
FCL_EXPORT bool collisionRecurse(typename DynamicAABBTreeCollisionManager_Array< S >::DynamicAABBNode *nodes, size_t root_id, CollisionObject< S > *query, void *cdata, CollisionCallBack< S > callback)
Definition: broadphase_dynamic_AABB_tree_array-inl.h:502
fcl::DynamicAABBTreeCollisionManager_Array::empty
bool empty() const
whether the manager is empty
Definition: broadphase_dynamic_AABB_tree_array-inl.h:993
fcl::OBB::overlap
bool overlap(const OBB< S > &other) const
Check collision between two OBB, return true if collision happens.
Definition: OBB-inl.h:98
fcl::CollisionGeometry< S_ >::threshold_occupied
S_ threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_geometry.h:105
fcl::DynamicAABBTreeCollisionManager_Array::clear
void clear()
clear the manager
Definition: broadphase_dynamic_AABB_tree_array-inl.h:882
fcl::DynamicAABBTreeCollisionManager_Array::dtree
detail::implementation_array::HierarchyTree< AABB< S > > dtree
Definition: broadphase_dynamic_AABB_tree_array.h:127
octree.h
fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:51
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::detail::implementation_array::NodeBase::isLeaf
bool isLeaf() const
Definition: node_base_array-inl.h:54
fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:66


fcl
Author(s):
autogenerated on Tue Jun 22 2021 07:27:49