diff --git a/trunk/fcl/include/fcl/octomap_extension.h b/trunk/fcl/include/fcl/octomap_extension.h
index fa52446a1f1ddcea237be2f6b15bff28198482c1..9ab327e639895cae01088e010ef44544e03d20e8 100644
--- a/trunk/fcl/include/fcl/octomap_extension.h
+++ b/trunk/fcl/include/fcl/octomap_extension.h
@@ -71,6 +71,8 @@ typedef bool (*CollisionCostOctomapCallBackExt)(CollisionObject* o1, CollisionOb
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback);
+void distance(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, DistanceCallBack callback);
+
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback);
FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes);
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 5e8fbb8a32d542a688f5e0abb78c74249c2a378f..fbacd9745107aabfe75c9fea31cb874852e82bdd 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -81,6 +81,9 @@ bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cda
if(cdata->min_distance > d) cdata->min_distance = d;
dist = cdata->min_distance;
+
+ if(dist <= 0) return true; // in collision or in touch
+
return cdata->done;
}
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
index 81b9cfc3e49aee8b311733f1f5328899ea82b346..9ee15c6aa0e1918e1e85fe4bfacd631ae0e2a492 100644
--- a/trunk/fcl/src/octomap_extension.cpp
+++ b/trunk/fcl/src/octomap_extension.cpp
@@ -40,6 +40,42 @@
namespace fcl
{
+static inline void computeChildBV(const AABB& root_bv, unsigned int i, AABB& child_bv)
+{
+ if(i&1)
+ {
+ child_bv.min_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+ child_bv.max_[0] = root_bv.max_[0];
+ }
+ else
+ {
+ child_bv.min_[0] = root_bv.min_[0];
+ child_bv.max_[0] = (root_bv.min_[0] + root_bv.max_[0]) * 0.5;
+ }
+
+ if(i&2)
+ {
+ child_bv.min_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+ child_bv.max_[1] = root_bv.max_[1];
+ }
+ else
+ {
+ child_bv.min_[1] = root_bv.min_[1];
+ child_bv.max_[1] = (root_bv.min_[1] + root_bv.max_[1]) * 0.5;
+ }
+
+ if(i&4)
+ {
+ child_bv.min_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+ child_bv.max_[2] = root_bv.max_[2];
+ }
+ else
+ {
+ child_bv.min_[2] = root_bv.min_[2];
+ child_bv.max_[2] = (root_bv.min_[2] + root_bv.max_[2]) * 0.5;
+ }
+}
+
class ExtendedBox : public Box
{
public:
@@ -91,38 +127,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1)
- {
- child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- child_bv.max_[0] = root2_bv.max_[0];
- }
- else
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
-
- if(i&2)
- {
- child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- child_bv.max_[1] = root2_bv.max_[1];
- }
- else
- {
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- }
-
- if(i&4)
- {
- child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- child_bv.max_[2] = root2_bv.max_[2];
- }
- else
- {
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- }
+ computeChildBV(root2_bv, i, child_bv);
if(collisionRecurse(root1, tree2, child, child_bv, cdata, callback))
return true;
@@ -133,6 +138,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
return false;
}
+
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
@@ -145,6 +151,79 @@ void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree,
}
+bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
+ octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
+ void* cdata, DistanceCallBack callback, FCL_REAL& min_dist)
+{
+ if(root1->isLeaf() && !root2->hasChildren())
+ {
+ if(tree2->isNodeOccupied(root2))
+ {
+ Box* box = new Box(root2_bv.max_[0] - root2_bv.min_[0],
+ root2_bv.max_[1] - root2_bv.min_[1],
+ root2_bv.max_[2] - root2_bv.min_[2]);
+ CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
+ return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
+ }
+ else return false;
+ }
+
+ if(!tree2->isNodeOccupied(root2)) return false;
+
+ if(!root2->hasChildren() || (!root1->isLeaf() && (root1->bv.size() > root2_bv.size())))
+ {
+ FCL_REAL d1 = root2_bv.distance(root1->childs[0]->bv);
+ FCL_REAL d2 = root2_bv.distance(root1->childs[1]->bv);
+
+ if(d2 < d1)
+ {
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ else
+ {
+ if(d1 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[0], tree2, root2, root2_bv, cdata, callback, min_dist))
+ return true;
+ }
+
+ if(d2 < min_dist)
+ {
+ if(distanceRecurse(root1->childs[1], tree2, root2, root2_bv, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+ else
+ {
+ for(unsigned int i = 0; i < 8; ++i)
+ {
+ if(root2->childExists(i))
+ {
+ octomap::OcTreeNode* child = root2->getChild(i);
+ AABB child_bv;
+ computeChildBV(root2_bv, i, child_bv);
+
+ if(distanceRecurse(root1, tree2, child, child_bv, cdata, callback, min_dist))
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+
bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
void* cdata, CollisionCostOctomapCallBack callback, FCL_REAL& cost)
@@ -184,38 +263,7 @@ bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1)
- {
- child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- child_bv.max_[0] = root2_bv.max_[0];
- }
- else
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
-
- if(i&2)
- {
- child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- child_bv.max_[1] = root2_bv.max_[1];
- }
- else
- {
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- }
-
- if(i&4)
- {
- child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- child_bv.max_[2] = root2_bv.max_[2];
- }
- else
- {
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- }
+ computeChildBV(root2_bv, i, child_bv);
if(collisionCostRecurse(root1, tree2, child, child_bv, cdata, callback, cost))
return true;
@@ -265,38 +313,7 @@ bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* r
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1)
- {
- child_bv.min_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- child_bv.max_[0] = root2_bv.max_[0];
- }
- else
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
-
- if(i&2)
- {
- child_bv.min_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- child_bv.max_[1] = root2_bv.max_[1];
- }
- else
- {
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
- }
-
- if(i&4)
- {
- child_bv.min_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- child_bv.max_[2] = root2_bv.max_[2];
- }
- else
- {
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
- }
+ computeChildBV(root2_bv, i, child_bv);
if(collisionCostExtRecurse(root1, tree2, child, child_bv, cdata, callback, cost, nodes))
return true;