40 #if HPP_FCL_HAVE_OCTOMAP 46 namespace dynamic_AABB_tree_array {
48 #if HPP_FCL_HAVE_OCTOMAP 51 bool collisionRecurse_(
54 const AABB& root2_bv,
const Transform3f&
tf2,
59 if (root1->isLeaf()) {
60 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
62 if (!obj1->collisionGeometry()->isFree()) {
67 if (obb1.overlap(obb2)) {
72 box->cost_density = tree2->getDefaultOccupancy();
74 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
75 return (*callback)(obj1, &obj2);
79 if (collisionRecurse_(nodes1, root1->children[0], tree2,
nullptr,
80 root2_bv, tf2, callback))
82 if (collisionRecurse_(nodes1, root1->children[1], tree2,
nullptr,
83 root2_bv, tf2, callback))
88 }
else if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
89 CollisionObject* obj1 =
static_cast<CollisionObject*
>(root1->data);
90 if (!tree2->isNodeFree(root2) && !obj1->collisionGeometry()->isFree()) {
95 if (obb1.overlap(obb2)) {
100 box->cost_density = root2->getOccupancy();
101 box->threshold_occupied = tree2->getOccupancyThres();
103 CollisionObject obj2(shared_ptr<CollisionGeometry>(box), box_tf);
104 return (*callback)(obj1, &obj2);
115 if (tree2->isNodeFree(root2) || !obb1.overlap(obb2))
return false;
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,
122 if (collisionRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
126 for (
unsigned int i = 0; i < 8; ++i) {
127 if (tree2->nodeChildExists(root2, i)) {
132 if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
138 if (collisionRecurse_(nodes1, root1_id, tree2,
nullptr, child_bv, tf2,
149 bool distanceRecurse_(
152 const AABB& root2_bv,
const Transform3f& tf2,
153 DistanceCallBackBase* callback,
FCL_REAL& min_dist) {
156 if (root1->isLeaf() && !tree2->nodeHasChildren(root2)) {
157 if (tree2->isNodeOccupied(root2)) {
158 Box* box =
new Box();
161 CollisionObject obj(shared_ptr<CollisionGeometry>(box), box_tf);
162 return (*callback)(
static_cast<CollisionObject*
>(root1->data), &obj,
168 if (!tree2->isNodeOccupied(root2))
return false;
170 if (!tree2->nodeHasChildren(root2) ||
171 (!root1->isLeaf() && (root1->bv.size() > root2_bv.size()))) {
175 FCL_REAL d1 = aabb2.distance((nodes1 + root1->children[0])->bv);
176 FCL_REAL d2 = aabb2.distance((nodes1 + root1->children[1])->bv);
180 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
181 tf2, callback, min_dist))
186 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
187 tf2, callback, min_dist))
192 if (distanceRecurse_(nodes1, root1->children[0], tree2, root2, root2_bv,
193 tf2, callback, min_dist))
198 if (distanceRecurse_(nodes1, root1->children[1], tree2, root2, root2_bv,
199 tf2, callback, min_dist))
204 for (
unsigned int i = 0; i < 8; ++i) {
205 if (tree2->nodeChildExists(root2, i)) {
212 FCL_REAL d = root1->bv.distance(aabb2);
215 if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv, tf2,
239 if (!root1->
bv.overlap(root2->
bv))
return false;
241 static_cast<CollisionObject*>(root2->
data));
244 if (!root1->
bv.overlap(root2->
bv))
return false;
247 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
270 if (!root->
bv.overlap(query->
getAABB()))
return false;
293 if (root->
isLeaf())
return false;
319 return (*callback)(root1_obj, root2_obj, min_dist);
323 (!root1->
isLeaf() && (root1->
bv.size() > root2->
bv.size()))) {
394 return (*callback)(root_obj, query, min_dist);
430 if (root->
isLeaf())
return false;
445 #if HPP_FCL_HAVE_OCTOMAP 454 return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
457 return collisionRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
468 return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv,
471 return distanceRecurse_(nodes1, root1_id, tree2, root2, root2_bv, tf2,
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;
493 octree_as_geometry_collide =
true;
494 octree_as_geometry_distance =
false;
499 const std::vector<CollisionObject*>& other_objs) {
500 if (other_objs.empty())
return;
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;
515 int n_leaves = (int)other_objs.size();
517 dtree.init(leaves, n_leaves, tree_init_level);
526 size_t node = dtree.insert(obj->
getAABB(), obj);
533 size_t node = table[obj];
541 int num = (int)dtree.size();
547 int height = (int)dtree.getMaxHeight();
550 max_tree_nonbalanced_level)
551 dtree.balanceIncremental(tree_incremental_balance_pass);
553 dtree.balanceTopdown();
561 for (
auto it = table.cbegin(), end = table.cend(); it != end; ++it) {
563 size_t node = it->second;
564 dtree.getNodes()[node].bv = obj->
getAABB();
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());
588 update_(updated_obj);
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]);
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));
619 if (size() == 0)
return;
621 #if HPP_FCL_HAVE_OCTOMAP 623 if (!octree_as_geometry_collide) {
627 dtree.getNodes(), dtree.getRoot(), octree, octree->
getRoot(),
631 dtree.getNodes(), dtree.getRoot(), obj,
callback);
636 dtree.getNodes(), dtree.getRoot(), obj,
callback);
644 if (size() == 0)
return;
645 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
647 #if HPP_FCL_HAVE_OCTOMAP 649 if (!octree_as_geometry_distance) {
653 dtree.getNodes(), dtree.getRoot(), octree, octree->
getRoot(),
657 dtree.getNodes(), dtree.getRoot(), obj,
callback, min_dist);
662 dtree.getNodes(), dtree.getRoot(), obj,
callback, min_dist);
670 if (size() == 0)
return;
672 dtree.getNodes(), dtree.getRoot(),
callback);
679 if (size() == 0)
return;
680 FCL_REAL min_dist = (std::numeric_limits<FCL_REAL>::max)();
682 dtree.getNodes(), dtree.getRoot(),
callback, min_dist);
692 if ((size() == 0) || (other_manager->
size() == 0))
return;
694 dtree.getNodes(), dtree.getRoot(), other_manager->
dtree.getNodes(),
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(),
714 return dtree.empty();
virtual void init()
Initialization of the callback before running the collision broadphase manager.
bool selfCollisionRecurse(DynamicAABBTreeArrayCollisionManager::DynamicAABBNode *nodes, size_t root_id, CollisionCallBackBase *callback)
OcTreeNode * getRoot() const
get the root node of the octree
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
virtual void registerObjects(const std::vector< CollisionObject *> &other_objs)
add objects to the manager
void update_(CollisionObject *updated_obj)
Base callback class for collision queries. This class can be supersed by child classes to provide des...
size_t size() const
the number of objects managed by the manager
AABB getRootBV() const
get the bounding volume for the root
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's i-th child
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's transform
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Class for hierarchy tree structure.
octomap::OcTreeNode OcTreeNode
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...
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.
const detail::implementation_array::HierarchyTree< AABB > & getTree() const
const AABB & getAABB() const
get the AABB in world space
detail::implementation_array::HierarchyTree< AABB > dtree
virtual void update()
update the condition of manager
DynamicAABBTreeArrayCollisionManager()
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...
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...
void clear()
clear the manager
bool empty() const
whether the manager is empty