42 #if HPP_FCL_HAVE_OCTOMAP
53 namespace dynamic_AABB_tree {
55 #if HPP_FCL_HAVE_OCTOMAP
59 const AABB& root2_bv,
const Transform3f&
tf2,
62 if (root1->isLeaf()) {
63 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
65 if (!obj1->collisionGeometry()->isFree()) {
70 if (obb1.overlap(obb2)) {
75 box->cost_density = tree2->getDefaultOccupancy();
77 CollisionObject obj2(shared_ptr<CollisionGeometry>(
box), box_tf);
82 if (collisionRecurse_(root1->children[0], tree2,
nullptr, root2_bv,
tf2,
85 if (collisionRecurse_(root1->children[1], tree2,
nullptr, root2_bv,
tf2,
91 }
else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
92 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
94 if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
99 if (obb1.overlap(obb2)) {
100 Box*
box =
new Box();
104 box->cost_density = root2->getOccupancy();
105 box->threshold_occupied = tree2->getOccupancyThres();
107 CollisionObject obj2(shared_ptr<CollisionGeometry>(
box), box_tf);
119 if (tree2->isNodeFree(root2) || !obb1.overlap(obb2))
return false;
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,
126 if (collisionRecurse_(root1->children[1], tree2, root2, root2_bv,
tf2,
130 for (
unsigned int i = 0; i < 8; ++i) {
131 if (tree2->nodeChildExists(root2, i)) {
136 if (collisionRecurse_(root1, tree2, child, child_bv,
tf2,
callback))
141 if (collisionRecurse_(root1, tree2,
nullptr, child_bv,
tf2,
callback))
152 const AABB& root2_bv,
const Transform3f&
tf2,
154 if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
155 if (tree2->isNodeOccupied(root2)) {
156 Box*
box =
new Box();
159 CollisionObject obj(shared_ptr<CollisionGeometry>(
box), box_tf);
160 return (*
callback)(
static_cast<CollisionObject*
>(root1->data), &obj,
166 if (!tree2->isNodeOccupied(root2))
return false;
168 if (!tree2->nodeHasChildren(root2) ||
169 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
173 FCL_REAL d1 = aabb2.distance(root1->children[0]->bv);
174 FCL_REAL d2 = aabb2.distance(root1->children[1]->bv);
178 if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
tf2,
184 if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
tf2,
190 if (distanceRecurse_(root1->children[0], tree2, root2, root2_bv,
tf2,
196 if (distanceRecurse_(root1->children[1], tree2, root2, root2_bv,
tf2,
202 for (
unsigned int i = 0; i < 8; ++i) {
203 if (tree2->nodeChildExists(root2, i)) {
210 FCL_REAL d = root1->bv.distance(aabb2);
213 if (distanceRecurse_(root1, tree2, child, child_bv,
tf2,
callback,
227 const AABB& root2_bv,
const Transform3f&
tf2,
229 if (
tf2.rotation().isIdentity())
230 return collisionRecurse_(root1, tree2, root2, root2_bv,
tf2.translation(),
233 return collisionRecurse_(root1, tree2, root2, root2_bv,
tf2,
callback);
239 const AABB& root2_bv,
const Transform3f&
tf2,
241 if (
tf2.rotation().isIdentity())
242 return distanceRecurse_(root1, tree2, root2, root2_bv,
tf2.translation(),
245 return distanceRecurse_(root1, tree2, root2, root2_bv,
tf2,
callback,
256 if (!root1->
bv.overlap(root2->
bv))
return false;
261 if (!root1->
bv.overlap(root2->
bv))
return false;
264 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
278 if (!root->
bv.overlap(query->
getAABB()))
return false;
282 if (!root->
bv.overlap(query->
getAABB()))
return false;
300 if (root->
isLeaf())
return false;
319 return (*
callback)(root1_obj, root2_obj, min_dist);
323 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
384 return (*
callback)(root_obj, query, min_dist);
418 if (root->
isLeaf())
return false;
452 const std::vector<CollisionObject*>& other_objs) {
453 if (other_objs.empty())
return;
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) {
463 node->
bv = other_objs[i]->getAABB();
466 node->
data = other_objs[i];
467 table[other_objs[i]] = node;
493 size_t num =
dtree.size();
499 size_t height =
dtree.getMaxHeight();
505 dtree.balanceTopdown();
513 for (
auto it =
table.cbegin(); it !=
table.cend(); ++it) {
517 if (node->
bv.volume() <= 0.)
518 throw std::invalid_argument(
"The bounding volume has a negative volume.");
529 const auto it =
table.find(updated_obj);
530 if (it !=
table.end()) {
532 if (!(node->
bv == updated_obj->
getAABB()))
546 const std::vector<CollisionObject*>& updated_objs) {
547 for (
size_t i = 0,
size = updated_objs.size(); i <
size; ++i)
560 std::vector<CollisionObject*>& objs)
const {
561 objs.resize(this->
size());
564 std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
571 if (
size() == 0)
return;
573 #if HPP_FCL_HAVE_OCTOMAP
596 if (
size() == 0)
return;
597 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
599 #if HPP_FCL_HAVE_OCTOMAP
622 if (
size() == 0)
return;
630 if (
size() == 0)
return;
631 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
643 if ((
size() == 0) || (other_manager->
size() == 0))
return;
655 if ((
size() == 0) || (other_manager->
size() == 0))
return;
656 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();