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

fcl_catkin
Author(s):
autogenerated on Thu Mar 23 2023 03:00:17