Commit 6a75e61a authored by jpan's avatar jpan
Browse files

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@15 253336fb-580f-4252-a368-f3cef5a2a82b
parent 9519d84c
......@@ -54,12 +54,15 @@ public:
/** \brief The max point in the AABB */
Vec3f max_;
/** \brief Constructor creating an AABB with infinite size */
AABB();
/** \brief Constructor creating an AABB at position v with zero size */
AABB(const Vec3f& v) : min_(v), max_(v)
{
}
/** \brief Constructor creating an AABB with two endpoints a and b */
AABB(const Vec3f& a, const Vec3f&b)
{
min_ = min(a, b);
......
......@@ -85,6 +85,7 @@ public:
return BVH_MODEL_UNKNOWN;
}
/** \brief Constructing an empty BVH */
BVHModel()
{
vertices = NULL;
......@@ -122,25 +123,34 @@ public:
delete [] primitive_indices;
}
/** \brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so
we must provide some flexibility here */
/** \brief Get the BV of index id */
const BVNode<BV>& getBV(int id) const
{
return bvs[id];
}
/** \brief Get the BV of index id */
BVNode<BV>& getBV(int id)
{
return bvs[id];
}
/** \brief Get the number of BVs */
int getNumBVs() const
{
return num_bvs;
}
/** \brief Get the object type: it is a BVH */
OBJECT_TYPE getObjectType() const { return OT_BVH; }
/** \brief Get the BV type */
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/** \brief Compute the AABB for the BVH, used for broad-phase collision */
void computeAABB();
/** \brief Geometry point data */
......@@ -241,6 +251,8 @@ private:
};
/** Specialization of getNodeType() for BVHModel with different BV types */
template<>
NODE_TYPE BVHModel<AABB>::getNodeType() const;
......
......@@ -49,16 +49,21 @@
namespace fcl
{
/** \brief Base class for BV splitting operation */
template<typename BV>
class BVSplitterBase
{
public:
/** \brief Set the geometry data needed by the split rule */
virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
/** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
/** \brief Apply the split rule on a given point */
virtual bool apply(const Vec3f& q) const = 0;
/** \brief Clear the geometry data set before */
virtual void clear() = 0;
};
......@@ -85,7 +90,7 @@ public:
type = type_;
}
/** \brief Compute the split rule according to a subset of geometry and the the corresponding BV node */
/** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
{
switch(split_method)
......@@ -239,7 +244,7 @@ public:
type = type_;
}
/** \brief Compute the split rule according to a subset of geometry and the the corresponding BV node */
/** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
void computeRule(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
{
Vec3f center = bv.center();
......
......@@ -76,6 +76,8 @@ public:
virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
virtual bool empty() const = 0;
virtual size_t size() const = 0;
};
class NaiveCollisionManager : public BroadPhaseCollisionManager
......@@ -100,6 +102,8 @@ public:
void collide(void* cdata, CollisionCallBack callback) const;
bool empty() const;
inline size_t size() const { return objs.size(); }
protected:
......@@ -139,6 +143,8 @@ public:
void collide(void* cdata, CollisionCallBack callback) const;
bool empty() const;
inline size_t size() const { return AABB_arr.size(); }
protected:
......@@ -249,6 +255,8 @@ public:
void collide(void* cdata, CollisionCallBack callback) const;
bool empty() const;
inline size_t size() const { return objs_x.size(); }
protected:
......@@ -335,6 +343,8 @@ public:
void collide(void* cdata, CollisionCallBack callback) const;
bool empty() const;
inline size_t size() const { return endpoints[0].size() / 2; }
protected:
......
......@@ -42,9 +42,15 @@
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
/** \brief Main namespace */
namespace fcl
{
/** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning
* returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
* performs the collision between them.
* Return value is the number of contacts returned
*/
int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
......
......@@ -42,10 +42,11 @@
#include "fcl/BVH_model.h"
#include <boost/math/constants/constants.hpp>
/** \brief Main namespace */
namespace fcl
{
/** \brief Generate BVH model from box */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape)
{
......@@ -89,6 +90,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
model.computeAABB();
}
/** Generate BVH model from sphere */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
{
......@@ -155,6 +157,8 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
model.computeAABB();
}
/** \brief Generate BVH model from cylinder */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
{
......
......@@ -50,6 +50,7 @@ namespace fcl
class ShapeBase : public CollisionObject
{
public:
/** \brief Default Constructor */
ShapeBase()
{
Rloc[0][0] = 1;
......@@ -57,6 +58,7 @@ public:
Rloc[2][2] = 1;
}
/** \brief Set the local frame of the shape */
void setLocalTransform(const Vec3f R_[3], const Vec3f& T_)
{
Rloc[0] = R_[0];
......@@ -65,6 +67,7 @@ public:
Tloc = T_;
}
/** \brief Set the local orientation of the shape */
void setLocalRotation(const Vec3f R[3])
{
Rloc[0] = R[0];
......@@ -72,12 +75,14 @@ public:
Rloc[2] = R[2];
}
/** \brief Set the local position of the shape */
void setLocalTranslation(const Vec3f& T)
{
Tloc = T;
}
void advanceLocalTransform(const Vec3f R[3], const Vec3f& T)
/** \brief Append additional transform to the local transform */
void appendLocalTransform(const Vec3f R[3], const Vec3f& T)
{
Vec3f R0[3];
for(int i = 0; i < 3; ++i)
......@@ -86,6 +91,7 @@ public:
Tloc = MxV(R, Tloc) + T;
}
/** \brief Get local transform */
void getLocalTransfrom(Vec3f R[3], Vec3f& T) const
{
T = Tloc;
......@@ -94,16 +100,19 @@ public:
R[2] = Rloc[2];
}
/** \brief Get local position */
inline const Vec3f& getLocalPosition() const
{
return Tloc;
}
/** \brief Get local orientation */
inline const Vec3f* getLocalRotation() const
{
return Rloc;
}
/** \brief Get object type: a geometric shape */
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
protected:
......@@ -122,8 +131,10 @@ public:
/** box side length */
Vec3f side;
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a box */
NODE_TYPE getNodeType() const { return GEOM_BOX; }
};
......@@ -132,10 +143,14 @@ class Sphere : public ShapeBase
{
public:
Sphere(BVH_REAL radius_) : ShapeBase(), radius(radius_) {}
/** \brief Radius of the sphere */
BVH_REAL radius;
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a sphere */
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
};
......@@ -144,11 +159,17 @@ class Capsule : public ShapeBase
{
public:
Capsule(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of capsule */
BVH_REAL radius;
BVH_REAL lz; // length along z axis
/** \brief Length along z axis */
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a capsule */
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
};
......@@ -157,11 +178,17 @@ class Cone : public ShapeBase
{
public:
Cone(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of the cone */
BVH_REAL radius;
BVH_REAL lz; // length along z axis
/** \brief Length along z axis */
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a cone */
NODE_TYPE getNodeType() const { return GEOM_CONE; }
};
......@@ -170,11 +197,17 @@ class Cylinder : public ShapeBase
{
public:
Cylinder(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
/** \brief Radius of the cylinder */
BVH_REAL radius;
BVH_REAL lz; // length along z axis
/** \brief Length along z axis */
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a cylinder */
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
};
......@@ -182,6 +215,7 @@ public:
class Convex : public ShapeBase
{
public:
/** Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information */
Convex(Vec3f* plane_normals_,
BVH_REAL* plane_dis_,
int num_planes_,
......@@ -207,6 +241,7 @@ public:
fillEdges();
}
/** Copy constructor */
Convex(const Convex& other) : ShapeBase(other)
{
plane_normals = other.plane_normals;
......@@ -223,10 +258,13 @@ public:
delete [] edges;
}
/** Compute AABB */
void computeAABB();
/** Get node type: a conex polytope */
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
Vec3f* plane_normals;
BVH_REAL* plane_dis;
......@@ -247,9 +285,11 @@ public:
Edge* edges;
/** \brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) */
Vec3f center;
protected:
/** \brief Get edge information */
void fillEdges();
};
......@@ -257,7 +297,10 @@ protected:
class Plane : public ShapeBase
{
public:
/** \brief Construct a plane with normal direction and offset */
Plane(const Vec3f& n_, BVH_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
/** \brief Construct a plane with normal direction and offset */
Plane(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d_)
{
n = Vec3f(a, b, c);
......@@ -265,15 +308,21 @@ public:
unitNormalTest();
}
/** \brief Compute AABB */
void computeAABB();
/** \brief Get node type: a plane */
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
/** \brief Plane normal */
Vec3f n;
/** \brief Plane offset */
BVH_REAL d;
protected:
/** \brief Turn non-unit normal into unit */
void unitNormalTest();
};
......
......@@ -43,10 +43,11 @@
#include "fcl/traversal_node_bvh_shape.h"
#include "fcl/BVH_utility.h"
/** \brief Main namespace */
namespace fcl
{
/** \brief Initialize traversal node for collision between two geometric shapes */
template<typename S1, typename S2>
bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2, bool enable_contact = false)
{
......@@ -54,6 +55,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
return true;
}
/** \brief Initialize traversal node for collision between two geometric shapes, given the current transforms of the two shapes */
template<typename S1, typename S2>
bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -71,7 +73,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
return true;
}
/** \brief Initialize traversal node for collision between one mesh and one shape */
template<typename BV, typename S>
bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, const BVHModel<BV>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -90,7 +92,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, const BVHModel<BV>
return true;
}
/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
template<typename BV, typename S>
bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& model1, S& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -115,7 +117,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
model1.replaceSubModel(vertices_transformed1);
model1.endReplaceModel(use_refit, refit_bottomup);
model2.advanceLocalTransform(R2, T2);
model2.appendLocalTransform(R2, T2);
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
......@@ -127,7 +129,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -146,7 +148,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
return true;
}
/** \brief Initialize traversal node for collision between one mesh and one shape, given the current transform, specialized for OBB type */
template<typename S>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, S& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -158,7 +160,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
node.model1 = &model1;
node.model2 = &model2;
model2.advanceLocalTransform(R2, T2);
model2.appendLocalTransform(R2, T2);
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
......@@ -171,7 +173,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
return true;
}
/** \brief Initialize traversal node for collision between two meshes */
template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -194,7 +196,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1
return true;
}
/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
......@@ -243,14 +245,18 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
return true;
}
/** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
#if USE_SVMLIGHT
/** \brief Initialize traversal node for collision between two point clouds */
template<typename BV>
bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -290,6 +296,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
return true;
}
/** \brief Initialize traversal node for collision between two point clouds, given current transforms */
template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -353,7 +360,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
return true;
}
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */
bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -363,6 +370,7 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
bool enable_contact = false,
BVH_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -372,6 +380,7 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
bool enable_contact = false,
BVH_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh */
template<typename BV>
bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, const BVHModel<BV>& model2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -406,6 +415,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
return true;
}
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
template<typename BV, bool use_refit, bool refit_bottomup>
bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -467,7 +477,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
}
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -477,7 +487,7 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
bool enable_contact = false,
BVH_REAL expand_r = 1);
/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
BVH_REAL collision_prob_threshold = 0.5,
......@@ -489,6 +499,7 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
#endif
/** \brief Initialize traversal node for continuous collision detection between two meshes */
template<typename BV>
bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -514,6 +525,7 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<B
return true;
}
/** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
template<typename BV>
bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -537,6 +549,7 @@ bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const
return true;
}
/** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
template<typename BV>
bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
......@@ -560,6 +573,7 @@ bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const
return true;
}
/** \brief Initialize traversal node for distance computation between two meshes */
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2)
{
......@@ -578,6 +592,8 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, const BVHModel<BV>& model1,
return true;
}
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
......@@ -623,9 +639,12 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
return true;
}
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,