Skip to content
Snippets Groups Projects
Commit f7bd815d authored by jpan's avatar jpan
Browse files

octomap is correct now. 

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@129 253336fb-580f-4252-a368-f3cef5a2a82b
parent b5646a33
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
trunk/fcl/include/fcl/geometric_shapes.h
+ 38
− 7
View file @ f7bd815d
......@@ -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 */
......
trunk/fcl/include/fcl/octomap_extension.h
+ 19
− 6
View file @ f7bd815d
......@@ -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);
}
......
trunk/fcl/include/fcl/transform.h
+ 5
− 0
View file @ f7bd815d
......@@ -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);
......
trunk/fcl/src/octomap_extension.cpp
+ 86
− 70
View file @ f7bd815d
......@@ -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;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment