src/broadphase/broadphase_dynamic_AABB_tree.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 #include <limits>
41 
42 #if HPP_FCL_HAVE_OCTOMAP
43 #include "hpp/fcl/octree.h"
44 #endif
45 
46 #include "hpp/fcl/BV/BV.h"
48 
49 namespace hpp {
50 namespace fcl {
51 namespace detail {
52 
53 namespace dynamic_AABB_tree {
54 
55 #if HPP_FCL_HAVE_OCTOMAP
56 //==============================================================================
57 bool collisionRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
58  const OcTree* tree2, const OcTree::OcTreeNode* root2,
59  const AABB& root2_bv, const Transform3f& tf2,
60  CollisionCallBackBase* callback) {
61  if (!root2) {
62  if (root1->isLeaf()) {
63  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
64 
65  if (!obj1->collisionGeometry()->isFree()) {
66  OBB obb1, obb2;
67  convertBV(root1->bv, Transform3f::Identity(), obb1);
68  convertBV(root2_bv, tf2, obb2);
69 
70  if (obb1.overlap(obb2)) {
71  Box* box = new Box();
72  Transform3f box_tf;
73  constructBox(root2_bv, tf2, *box, box_tf);
74 
75  box->cost_density = tree2->getDefaultOccupancy();
76 
77  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
78  return (*callback)(obj1, &obj2);
79  }
80  }
81  } else {
82  if (collisionRecurse_(root1->children[0], tree2, nullptr, root2_bv, tf2,
83  callback))
84  return true;
85  if (collisionRecurse_(root1->children[1], tree2, nullptr, root2_bv, tf2,
86  callback))
87  return true;
88  }
89 
90  return false;
91  } else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
92  CollisionObject* obj1 = static_cast<CollisionObject*>(root1->data);
93 
94  if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
95  OBB obb1, obb2;
96  convertBV(root1->bv, Transform3f::Identity(), obb1);
97  convertBV(root2_bv, tf2, obb2);
98 
99  if (obb1.overlap(obb2)) {
100  Box* box = new Box();
101  Transform3f box_tf;
102  constructBox(root2_bv, tf2, *box, box_tf);
103 
104  box->cost_density = root2->getOccupancy();
105  box->threshold_occupied = tree2->getOccupancyThres();
106 
107  CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
108  return (*callback)(obj1, &obj2);
109  } else
110  return false;
111  } else
112  return false;
113  }
114 
115  OBB obb1, obb2;
116  convertBV(root1->bv, Transform3f::Identity(), obb1);
117  convertBV(root2_bv, tf2, obb2);
118 
119  if (tree2->isNodeFree(root2) || !obb1.overlap(obb2)) return false;
120 
121  if (!tree2->nodeHasChildren(root2) ||
122  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
123  if (collisionRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
124  callback))
125  return true;
126  if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
127  callback))
128  return true;
129  } else {
130  for (unsigned int i = 0; i < 8; ++i) {
131  if (tree2->nodeChildExists(root2, i)) {
132  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
133  AABB child_bv;
134  computeChildBV(root2_bv, i, child_bv);
135 
136  if (collisionRecurse_(root1, tree2, child, child_bv, tf2, callback))
137  return true;
138  } else {
139  AABB child_bv;
140  computeChildBV(root2_bv, i, child_bv);
141  if (collisionRecurse_(root1, tree2, nullptr, child_bv, tf2, callback))
142  return true;
143  }
144  }
145  }
146  return false;
147 }
148 
149 //==============================================================================
150 bool distanceRecurse_(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
151  const OcTree* tree2, const OcTree::OcTreeNode* root2,
152  const AABB& root2_bv, const Transform3f& tf2,
153  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
154  if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
155  if (tree2->isNodeOccupied(root2)) {
156  Box* box = new Box();
157  Transform3f box_tf;
158  constructBox(root2_bv, tf2, *box, box_tf);
159  CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
160  return (*callback)(static_cast<CollisionObject*>(root1->data), &obj,
161  min_dist);
162  } else
163  return false;
164  }
165 
166  if (!tree2->isNodeOccupied(root2)) return false;
167 
168  if (!tree2->nodeHasChildren(root2) ||
169  (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
170  AABB aabb2;
171  convertBV(root2_bv, tf2, aabb2);
172 
173  FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
174  FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
175 
176  if (d2 < d1) {
177  if (d2 < min_dist) {
178  if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
179  callback, min_dist))
180  return true;
181  }
182 
183  if (d1 < min_dist) {
184  if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
185  callback, min_dist))
186  return true;
187  }
188  } else {
189  if (d1 < min_dist) {
190  if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv, tf2,
191  callback, min_dist))
192  return true;
193  }
194 
195  if (d2 < min_dist) {
196  if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv, tf2,
197  callback, min_dist))
198  return true;
199  }
200  }
201  } else {
202  for (unsigned int i = 0; i < 8; ++i) {
203  if (tree2->nodeChildExists(root2, i)) {
204  const OcTree::OcTreeNode* child = tree2->getNodeChild(root2, i);
205  AABB child_bv;
206  computeChildBV(root2_bv, i, child_bv);
207 
208  AABB aabb2;
209  convertBV(child_bv, tf2, aabb2);
210  FCL_REAL d = root1->bv.distance(aabb2);
211 
212  if (d < min_dist) {
213  if (distanceRecurse_(root1, tree2, child, child_bv, tf2, callback,
214  min_dist))
215  return true;
216  }
217  }
218  }
219  }
220 
221  return false;
222 }
223 
224 //==============================================================================
226  const OcTree* tree2, const OcTree::OcTreeNode* root2,
227  const AABB& root2_bv, const Transform3f& tf2,
228  CollisionCallBackBase* callback) {
229  if (tf2.rotation().isIdentity())
230  return collisionRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
231  callback);
232  else // has rotation
233  return collisionRecurse_(root1, tree2, root2, root2_bv, tf2, callback);
234 }
235 
236 //==============================================================================
238  const OcTree* tree2, const OcTree::OcTreeNode* root2,
239  const AABB& root2_bv, const Transform3f& tf2,
240  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
241  if (tf2.rotation().isIdentity())
242  return distanceRecurse_(root1, tree2, root2, root2_bv, tf2.translation(),
243  callback, min_dist);
244  else
245  return distanceRecurse_(root1, tree2, root2, root2_bv, tf2, callback,
246  min_dist);
247 }
248 
249 #endif
250 
251 //==============================================================================
255  if (root1->isLeaf() && root2->isLeaf()) {
256  if (!root1->bv.overlap(root2->bv)) return false;
257  return (*callback)(static_cast<CollisionObject*>(root1->data),
258  static_cast<CollisionObject*>(root2->data));
259  }
260 
261  if (!root1->bv.overlap(root2->bv)) return false;
262 
263  if (root2->isLeaf() ||
264  (!root1->isLeaf() && (root1->bv.size() > root2->bv.size()))) {
265  if (collisionRecurse(root1->children[0], root2, callback)) return true;
266  if (collisionRecurse(root1->children[1], root2, callback)) return true;
267  } else {
268  if (collisionRecurse(root1, root2->children[0], callback)) return true;
269  if (collisionRecurse(root1, root2->children[1], callback)) return true;
270  }
271  return false;
272 }
273 
274 //==============================================================================
277  if (root->isLeaf()) {
278  if (!root->bv.overlap(query->getAABB())) return false;
279  return (*callback)(static_cast<CollisionObject*>(root->data), query);
280  }
281 
282  if (!root->bv.overlap(query->getAABB())) return false;
283 
284  size_t select_res =
285  select(query->getAABB(), *(root->children[0]), *(root->children[1]));
286 
287  if (collisionRecurse(root->children[select_res], query, callback))
288  return true;
289 
290  if (collisionRecurse(root->children[1 - select_res], query, callback))
291  return true;
292 
293  return false;
294 }
295 
296 //==============================================================================
300  if (root->isLeaf()) return false;
301 
302  if (selfCollisionRecurse(root->children[0], callback)) return true;
303 
304  if (selfCollisionRecurse(root->children[1], callback)) return true;
305 
306  if (collisionRecurse(root->children[0], root->children[1], callback))
307  return true;
308 
309  return false;
310 }
311 
312 //==============================================================================
315  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
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(root1->children[0]->bv);
325  FCL_REAL d2 = root2->bv.distance(root1->children[1]->bv);
326 
327  if (d2 < d1) {
328  if (d2 < min_dist) {
329  if (distanceRecurse(root1->children[1], root2, callback, min_dist))
330  return true;
331  }
332 
333  if (d1 < min_dist) {
334  if (distanceRecurse(root1->children[0], root2, callback, min_dist))
335  return true;
336  }
337  } else {
338  if (d1 < min_dist) {
339  if (distanceRecurse(root1->children[0], root2, callback, min_dist))
340  return true;
341  }
342 
343  if (d2 < min_dist) {
344  if (distanceRecurse(root1->children[1], root2, callback, min_dist))
345  return true;
346  }
347  }
348  } else {
349  FCL_REAL d1 = root1->bv.distance(root2->children[0]->bv);
350  FCL_REAL d2 = root1->bv.distance(root2->children[1]->bv);
351 
352  if (d2 < d1) {
353  if (d2 < min_dist) {
354  if (distanceRecurse(root1, root2->children[1], callback, min_dist))
355  return true;
356  }
357 
358  if (d1 < min_dist) {
359  if (distanceRecurse(root1, root2->children[0], callback, min_dist))
360  return true;
361  }
362  } else {
363  if (d1 < min_dist) {
364  if (distanceRecurse(root1, root2->children[0], callback, min_dist))
365  return true;
366  }
367 
368  if (d2 < min_dist) {
369  if (distanceRecurse(root1, root2->children[1], callback, min_dist))
370  return true;
371  }
372  }
373  }
374 
375  return false;
376 }
377 
378 //==============================================================================
381  FCL_REAL& min_dist) {
382  if (root->isLeaf()) {
383  CollisionObject* root_obj = static_cast<CollisionObject*>(root->data);
384  return (*callback)(root_obj, query, min_dist);
385  }
386 
387  FCL_REAL d1 = query->getAABB().distance(root->children[0]->bv);
388  FCL_REAL d2 = query->getAABB().distance(root->children[1]->bv);
389 
390  if (d2 < d1) {
391  if (d2 < min_dist) {
392  if (distanceRecurse(root->children[1], query, callback, min_dist))
393  return true;
394  }
395 
396  if (d1 < min_dist) {
397  if (distanceRecurse(root->children[0], query, callback, min_dist))
398  return true;
399  }
400  } else {
401  if (d1 < min_dist) {
402  if (distanceRecurse(root->children[0], query, callback, min_dist))
403  return true;
404  }
405 
406  if (d2 < min_dist) {
407  if (distanceRecurse(root->children[1], query, callback, min_dist))
408  return true;
409  }
410  }
411 
412  return false;
413 }
414 
415 //==============================================================================
417  DistanceCallBackBase* callback, FCL_REAL& min_dist) {
418  if (root->isLeaf()) return false;
419 
420  if (selfDistanceRecurse(root->children[0], callback, min_dist)) return true;
421 
422  if (selfDistanceRecurse(root->children[1], callback, min_dist)) return true;
423 
424  if (distanceRecurse(root->children[0], root->children[1], callback, min_dist))
425  return true;
426 
427  return false;
428 }
429 
430 } // namespace dynamic_AABB_tree
431 
432 } // namespace detail
433 
434 //==============================================================================
436  tree_topdown_balance_threshold = &dtree.bu_threshold;
437  tree_topdown_level = &dtree.topdown_level;
441  *tree_topdown_level = 0;
442  tree_init_level = 0;
443  setup_ = false;
444 
445  // from experiment, this is the optimal setting
448 }
449 
450 //==============================================================================
452  const std::vector<CollisionObject*>& other_objs) {
453  if (other_objs.empty()) return;
454 
455  if (size() > 0) {
457  } else {
458  std::vector<DynamicAABBNode*> leaves(other_objs.size());
459  table.rehash(other_objs.size());
460  for (size_t i = 0, size = other_objs.size(); i < size; ++i) {
461  DynamicAABBNode* node =
462  new DynamicAABBNode; // node will be managed by the dtree
463  node->bv = other_objs[i]->getAABB();
464  node->parent = nullptr;
465  node->children[1] = nullptr;
466  node->data = other_objs[i];
467  table[other_objs[i]] = node;
468  leaves[i] = node;
469  }
470 
471  dtree.init(leaves, tree_init_level);
472 
473  setup_ = true;
474  }
475 }
476 
477 //==============================================================================
479  DynamicAABBNode* node = dtree.insert(obj->getAABB(), obj);
480  table[obj] = node;
481 }
482 
483 //==============================================================================
485  DynamicAABBNode* node = table[obj];
486  table.erase(obj);
487  dtree.remove(node);
488 }
489 
490 //==============================================================================
492  if (!setup_) {
493  size_t num = dtree.size();
494  if (num == 0) {
495  setup_ = true;
496  return;
497  }
498 
499  size_t height = dtree.getMaxHeight();
500 
501  if (((FCL_REAL)height - std::log((FCL_REAL)num) / std::log(2.0)) <
503  dtree.balanceIncremental(tree_incremental_balance_pass);
504  else
505  dtree.balanceTopdown();
506 
507  setup_ = true;
508  }
509 }
510 
511 //==============================================================================
513  for (auto it = table.cbegin(); it != table.cend(); ++it) {
514  CollisionObject* obj = it->first;
515  DynamicAABBNode* node = it->second;
516  node->bv = obj->getAABB();
517  if (node->bv.volume() <= 0.)
518  throw std::invalid_argument("The bounding volume has a negative volume.");
519  }
520 
521  dtree.refit();
522  setup_ = false;
523 
524  setup();
525 }
526 
527 //==============================================================================
529  const auto it = table.find(updated_obj);
530  if (it != table.end()) {
531  DynamicAABBNode* node = it->second;
532  if (!(node->bv == updated_obj->getAABB()))
533  dtree.update(node, updated_obj->getAABB());
534  }
535  setup_ = false;
536 }
537 
538 //==============================================================================
540  update_(updated_obj);
541  setup();
542 }
543 
544 //==============================================================================
546  const std::vector<CollisionObject*>& updated_objs) {
547  for (size_t i = 0, size = updated_objs.size(); i < size; ++i)
548  update_(updated_objs[i]);
549  setup();
550 }
551 
552 //==============================================================================
554  dtree.clear();
555  table.clear();
556 }
557 
558 //==============================================================================
560  std::vector<CollisionObject*>& objs) const {
561  objs.resize(this->size());
563  table.begin(), table.end(), objs.begin(),
564  std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
565 }
566 
567 //==============================================================================
570  callback->init();
571  if (size() == 0) return;
572  switch (obj->collisionGeometry()->getNodeType()) {
573 #if HPP_FCL_HAVE_OCTOMAP
574  case GEOM_OCTREE: {
576  const OcTree* octree =
577  static_cast<const OcTree*>(obj->collisionGeometryPtr());
579  dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(),
580  obj->getTransform(), callback);
581  } else
583  callback);
584  } break;
585 #endif
586  default:
588  callback);
589  }
590 }
591 
592 //==============================================================================
595  callback->init();
596  if (size() == 0) return;
597  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
598  switch (obj->collisionGeometry()->getNodeType()) {
599 #if HPP_FCL_HAVE_OCTOMAP
600  case GEOM_OCTREE: {
602  const OcTree* octree =
603  static_cast<const OcTree*>(obj->collisionGeometryPtr());
605  dtree.getRoot(), octree, octree->getRoot(), octree->getRootBV(),
606  obj->getTransform(), callback, min_dist);
607  } else
609  callback, min_dist);
610  } break;
611 #endif
612  default:
614  min_dist);
615  }
616 }
617 
618 //==============================================================================
621  callback->init();
622  if (size() == 0) return;
624 }
625 
626 //==============================================================================
629  callback->init();
630  if (size() == 0) return;
631  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
633  min_dist);
634 }
635 
636 //==============================================================================
638  BroadPhaseCollisionManager* other_manager_,
640  callback->init();
641  DynamicAABBTreeCollisionManager* other_manager =
642  static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
643  if ((size() == 0) || (other_manager->size() == 0)) return;
645  dtree.getRoot(), other_manager->dtree.getRoot(), callback);
646 }
647 
648 //==============================================================================
650  BroadPhaseCollisionManager* other_manager_,
652  callback->init();
653  DynamicAABBTreeCollisionManager* other_manager =
654  static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
655  if ((size() == 0) || (other_manager->size() == 0)) return;
656  FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
658  dtree.getRoot(), other_manager->dtree.getRoot(), callback, min_dist);
659 }
660 
661 //==============================================================================
662 bool DynamicAABBTreeCollisionManager::empty() const { return dtree.empty(); }
663 
664 //==============================================================================
665 size_t DynamicAABBTreeCollisionManager::size() const { return dtree.size(); }
666 
667 //==============================================================================
669  const {
670  return dtree;
671 }
672 
674  return dtree;
675 }
676 
677 } // namespace fcl
678 } // namespace hpp
hpp::fcl::detail::NodeBase::data
void * data
Definition: node_base.h:66
hpp::fcl::detail::NodeBase::parent
NodeBase< BV > * parent
pointer to parent node
Definition: node_base.h:55
hpp::fcl::detail::dynamic_AABB_tree::collisionRecurse
bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode *root1, DynamicAABBTreeCollisionManager::DynamicAABBNode *root2, CollisionCallBackBase *callback)
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:252
hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBNode
detail::NodeBase< AABB > DynamicAABBNode
Definition: broadphase_dynamic_AABB_tree.h:60
hpp::fcl::transform
HPP_FCL_DLLAPI Halfspace transform(const Halfspace &a, const Transform3f &tf)
Definition: geometric_shapes_utility.cpp:248
geometric_shapes.d1
float d1
Definition: scripts/geometric_shapes.py:31
hpp::fcl::DynamicAABBTreeCollisionManager::dtree
detail::HierarchyTree< AABB > dtree
Definition: broadphase_dynamic_AABB_tree.h:139
collision_manager.callback
callback
Definition: collision_manager.py:27
collision_manager.box
box
Definition: collision_manager.py:11
hpp::fcl::DynamicAABBTreeCollisionManager::collide
void collide(CollisionObject *obj, CollisionCallBackBase *callback) const
perform collision test between one object and all the objects belonging to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:568
hpp::fcl::DynamicAABBTreeCollisionManager::getTree
const detail::HierarchyTree< AABB > & getTree() const
returns the AABB tree structure.
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:668
hpp::fcl::detail::NodeBase::isLeaf
bool isLeaf() const
whether is a leaf
Definition: node_base-inl.h:55
hpp::fcl::DynamicAABBTreeCollisionManager::update_
void update_(CollisionObject *updated_obj)
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:528
hpp::fcl::DynamicAABBTreeCollisionManager::update
virtual void update()
update the condition of manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:512
hpp::fcl::DynamicAABBTreeCollisionManager::distance
void distance(CollisionObject *obj, DistanceCallBackBase *callback) const
perform distance computation between one object and all the objects belonging to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:593
hpp::fcl::detail::dynamic_AABB_tree::distanceRecurse
bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode *root1, DynamicAABBTreeCollisionManager::DynamicAABBNode *root2, DistanceCallBackBase *callback, FCL_REAL &min_dist)
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:313
hpp::fcl::DynamicAABBTreeCollisionManager::tree_init_level
int tree_init_level
Definition: broadphase_dynamic_AABB_tree.h:68
hpp::fcl::DynamicAABBTreeCollisionManager::tree_topdown_balance_threshold
int * tree_topdown_balance_threshold
Definition: broadphase_dynamic_AABB_tree.h:66
hpp::fcl::DynamicAABBTreeCollisionManager::registerObject
void registerObject(CollisionObject *obj)
add one object to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:478
hpp::fcl::CollisionObject::collisionGeometryPtr
const CollisionGeometry * collisionGeometryPtr() const
get raw pointer to collision geometry of the object instance
Definition: collision_object.h:312
hpp::fcl::detail::HierarchyTree
Class for hierarchy tree structure.
Definition: hierarchy_tree.h:57
octree
Definition: octree.py:1
hpp::fcl::BroadPhaseCollisionManager
Base class for broad phase collision. It helps to accelerate the collision/distance between N objects...
Definition: broadphase_collision_manager.h:54
hpp::fcl::DynamicAABBTreeCollisionManager::max_tree_nonbalanced_level
int max_tree_nonbalanced_level
Definition: broadphase_dynamic_AABB_tree.h:64
geometric_shapes_utility.h
hpp::fcl::DynamicAABBTreeCollisionManager
Definition: broadphase_dynamic_AABB_tree.h:54
hpp::fcl::OcTree::OcTreeNode
octomap::OcTreeNode OcTreeNode
Definition: octree.h:63
hpp::fcl::detail::NodeBase::bv
BV bv
the bounding volume for the node
Definition: node_base.h:52
hpp::fcl::DynamicAABBTreeCollisionManager::setup
void setup()
initialize the manager, related with the specific type of manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:491
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:65
hpp::fcl::DynamicAABBTreeCollisionManager::clear
void clear()
clear the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:553
hpp::fcl::DistanceCallBackBase
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
Definition: broadphase_callbacks.h:73
hpp::fcl::computeChildBV
static void computeChildBV(const AABB &root_bv, unsigned int i, AABB &child_bv)
compute the bounding volume of an octree node's i-th child
Definition: octree.h:239
hpp::fcl::DynamicAABBTreeCollisionManager::table
std::unordered_map< CollisionObject *, DynamicAABBNode * > table
Definition: broadphase_dynamic_AABB_tree.h:140
hpp::fcl::DynamicAABBTreeCollisionManager::unregisterObject
void unregisterObject(CollisionObject *obj)
remove one object from the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:484
hpp::fcl::DynamicAABBTreeCollisionManager::tree_incremental_balance_pass
int tree_incremental_balance_pass
Definition: broadphase_dynamic_AABB_tree.h:65
hpp::fcl::DynamicAABBTreeCollisionManager::octree_as_geometry_collide
bool octree_as_geometry_collide
Definition: broadphase_dynamic_AABB_tree.h:70
hpp::fcl::convertBV
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::BroadPhaseCollisionManager::getObjects
virtual std::vector< CollisionObject * > getObjects() const
return the objects managed by the manager
Definition: broadphase_collision_manager.h:88
hpp::fcl::constructBox
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
Definition: geometric_shapes_utility.cpp:911
hpp::fcl::CollisionObject::getTransform
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:283
BV.h
hpp
Main namespace.
Definition: broadphase_bruteforce.h:44
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
hpp::fcl::AABB::distance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
Definition: AABB.cpp:114
gjk.tf2
tuple tf2
Definition: test/scripts/gjk.py:36
hpp::fcl::GEOM_OCTREE
@ GEOM_OCTREE
Definition: collision_object.h:84
hpp::fcl::detail::NodeBase
dynamic AABB tree node
Definition: node_base.h:50
hpp::fcl::CollisionObject::collisionGeometry
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get shared pointer to collision geometry of the object instance
Definition: collision_object.h:304
hpp::fcl::Transform3f::Identity
static Transform3f Identity()
Definition: transform.h:67
hpp::fcl::detail::dynamic_AABB_tree::selfDistanceRecurse
bool selfDistanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode *root, DistanceCallBackBase *callback, FCL_REAL &min_dist)
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:416
hpp::fcl::DynamicAABBTreeCollisionManager::empty
bool empty() const
whether the manager is empty
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:662
broadphase_dynamic_AABB_tree.h
hpp::fcl::detail::NodeBase::children
NodeBase< BV > * children[2]
for leaf node, children nodes
Definition: node_base.h:65
hpp::fcl::DynamicAABBTreeCollisionManager::registerObjects
void registerObjects(const std::vector< CollisionObject * > &other_objs)
add objects to the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:451
hpp::fcl::CollisionObject::getAABB
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
hpp::fcl::CollisionCallBackBase
Base callback class for collision queries. This class can be supersed by child classes to provide des...
Definition: broadphase_callbacks.h:50
hpp::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:953
hpp::fcl::OcTree
Octree is one type of collision geometry which can encode uncertainty information in the sensor data.
Definition: octree.h:53
hpp::fcl::DynamicAABBTreeCollisionManager::setup_
bool setup_
Definition: broadphase_dynamic_AABB_tree.h:142
octree.h
hpp::fcl::BroadPhaseCollisionManager::registerObjects
virtual void registerObjects(const std::vector< CollisionObject * > &other_objs)
add objects to the manager
Definition: broadphase_collision_manager.cpp:55
hpp::fcl::DynamicAABBTreeCollisionManager::octree_as_geometry_distance
bool octree_as_geometry_distance
Definition: broadphase_dynamic_AABB_tree.h:71
hpp::fcl::DynamicAABBTreeCollisionManager::DynamicAABBTreeCollisionManager
DynamicAABBTreeCollisionManager()
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:435
hpp::fcl::DynamicAABBTreeCollisionManager::size
size_t size() const
the number of objects managed by the manager
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:665
hpp::fcl::DynamicAABBTreeCollisionManager::tree_topdown_level
int * tree_topdown_level
Definition: broadphase_dynamic_AABB_tree.h:67
hpp::fcl::detail::dynamic_AABB_tree::selfCollisionRecurse
bool selfCollisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode *root, CollisionCallBackBase *callback)
Definition: src/broadphase/broadphase_dynamic_AABB_tree.cpp:297


hpp-fcl
Author(s):
autogenerated on Fri Aug 2 2024 02:45:12