40 #ifdef COAL_HAVE_OCTOMAP
45 namespace dynamic_AABB_tree_array {
50 bool collisionRecurse_(
53 const AABB& root2_bv,
const Transform3s&
tf2,
58 if (root1->isLeaf()) {
59 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
61 if (!obj1->collisionGeometry()->isFree()) {
66 if (obb1.overlap(obb2)) {
71 box->cost_density = tree2->getDefaultOccupancy();
73 CollisionObject obj2(shared_ptr<CollisionGeometry>(
box), box_tf);
78 if (collisionRecurse_(nodes1, root1->children[0], tree2,
nullptr,
81 if (collisionRecurse_(nodes1, root1->children[1], tree2,
nullptr,
87 }
else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
88 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
89 if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
94 if (obb1.overlap(obb2)) {
99 box->cost_density = root2->getOccupancy();
100 box->threshold_occupied = tree2->getOccupancyThres();
102 CollisionObject obj2(shared_ptr<CollisionGeometry>(
box), box_tf);
114 if (tree2->isNodeFree(root2) || !obb1.overlap(obb2))
return false;
116 if (!tree2->nodeHasChildren(root2) ||
117 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
118 if (collisionRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
121 if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
125 for (
unsigned int i = 0; i < 8; ++i) {
126 if (tree2->nodeChildExists(root2, i)) {
131 if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
tf2,
137 if (collisionRecurse_(nodes1, root1_id, tree2,
nullptr, child_bv,
tf2,
148 bool distanceRecurse_(
151 const AABB& root2_bv,
const Transform3s&
tf2,
155 if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
156 if (tree2->isNodeOccupied(root2)) {
157 Box*
box =
new Box();
160 CollisionObject obj(shared_ptr<CollisionGeometry>(
box), box_tf);
161 return (*
callback)(
static_cast<CollisionObject*
>(root1->data), &obj,
167 if (!tree2->isNodeOccupied(root2))
return false;
169 if (!tree2->nodeHasChildren(root2) ||
170 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
174 CoalScalar d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
175 CoalScalar d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
179 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
185 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
191 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
197 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
203 for (
unsigned int i = 0; i < 8; ++i) {
204 if (tree2->nodeChildExists(root2, i)) {
214 if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
tf2,
238 if (!root1->
bv.overlap(root2->
bv))
return false;
243 if (!root1->
bv.overlap(root2->
bv))
return false;
246 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
269 if (!root->
bv.overlap(query->
getAABB()))
return false;
292 if (root->
isLeaf())
return false;
318 return (*
callback)(root1_obj, root2_obj, min_dist);
322 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
393 return (*
callback)(root_obj, query, min_dist);
429 if (root->
isLeaf())
return false;
444 #if COAL_HAVE_OCTOMAP
452 if (
tf2.rotation().isIdentity())
453 return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
456 return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
tf2,
466 if (
tf2.rotation().isIdentity())
467 return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
470 return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
tf2,
498 const std::vector<CollisionObject*>& other_objs) {
499 if (other_objs.empty())
return;
505 table.rehash(other_objs.size());
506 for (
size_t i = 0,
size = other_objs.size(); i <
size; ++i) {
507 leaves[i].
bv = other_objs[i]->getAABB();
510 leaves[i].
data = other_objs[i];
511 table[other_objs[i]] = i;
514 int n_leaves = (int)other_objs.size();
532 size_t node =
table[obj];
540 int num = (int)
dtree.size();
546 int height = (int)
dtree.getMaxHeight();
552 dtree.balanceTopdown();
560 for (
auto it =
table.cbegin(), end =
table.cend(); it != end; ++it) {
562 size_t node = it->second;
575 const auto it =
table.find(updated_obj);
576 if (it !=
table.end()) {
577 size_t node = it->second;
578 if (!(
dtree.getNodes()[node].bv == updated_obj->
getAABB()))
593 const std::vector<CollisionObject*>& updated_objs) {
594 for (
size_t i = 0,
size = updated_objs.size(); i <
size; ++i)
607 std::vector<CollisionObject*>& objs)
const {
608 objs.resize(this->
size());
611 std::bind(&DynamicAABBTable::value_type::first, std::placeholders::_1));
618 if (
size() == 0)
return;
620 #if COAL_HAVE_OCTOMAP
643 if (
size() == 0)
return;
644 CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
646 #if COAL_HAVE_OCTOMAP
669 if (
size() == 0)
return;
678 if (
size() == 0)
return;
679 CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
691 if ((
size() == 0) || (other_manager->
size() == 0))
return;
704 if ((
size() == 0) || (other_manager->
size() == 0))
return;
705 CoalScalar min_dist = (std::numeric_limits<CoalScalar>::max)();
713 return dtree.empty();