broadphase_dynamic_AABB_tree_array.cpp
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 
39 
40 #if HPP_FCL_HAVE_OCTOMAP
41 #include "hpp/fcl/octree.h"
42 #endif
43 namespace hpp {
44 namespace fcl {
45 namespace detail {
46 namespace dynamic_AABB_tree_array {
47 
48 #if HPP_FCL_HAVE_OCTOMAP
49 
50 //==============================================================================
51 bool collisionRecurse_(
53  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
54  const AABB& root2_bv, const Transform3f& tf2,
55  CollisionCallBackBase* callback) {
57  nodes1 + root1_id;
58  if (!root2) {
59  if (root1->isLeaf()) {
60  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
61 
62  if (!obj1->collisionGeometry()->isFree()) {
63  OBB obb1, obb2;
64  convertBV(root1->bv, Transform3f::Identity(), obb1);
65  convertBV(root2_bv, tf2, obb2);
66 
67  if (obb1.overlap(obb2)) {
68  Box* box = new Box();
69  Transform3f box_tf;
70  constructBox(root2_bv, tf2, *box, box_tf);
71 
72  box->cost_density = tree2->getDefaultOccupancy();
73 
74  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
75  return (*callback)(obj1, &obj2);
76  }
77  }
78  } else {
79  if (collisionRecurse_(nodes1, root1->children[0], tree2, nullptr,
80  root2_bv, tf2, callback))
81  return true;
82  if (collisionRecurse_(nodes1, root1->children[1], tree2, nullptr,
83  root2_bv, tf2, callback))
84  return true;
85  }
86 
87  return false;
88  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
89  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
90  if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
91  OBB obb1, obb2;
92  convertBV(root1->bv, Transform3f::Identity(), obb1);
93  convertBV(root2_bv, tf2, obb2);
94 
95  if (obb1.overlap(obb2)) {
96  Box* box = new Box();
97  Transform3f box_tf;
98  constructBox(root2_bv, tf2, *box, box_tf);
99 
100  box->cost_density = root2->getOccupancy();
101  box->threshold_occupied = tree2->getOccupancyThres();
102 
103  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
104  return (*callback)(obj1, &obj2);
105  } else
106  return false;
107  } else
108  return false;
109  }
110 
111  OBB obb1, obb2;
112  convertBV(root1->bv, Transform3f::Identity(), obb1);
113  convertBV(root2_bv, tf2, obb2);
114 
115  if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
116 
117  if (!tree2->nodeHasChildren(root2) ||
118  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
119  if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
120  tf2, callback))
121  return true;
122  if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
123  tf2, callback))
124  return true;
125  } else {
126  for (unsigned int i = 0; i < 8; ++i) {
127  if (tree2->nodeChildExists(root2, i)) {
128  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
129  AABB child_bv;
130  computeChildBV(root2_bv, i, child_bv);
131 
132  if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
133  callback))
134  return true;
135  } else {
136  AABB child_bv;
137  computeChildBV(root2_bv, i, child_bv);
138  if (collisionRecurse_(nodes1, root1_id, tree2, nullptr, child_bv, tf2,
139  callback))
140  return true;
141  }
142  }
143  }
144 
145  return false;
146 }
147 
148 //==============================================================================
149 bool distanceRecurse_(
151  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
152  const AABB& root2_bv, const Transform3f& tf2,
153  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
155  nodes1 + root1_id;
156  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
157  if (tree2->isNodeOccupied(root2)) {
158  Box* box = new Box();
159  Transform3f box_tf;
160  constructBox(root2_bv, tf2, *box, box_tf);
161  CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
162  return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
163  min_dist);
164  } else
165  return false;
166  }
167 
168  if (!tree2->isNodeOccupied(root2)) return false;
169 
170  if (!tree2->nodeHasChildren(root2) ||
171  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
172  AABB aabb2;
173  convertBV(root2_bv, tf2, aabb2);
174 
175  FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
176  FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
177 
178  if (d2 < d1) {
179  if (d2 < min_dist) {
180  if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
181  tf2, callback, min_dist))
182  return true;
183  }
184 
185  if (d1 < min_dist) {
186  if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
187  tf2, callback, min_dist))
188  return true;
189  }
190  } else {
191  if (d1 < min_dist) {
192  if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
193  tf2, callback, min_dist))
194  return true;
195  }
196 
197  if (d2 < min_dist) {
198  if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
199  tf2, callback, min_dist))
200  return true;
201  }
202  }
203  } else {
204  for (unsigned int i = 0; i < 8; ++i) {
205  if (tree2->nodeChildExists(root2, i)) {
206  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
207  AABB child_bv;
208  computeChildBV(root2_bv, i, child_bv);
209 
210  AABB aabb2;
211  convertBV(child_bv, tf2, aabb2);
212  FCL_REAL d = root1->bv.distance(aabb2);
213 
214  if (d < min_dist) {
215  if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
216  callback, min_dist))
217  return true;
218  }
219  }
220  }
221  }
222 
223  return false;
224 }
225 
226 #endif
227 
228 //==============================================================================
231  size_t root1_id,
233  size_t root2_id, CollisionCallBackBase* callback) {
235  nodes1 + root1_id;
237  nodes2 + root2_id;
238  if (root1->isLeaf() && root2->isLeaf()) {
239  if (!root1->bv.overlap(root2->bv)) return false;
240  return (*callback)(static_cast<CollisionObject*>(root1->data),
241  static_cast<CollisionObject*>(root2->data));
242  }
243 
244  if (!root1->bv.overlap(root2->bv)) return false;
245 
246  if (root2->isLeaf() ||
247  (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
248  if (collisionRecurse(nodes1, root1->children[0], nodes2, root2_id,
249  callback))
250  return true;
251  if (collisionRecurse(nodes1, root1->children[1], nodes2, root2_id,
252  callback))
253  return true;
254  } else {
255  if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[0],
256  callback))
257  return true;
258  if (collisionRecurse(nodes1, root1_id, nodes2, root2->children[1],
259  callback))
260  return true;
261  }
262  return false;
263 }
264 
265 //==============================================================================
266 inline HPP_FCL_DLLAPI bool collisionRecurse(
268  size_t root_id, CollisionObject* query, CollisionCallBackBase* callback) {
270  if (!root->bv.overlap(query->getAABB())) return false;
271 
272  if (root->isLeaf()) {
273  return (*callback)(static_cast<CollisionObject*>(root->data), query);
274  }
275 
276  size_t select_res = implementation_array::select(
277  query->getAABB(), root->children[0], root->children[1], nodes);
278 
279  if (collisionRecurse(nodes, root->children[select_res], query, callback))
280  return true;
281 
282  if (collisionRecurse(nodes, root->children[1 - select_res], query, callback))
283  return true;
284 
285  return false;
286 }
287 
288 //==============================================================================
291  size_t root_id, CollisionCallBackBase* callback) {
293  if (root->isLeaf()) return false;
294 
295  if (selfCollisionRecurse(nodes, root->children[0], callback)) return true;
296 
297  if (selfCollisionRecurse(nodes, root->children[1], callback)) return true;
298 
299  if (collisionRecurse(nodes, root->children[0], nodes, root->children[1],
300  callback))
301  return true;
302 
303  return false;
304 }
305 
306 //==============================================================================
309  size_t root1_id,
311  size_t root2_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) {
313  nodes1 + root1_id;
315  nodes2 + root2_id;
316  if (root1->isLeaf() && root2->isLeaf()) {
317  CollisionObject* root1_obj = static_cast<CollisionObject*>(root1->data);
318  CollisionObject* root2_obj = static_cast<CollisionObject*>(root2->data);
319  return (*callback)(root1_obj, root2_obj, min_dist);
320  }
321 
322  if (root2->isLeaf() ||
323  (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
324  FCL_REAL d1 = root2->bv.distance((nodes1 + root1->children[0])->bv);
325  FCL_REAL d2 = root2->bv.distance((nodes1 + root1->children[1])->bv);
326 
327  if (d2 < d1) {
328  if (d2 < min_dist) {
329  if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id,
330  callback, min_dist))
331  return true;
332  }
333 
334  if (d1 < min_dist) {
335  if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id,
336  callback, min_dist))
337  return true;
338  }
339  } else {
340  if (d1 < min_dist) {
341  if (distanceRecurse(nodes1, root1->children[0], nodes2, root2_id,
342  callback, min_dist))
343  return true;
344  }
345 
346  if (d2 < min_dist) {
347  if (distanceRecurse(nodes1, root1->children[1], nodes2, root2_id,
348  callback, min_dist))
349  return true;
350  }
351  }
352  } else {
353  FCL_REAL d1 = root1->bv.distance((nodes2 + root2->children[0])->bv);
354  FCL_REAL d2 = root1->bv.distance((nodes2 + root2->children[1])->bv);
355 
356  if (d2 < d1) {
357  if (d2 < min_dist) {
358  if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1],
359  callback, min_dist))
360  return true;
361  }
362 
363  if (d1 < min_dist) {
364  if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0],
365  callback, min_dist))
366  return true;
367  }
368  } else {
369  if (d1 < min_dist) {
370  if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[0],
371  callback, min_dist))
372  return true;
373  }
374 
375  if (d2 < min_dist) {
376  if (distanceRecurse(nodes1, root1_id, nodes2, root2->children[1],
377  callback, min_dist))
378  return true;
379  }
380  }
381  }
382 
383  return false;
384 }
385 
386 //==============================================================================
389  size_t root_id, CollisionObject* query, DistanceCallBackBase* callback,
390  FCL_REAL& min_dist) {
392  if (root->isLeaf()) {
393  CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
394  return (*callback)(root_obj, query, min_dist);
395  }
396 
397  FCL_REAL d1 = query->getAABB().distance((nodes + root->children[0])->bv);
398  FCL_REAL d2 = query->getAABB().distance((nodes + root->children[1])->bv);
399 
400  if (d2 < d1) {
401  if (d2 < min_dist) {
402  if (distanceRecurse(nodes, root->children[1], query, callback, min_dist))
403  return true;
404  }
405 
406  if (d1 < min_dist) {
407  if (distanceRecurse(nodes, root->children[0], query, callback, min_dist))
408  return true;
409  }
410  } else {
411  if (d1 < min_dist) {
412  if (distanceRecurse(nodes, root->children[0], query, callback, min_dist))
413  return true;
414  }
415 
416  if (d2 < min_dist) {
417  if (distanceRecurse(nodes, root->children[1], query, callback, min_dist))
418  return true;
419  }
420  }
421 
422  return false;
423 }
424 
425 //==============================================================================
428  size_t root_id, DistanceCallBackBase* callback, FCL_REAL& min_dist) {
430  if (root->isLeaf()) return false;
431 
432  if (selfDistanceRecurse(nodes, root->children[0], callback, min_dist))
433  return true;
434 
435  if (selfDistanceRecurse(nodes, root->children[1], callback, min_dist))
436  return true;
437 
438  if (distanceRecurse(nodes, root->children[0], nodes, root->children[1],
439  callback, min_dist))
440  return true;
441 
442  return false;
443 }
444 
445 #if HPP_FCL_HAVE_OCTOMAP
446 
447 //==============================================================================
448 bool collisionRecurse(
450  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
451  const AABB& root2_bv, const Transform3f& tf2,
452  CollisionCallBackBase* callback) {
453  if (tf2.rotation().isIdentity())
454  return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
455  tf2.translation(), callback);
456  else
457  return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
458  callback);
459 }
460 
461 //==============================================================================
462 bool distanceRecurse(
464  size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2,
465  const AABB& root2_bv, const Transform3f& tf2,
466  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
467  if (tf2.rotation().isIdentity())
468  return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
469  tf2.translation(), callback, min_dist);
470  else
471  return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
472  callback, min_dist);
473 }
474 
475 #endif
476 
477 } // namespace dynamic_AABB_tree_array
478 
479 } // namespace detail
480 
481 //==============================================================================
483  tree_topdown_balance_threshold = &dtree.bu_threshold;
484  tree_topdown_level = &dtree.topdown_level;
485  max_tree_nonbalanced_level = 10;
486  tree_incremental_balance_pass = 10;
487  *tree_topdown_balance_threshold = 2;
488  *tree_topdown_level = 0;
489  tree_init_level = 0;
490  setup_ = false;
491 
492  // from experiment, this is the optimal setting
493  octree_as_geometry_collide = true;
494  octree_as_geometry_distance = false;
495 }
496 
497 //==============================================================================
499  const std::vector<CollisionObject*>& other_objs) {
500  if (other_objs.empty()) return;
501 
502  if (size() > 0) {
504  } else {
505  DynamicAABBNode* leaves = new DynamicAABBNode[other_objs.size()];
506  table.rehash(other_objs.size());
507  for (size_t i = 0, size = other_objs.size(); i < size; ++i) {
508  leaves[i].bv = other_objs[i]->getAABB();
509  leaves[i].parent = dtree.NULL_NODE;
510  leaves[i].children[1] = dtree.NULL_NODE;
511  leaves[i].data = other_objs[i];
512  table[other_objs[i]] = i;
513  }
514 
515  int n_leaves = (int)other_objs.size();
516 
517  dtree.init(leaves, n_leaves, tree_init_level);
518 
519  setup_ = true;
520  }
521 }
522 
523 //==============================================================================
525  CollisionObject* obj) {
526  size_t node = dtree.insert(obj->getAABB(), obj);
527  table[obj] = node;
528 }
529 
530 //==============================================================================
532  CollisionObject* obj) {
533  size_t node = table[obj];
534  table.erase(obj);
535  dtree.remove(node);
536 }
537 
538 //==============================================================================
540  if (!setup_) {
541  int num = (int)dtree.size();
542  if (num == 0) {
543  setup_ = true;
544  return;
545  }
546 
547  int height = (int)dtree.getMaxHeight();
548 
549  if ((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0) <
550  max_tree_nonbalanced_level)
551  dtree.balanceIncremental(tree_incremental_balance_pass);
552  else
553  dtree.balanceTopdown();
554 
555  setup_ = true;
556  }
557 }
558 
559 //==============================================================================
561  for (auto it = table.cbegin(), end = table.cend(); it != end; ++it) {
562  const CollisionObject* obj = it->first;
563  size_t node = it->second;
564  dtree.getNodes()[node].bv = obj->getAABB();
565  }
566 
567  dtree.refit();
568  setup_ = false;
569 
570  setup();
571 }
572 
573 //==============================================================================
575  CollisionObject* updated_obj) {
576  const auto it = table.find(updated_obj);
577  if (it != table.end()) {
578  size_t node = it->second;
579  if (!(dtree.getNodes()[node].bv == updated_obj->getAABB()))
580  dtree.update(node, updated_obj->getAABB());
581  }
582  setup_ = false;
583 }
584 
585 //==============================================================================
587  CollisionObject* updated_obj) {
588  update_(updated_obj);
589  setup();
590 }
591 
592 //==============================================================================
594  const std::vector<CollisionObject*>& updated_objs) {
595  for (size_t i = 0, size = updated_objs.size(); i < size; ++i)
596  update_(updated_objs[i]);
597  setup();
598 }
599 
600 //==============================================================================
602  dtree.clear();
603  table.clear();
604 }
605 
606 //==============================================================================
608  std::vector<CollisionObject*>& objs) const {
609  objs.resize(this->size());
611  table.begin(), table.end(), objs.begin(),
612  std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
613 }
614 
615 //==============================================================================
617  CollisionObject* obj, CollisionCallBackBase* callback) const {
618  callback->init();
619  if (size() == 0) return;
620  switch (obj->collisionGeometry()->getNodeType()) {
621 #if HPP_FCL_HAVE_OCTOMAP
622  case GEOM_OCTREE: {
623  if (!octree_as_geometry_collide) {
624  const OcTree* octree =
625  static_cast<const OcTree*>(obj->collisionGeometry().get());
627  dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(),
628  octree->getRootBV(), obj->getTransform(), callback);
629  } else
631  dtree.getNodes(), dtree.getRoot(), obj, callback);
632  } break;
633 #endif
634  default:
636  dtree.getNodes(), dtree.getRoot(), obj, callback);
637  }
638 }
639 
640 //==============================================================================
642  CollisionObject* obj, DistanceCallBackBase* callback) const {
643  callback->init();
644  if (size() == 0) return;
645  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
646  switch (obj->collisionGeometry()->getNodeType()) {
647 #if HPP_FCL_HAVE_OCTOMAP
648  case GEOM_OCTREE: {
649  if (!octree_as_geometry_distance) {
650  const OcTree* octree =
651  static_cast<const OcTree*>(obj->collisionGeometry().get());
653  dtree.getNodes(), dtree.getRoot(), octree, octree->getRoot(),
654  octree->getRootBV(), obj->getTransform(), callback, min_dist);
655  } else
657  dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist);
658  } break;
659 #endif
660  default:
662  dtree.getNodes(), dtree.getRoot(), obj, callback, min_dist);
663  }
664 }
665 
666 //==============================================================================
668  CollisionCallBackBase* callback) const {
669  callback->init();
670  if (size() == 0) return;
672  dtree.getNodes(), dtree.getRoot(), callback);
673 }
674 
675 //==============================================================================
677  DistanceCallBackBase* callback) const {
678  callback->init();
679  if (size() == 0) return;
680  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
682  dtree.getNodes(), dtree.getRoot(), callback, min_dist);
683 }
684 
685 //==============================================================================
687  BroadPhaseCollisionManager* other_manager_,
688  CollisionCallBackBase* callback) const {
689  callback->init();
690  DynamicAABBTreeArrayCollisionManager* other_manager =
691  static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
692  if ((size() == 0) || (other_manager->size() == 0)) return;
694  dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(),
695  other_manager->dtree.getRoot(), callback);
696 }
697 
698 //==============================================================================
700  BroadPhaseCollisionManager* other_manager_,
701  DistanceCallBackBase* callback) const {
702  callback->init();
703  DynamicAABBTreeArrayCollisionManager* other_manager =
704  static_cast<DynamicAABBTreeArrayCollisionManager*>(other_manager_);
705  if ((size() == 0) || (other_manager->size() == 0)) return;
706  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
708  dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(),
709  other_manager->dtree.getRoot(), callback, min_dist);
710 }
711 
712 //==============================================================================
714  return dtree.empty();
715 }
716 
717 //==============================================================================
719  return dtree.size();
720 }
721 
722 //==============================================================================
725  return dtree;
726 }
727 
728 } // namespace fcl
729 } // namespace hpp
virtual void init()
Initialization of the callback before running the collision broadphase manager.
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
bool selfCollisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes, size_t root_id, CollisionCallBackBase *callback)
OcTreeNode * getRoot() const
get the root node of the octree
Definition: octree.h:118
detail::implementation_array::NodeBase< AABB > DynamicAABBNode
void registerObjects(const std::vector< CollisionObject *> &other_objs)
add objects to the manager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
void registerObject(CollisionObject *obj)
add one object to the manager
Main namespace.
virtual void registerObjects(const std::vector< CollisionObject *> &other_objs)
add objects to the manager
Base callback class for collision queries. This class can be supersed by child classes to provide des...
tuple tf2
size_t size() const
the number of objects managed by the manager
AABB getRootBV() const
get the bounding volume for the root
Definition: octree.h:107
size_t select(size_t query, size_t node1, size_t node2, NodeBase< BV > *nodes)
select the node from node1 and node2 which is close to the query-th node in the nodes. 0 for node1 and 1 for node2.
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node&#39;s i-th child
Definition: octree.h:239
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager ...
virtual void init()
Initialization of the callback before running the collision broadphase manager.
bool distanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes2, size_t root2_id, DistanceCallBackBase *callback, FCL_REAL &min_dist)
const Transform3f & getTransform() const
get object&#39;s transform
const Vec3f & translation() const
get translation
Definition: transform.h:103
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Definition: octree.h:53
Definition: octree.py:1
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
double FCL_REAL
Definition: data_types.h:65
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager ...
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
void setup()
initialize the manager, related with the specific type of manager
void unregisterObject(CollisionObject *obj)
remove one object from the manager
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: BV/AABB.h:54
bool collisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes1, size_t root1_id, DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes2, size_t root2_id, CollisionCallBackBase *callback)
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get geometry from the object instance
bool selfDistanceRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes, size_t root_id, DistanceCallBackBase *callback, FCL_REAL &min_dist)
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
const detail::implementation_array::HierarchyTree< AABB > & getTree() const
const Matrix3f & rotation() const
get rotation
Definition: transform.h:112
const AABB & getAABB() const
get the AABB in world space
detail::implementation_array::HierarchyTree< AABB > dtree
static Transform3f Identity()
Definition: transform.h:67
virtual void update()
update the condition of manager
the object for collision or distance computation, contains the geometry and the transform information...
static void convertBV(const BV1 &bv1, const Transform3f &tf1, BV2 &bv2)
Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity...
Definition: BV.h:277
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
Base callback class for distance queries. This class can be supersed by child classes to provide desi...


hpp-fcl
Author(s):
autogenerated on Fri Jun 2 2023 02:39:00