38 #ifndef HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H 39 #define HPP_FCL_BROAD_PHASE_DYNAMIC_AABB_TREE_ARRAY_INL_H 44 #if HPP_FCL_HAVE_OCTOMAP 51 namespace dynamic_AABB_tree_array {
53 #if HPP_FCL_HAVE_OCTOMAP 56 template <
typename Derived>
57 bool collisionRecurse_(
60 const AABB& root2_bv,
const Eigen::MatrixBase<Derived>& translation2,
70 if (root1->
bv.overlap(root_bv_t)) {
80 return (*callback)(obj1, &obj2);
84 if (collisionRecurse_(nodes1, root1->
children[0], tree2,
nullptr,
85 root2_bv, translation2, callback))
87 if (collisionRecurse_(nodes1, root1->
children[1], tree2,
nullptr,
88 root2_bv, translation2, callback))
97 if (root1->
bv.overlap(root_bv_t)) {
108 return (*callback)(obj1, &obj2);
116 if (tree2->
isNodeFree(root2) || !root1->
bv.overlap(root_bv_t))
return false;
119 (!root1->
isLeaf() && (root1->
bv.size() > root2_bv.
size()))) {
120 if (collisionRecurse_(nodes1, root1->
children[0], tree2, root2, root2_bv,
121 translation2, callback))
123 if (collisionRecurse_(nodes1, root1->
children[1], tree2, root2, root2_bv,
124 translation2, callback))
127 for (
unsigned int i = 0; i < 8; ++i) {
133 if (collisionRecurse_(nodes1, root1_id, tree2, child, child_bv,
134 translation2, callback))
139 if (collisionRecurse_(nodes1, root1_id, tree2,
nullptr, child_bv,
140 translation2, callback))
150 template <
typename Derived>
151 bool distanceRecurse_(
154 const AABB& root2_bv,
const Eigen::MatrixBase<Derived>& translation2,
175 (!root1->
isLeaf() && (root1->
bv.size() > root2_bv.
size()))) {
183 if (distanceRecurse_(nodes1, root1->
children[1], tree2, root2, root2_bv,
184 translation2, callback, min_dist))
189 if (distanceRecurse_(nodes1, root1->
children[0], tree2, root2, root2_bv,
190 translation2, callback, min_dist))
195 if (distanceRecurse_(nodes1, root1->
children[0], tree2, root2, root2_bv,
196 translation2, callback, min_dist))
201 if (distanceRecurse_(nodes1, root1->
children[1], tree2, root2, root2_bv,
202 translation2, callback, min_dist))
207 for (
unsigned int i = 0; i < 8; ++i) {
217 if (distanceRecurse_(nodes1, root1_id, tree2, child, child_bv,
218 translation2, callback, min_dist))
bool isNodeFree(const OcTreeNode *node) const
whether one node is completely free
FCL_REAL cost_density
collision cost for unit volume
FCL_REAL getOccupancyThres() const
the threshold used to decide whether one node is occupied, this is NOT the octree occupied_thresold ...
double getOccupancy() const
FCL_REAL getDefaultOccupancy() const
Base callback class for collision queries. This class can be supersed by child classes to provide des...
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
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
FCL_REAL size() const
Size of the AABB (used in BV_Splitter to order two AABBs)
Octree is one type of collision geometry which can encode uncertainty information in the sensor data...
Center at zero point, axis aligned box.
OcTreeNode * getNodeChild(OcTreeNode *node, unsigned int childIdx)
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get geometry from the object instance
FCL_REAL distance(const AABB &other) const
Distance between two AABBs.
the object for collision or distance computation, contains the geometry and the transform information...
HPP_FCL_DLLAPI void constructBox(const AABB &bv, Box &box, Transform3f &tf)
construct a box shape (with a configuration) from a given bounding volume
bool isNodeOccupied(const OcTreeNode *node) const
whether one node is completely occupied
Base callback class for distance queries. This class can be supersed by child classes to provide desi...
bool nodeChildExists(const OcTreeNode *node, unsigned int childIdx) const
return true if the child at childIdx exists
bool nodeHasChildren(const OcTreeNode *node) const
return true if node has at least one child