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