diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 2757df853645123b7cf358e2c93fd208367825e3..4681fc8957aee7f1ec46e0a0e7c157a235f075b8 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -60,7 +60,10 @@ public:
class Triangle2 : public ShapeBase
{
public:
- Triangle2(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : a(a_), b(b_), c(c_) {}
+ Triangle2(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
+ {
+ computeLocalAABB();
+ }
void computeLocalAABB();
@@ -73,7 +76,10 @@ public:
class Box : public ShapeBase
{
public:
- Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z) {}
+ Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), side(x, y, z)
+ {
+ computeLocalAABB();
+ }
/** box side length */
Vec3f side;
@@ -89,7 +95,10 @@ public:
class Sphere : public ShapeBase
{
public:
- Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
+ Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
+ {
+ computeLocalAABB();
+ }
/** \brief Radius of the sphere */
FCL_REAL radius;
@@ -105,7 +114,10 @@ public:
class Capsule : public ShapeBase
{
public:
- Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ {
+ computeLocalAABB();
+ }
/** \brief Radius of capsule */
FCL_REAL radius;
@@ -124,7 +136,11 @@ public:
class Cone : public ShapeBase
{
public:
- Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ {
+ computeLocalAABB();
+ }
+
/** \brief Radius of the cone */
FCL_REAL radius;
@@ -143,7 +159,11 @@ public:
class Cylinder : public ShapeBase
{
public:
- Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+ Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_)
+ {
+ computeLocalAABB();
+ }
+
/** \brief Radius of the cylinder */
FCL_REAL radius;
@@ -186,6 +206,8 @@ public:
center = sum * (FCL_REAL)(1.0 / num_points);
fillEdges();
+
+ computeLocalAABB();
}
/** Copy constructor */
@@ -198,6 +220,8 @@ public:
polygons = other.polygons;
edges = new Edge[other.num_edges];
memcpy(edges, other.edges, sizeof(Edge) * num_edges);
+
+ computeLocalAABB();
}
~Convex()
@@ -245,12 +269,19 @@ class Plane : public ShapeBase
{
public:
/** \brief Construct a plane with normal direction and offset */
- Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
+ Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
+ {
+ unitNormalTest();
+
+ computeLocalAABB();
+ }
/** \brief Construct a plane with normal direction and offset */
Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : n(a, b, c), d(d_)
{
unitNormalTest();
+
+ computeLocalAABB();
}
/** \brief Compute AABB */
diff --git a/trunk/fcl/include/fcl/octomap_extension.h b/trunk/fcl/include/fcl/octomap_extension.h
index 6b45b42bd4adb6adeef03edb815c97d214aec6dd..fa52446a1f1ddcea237be2f6b15bff28198482c1 100644
--- a/trunk/fcl/include/fcl/octomap_extension.h
+++ b/trunk/fcl/include/fcl/octomap_extension.h
@@ -38,6 +38,7 @@
#ifndef FCL_OCTOMAP_EXTENSION_H
#define FCL_OCTOMAP_EXTENSION_H
+#include <vector>
#include <set>
#include <octomap/octomap.h>
#include "fcl/broad_phase_collision.h"
@@ -45,22 +46,34 @@
namespace fcl
{
+struct OcTreeNode_AABB_pair
+{
+ octomap::OcTreeNode* node;
+ AABB aabb;
+
+ OcTreeNode_AABB_pair(octomap::OcTreeNode* node_, const AABB& aabb_) : node(node_), aabb(aabb_) {}
+
+ bool operator < (const OcTreeNode_AABB_pair& other) const
+ {
+ return node < other.node;
+ }
+};
-bool defaultCollisionCostFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
+bool defaultCollisionCostOctomapFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
-bool defaultCollisionCostExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes);
+bool defaultCollisionCostOctomapExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes);
-typedef bool (*CollisionCostCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
+typedef bool (*CollisionCostOctomapCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost);
-typedef bool (*CollisionCostCallBackExt)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes);
+typedef bool (*CollisionCostOctomapCallBackExt)(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes);
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback);
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBack callback);
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback);
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBackExt callback, std::set<octomap::OcTreeNode*>& nodes);
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes);
}
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index a5e9f7c9e29a92bf4aca083a1d3a3d127bf6e0b6..cdaff8d4f5889bb08d4c2019d622d9e4721b8625 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -65,6 +65,11 @@ public:
data[3] = d; // z
}
+ bool isIdentity() const
+ {
+ return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0);
+ }
+
/** \brief Matrix to quaternion */
void fromRotation(const Matrix3f& R);
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
index 69ec965c138094606f93e61c3a0d3cf390cac423..81b9cfc3e49aee8b311733f1f5328899ea82b346 100644
--- a/trunk/fcl/src/octomap_extension.cpp
+++ b/trunk/fcl/src/octomap_extension.cpp
@@ -63,13 +63,15 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->bv.overlap(root2_bv))
{
- CollisionGeometry* box = new ExtendedBox(root2_bv.max_[0] - root2_bv.min_[0],
- root2_bv.max_[1] - root2_bv.min_[1],
- root2_bv.max_[2] - root2_bv.min_[2]);
+ 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);
}
+ else return false;
}
+ else return false;
}
if(!tree2->isNodeOccupied(root2) || !root1->bv.overlap(root2_bv)) return false;
@@ -83,43 +85,43 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
else
{
- for(int i = 0; i < 8; ++i)
+ for(unsigned int i = 0; i < 8; ++i)
{
if(root2->childExists(i))
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1 == 0)
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
- else
+ 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];
}
-
- if(i&2 == 0)
+ else
{
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
}
- else
+
+ 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];
}
-
- if(i&4 == 0)
+ else
{
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
}
- else
+
+ 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;
}
if(collisionRecurse(root1, tree2, child, child_bv, cdata, callback))
@@ -127,21 +129,25 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
}
}
}
+
+ return false;
}
void collide(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
- const octomap::point3d& bv_min = octree->getBBXMin();
- const octomap::point3d& bv_max = octree->getBBXMax();
- collisionRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback);
+
+ FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
+
+ collisionRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)),
+ cdata, callback);
}
bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, CollisionCostCallBack callback, FCL_REAL& cost)
+ void* cdata, CollisionCostOctomapCallBack callback, FCL_REAL& cost)
{
if(root1->isLeaf() && !root2->hasChildren())
{
@@ -178,37 +184,37 @@ bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1 == 0)
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
- else
+ 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];
}
-
- if(i&2 == 0)
+ else
{
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
}
- else
+
+ 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];
}
-
- if(i&4 == 0)
+ else
{
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
}
- else
+
+ 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;
}
if(collisionCostRecurse(root1, tree2, child, child_bv, cdata, callback, cost))
@@ -216,12 +222,14 @@ bool collisionCostRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root
}
}
}
+
+ return false;
}
bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
octomap::OcTree* tree2, octomap::OcTreeNode* root2, const AABB& root2_bv,
- void* cdata, CollisionCostCallBackExt callback, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes)
+ void* cdata, CollisionCostOctomapCallBackExt callback, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes)
{
if(root1->isLeaf() && !root2->hasChildren())
{
@@ -234,8 +242,6 @@ bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* r
root2_bv.max_[2] - root2_bv.min_[2]);
box->prob = root2->getOccupancy();
- box->node = root2;
-
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, cost, nodes);
}
@@ -259,37 +265,37 @@ bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* r
{
octomap::OcTreeNode* child = root2->getChild(i);
AABB child_bv;
- if(i&1 == 0)
- {
- child_bv.min_[0] = root2_bv.min_[0];
- child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
- }
- else
+ 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];
}
-
- if(i&2 == 0)
+ else
{
- child_bv.min_[1] = root2_bv.min_[1];
- child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
+ child_bv.min_[0] = root2_bv.min_[0];
+ child_bv.max_[0] = (root2_bv.min_[0] + root2_bv.max_[0]) * 0.5;
}
- else
+
+ 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];
}
-
- if(i&4 == 0)
+ else
{
- child_bv.min_[2] = root2_bv.min_[2];
- child_bv.max_[2] = (root2_bv.min_[2] + root2_bv.max_[2]) * 0.5;
+ child_bv.min_[1] = root2_bv.min_[1];
+ child_bv.max_[1] = (root2_bv.min_[1] + root2_bv.max_[1]) * 0.5;
}
- else
+
+ 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;
}
if(collisionCostExtRecurse(root1, tree2, child, child_bv, cdata, callback, cost, nodes))
@@ -297,11 +303,13 @@ bool collisionCostExtRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* r
}
}
}
+
+ return false;
}
-bool defaultCollisionCostFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost)
+bool defaultCollisionCostOctomapFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost)
{
const AABB& aabb1 = o1->getAABB();
const AABB& aabb2 = o2->getAABB();
@@ -311,36 +319,44 @@ bool defaultCollisionCostFunction(CollisionObject* o1, CollisionObject* o2, void
return false;
}
-bool defaultCollisionCostExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<octomap::OcTreeNode*>& nodes)
+
+bool defaultCollisionCostOctomapExtFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& cost, std::set<OcTreeNode_AABB_pair>& nodes)
{
const AABB& aabb1 = o1->getAABB();
const AABB& aabb2 = o2->getAABB();
Vec3f delta = min(aabb1.max_, aabb2.max_) - max(aabb1.min_, aabb2.min_);
const ExtendedBox* box = static_cast<const ExtendedBox*>(o2->getCollisionGeometry());
cost += delta[0] * delta[1] * delta[2] * box->prob;
- nodes.insert(box->node);
+
+ const Vec3f& c = box->aabb_center;
+ const Vec3f& side = box->side;
+ AABB aabb(Vec3f(c[0] - side[0] / 2, c[1] - side[1] / 2, c[2] - side[2] / 2), Vec3f(c[0] + side[0] / 2, c[1] + side[1] / 2, c[2] + side[2] / 2));
+ nodes.insert(OcTreeNode_AABB_pair(box->node, aabb));
return false;
}
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBack callback)
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBack callback)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
- const octomap::point3d& bv_min = octree->getBBXMin();
- const octomap::point3d& bv_max = octree->getBBXMax();
+
+ FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
FCL_REAL cost = 0;
- collisionCostRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback, cost);
+ collisionCostRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)), cdata, callback, cost);
return cost;
}
-FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostCallBackExt callback, std::set<octomap::OcTreeNode*>& nodes)
+FCL_REAL collideCost(DynamicAABBTreeCollisionManager* manager, octomap::OcTree* octree, void* cdata, CollisionCostOctomapCallBackExt callback, std::vector<AABB>& nodes)
{
DynamicAABBTreeCollisionManager::DynamicAABBNode* root1 = manager->getTree().getRoot();
octomap::OcTreeNode* root2 = octree->getRoot();
- const octomap::point3d& bv_min = octree->getBBXMin();
- const octomap::point3d& bv_max = octree->getBBXMax();
+
+ FCL_REAL delta = (1 << octree->getTreeDepth()) * octree->getResolution() / 2;
FCL_REAL cost = 0;
- collisionCostExtRecurse(root1, octree, root2, AABB(Vec3f(bv_min.x(), bv_min.y(), bv_min.z()), Vec3f(bv_max.x(), bv_max.y(), bv_max.z())), cdata, callback, cost, nodes);
+ std::set<OcTreeNode_AABB_pair> pairs;
+ collisionCostExtRecurse(root1, octree, root2, AABB(Vec3f(-delta, -delta, -delta), Vec3f(delta, delta, delta)), cdata, callback, cost, pairs);
+ for(std::set<OcTreeNode_AABB_pair>::iterator it = pairs.begin(), end = pairs.end(); it != end; ++it)
+ nodes.push_back(it->aabb);
return cost;
}