Commit 035bab78 authored by jpan's avatar jpan
Browse files

fix some bug in shape collision and more refactoring.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@30 253336fb-580f-4252-a368-f3cef5a2a82b
parent d0dc110b
......@@ -258,7 +258,7 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
g->beginModel();
g->addSubModel(points, tri_indices);
g->endModel();
g->computeAABB();
g->computeLocalAABB();
}
}
break;
......@@ -276,7 +276,7 @@ void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom, const btTransf
const btVector3& o = pose.getOrigin();
geom->setQuatRotation(fcl::SimpleQuaternion(q.getW(), q.getX(), q.getY(), q.getZ()));
geom->setTranslation(fcl::Vec3f(o.getX(), o.getY(), o.getZ()));
geom->computeCachedAABB(); // update AABB
geom->computeAABB(); // update AABB
}
......
......@@ -151,7 +151,7 @@ public:
NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
/** \brief Compute the AABB for the BVH, used for broad-phase collision */
void computeAABB();
void computeLocalAABB();
/** \brief Geometry point data */
Vec3f* vertices;
......
......@@ -334,7 +334,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
if(a->getCachedAABB().min_[0] < b->getCachedAABB().min_[0])
if(a->getAABB().min_[0] < b->getAABB().min_[0])
return true;
return false;
}
......@@ -345,7 +345,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
if(a->getCachedAABB().min_[1] < b->getCachedAABB().min_[1])
if(a->getAABB().min_[1] < b->getAABB().min_[1])
return true;
return false;
}
......@@ -356,7 +356,7 @@ protected:
{
bool operator()(const CollisionObject* a, const CollisionObject* b) const
{
if(a->getCachedAABB().min_[2] < b->getCachedAABB().min_[2])
if(a->getAABB().min_[2] < b->getAABB().min_[2])
return true;
return false;
}
......@@ -366,12 +366,12 @@ protected:
class DummyCollisionObject : public CollisionObject
{
public:
DummyCollisionObject(const AABB& aabb)
DummyCollisionObject(const AABB& aabb_)
{
aabb_cache = aabb;
aabb = aabb_;
}
void computeAABB() {}
void computeLocalAABB() {}
};
/** \brief check collision between one object and a list of objects */
......
......@@ -59,19 +59,19 @@ public:
virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
virtual void computeAABB() = 0;
virtual void computeLocalAABB() = 0;
inline const AABB& getCachedAABB() const
inline const AABB& getAABB() const
{
return aabb_cache;
return aabb;
}
inline void computeCachedAABB()
inline void computeAABB()
{
Vec3f center = t.transform(aabb_center);
Vec3f delta(aabb_radius, aabb_radius, aabb_radius);
aabb_cache.min_ = center - delta;
aabb_cache.max_ = center + delta;
aabb.min_ = center - delta;
aabb.max_ = center + delta;
}
inline const Vec3f& getTranslation() const
......@@ -117,10 +117,10 @@ public:
protected:
/** AABB in global coordinate */
mutable AABB aabb_cache;
mutable AABB aabb;
/** AABB in local coordinate */
AABB aabb;
AABB aabb_local;
/** AABB center in local coordinate */
Vec3f aabb_center;
......
......@@ -79,15 +79,15 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
v = MxV(shape.getRotation(), v) + shape.getTranslation();
Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeAABB();
model.computeLocalAABB();
}
/** Generate BVH model from sphere */
......@@ -146,15 +146,15 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = MxV(shape.getLocalRotation(), points[i]) + shape.getLocalPosition();
v = MxV(shape.getRotation(), v) + shape.getTranslation();
Vec3f v = matMulVec(shape.getLocalRotation(), points[i]) + shape.getLocalTranslation();
v = matMulVec(shape.getRotation(), v) + shape.getTranslation();
points[i] = v;
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeAABB();
model.computeLocalAABB();
}
......@@ -224,7 +224,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeAABB();
model.computeLocalAABB();
}
}
......
......@@ -87,12 +87,12 @@ public:
Vec3f R0[3];
for(int i = 0; i < 3; ++i)
R0[i] = Rloc[i];
MxM(R, R0, Rloc);
Tloc = MxV(R, Tloc) + T;
matMulMat(R, R0, Rloc);
Tloc = matMulVec(R, Tloc) + T;
}
/** \brief Get local transform */
void getLocalTransfrom(Vec3f R[3], Vec3f& T) const
void getLocalTransform(Vec3f R[3], Vec3f& T) const
{
T = Tloc;
R[0] = Rloc[0];
......@@ -101,7 +101,7 @@ public:
}
/** \brief Get local position */
inline const Vec3f& getLocalPosition() const
inline const Vec3f& getLocalTranslation() const
{
return Tloc;
}
......@@ -132,7 +132,7 @@ public:
Vec3f side;
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a box */
NODE_TYPE getNodeType() const { return GEOM_BOX; }
......@@ -148,7 +148,7 @@ public:
BVH_REAL radius;
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a sphere */
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
......@@ -167,7 +167,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a capsule */
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
......@@ -186,7 +186,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a cone */
NODE_TYPE getNodeType() const { return GEOM_CONE; }
......@@ -205,7 +205,7 @@ public:
BVH_REAL lz;
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a cylinder */
NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
......@@ -259,7 +259,7 @@ public:
}
/** Compute AABB */
void computeAABB();
void computeLocalAABB();
/** Get node type: a conex polytope */
NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
......@@ -309,7 +309,7 @@ public:
}
/** \brief Compute AABB */
void computeAABB();
void computeLocalAABB();
/** \brief Get node type: a plane */
NODE_TYPE getNodeType() const { return GEOM_PLANE; }
......
......@@ -47,11 +47,11 @@
namespace fcl
{
/** recall function used by GJK algorithm */
/** \brief recall function used by GJK algorithm */
typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
/** initialize GJK stuffs */
/** \brief initialize GJK stuffs */
template<typename T>
class GJKInitializer
{
......@@ -72,6 +72,7 @@ public:
static void deleteGJKObject(void* o) {}
};
/** \brief initialize GJK Cylinder */
template<>
class GJKInitializer<Cylinder>
{
......@@ -82,7 +83,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Sphere */
template<>
class GJKInitializer<Sphere>
{
......@@ -93,7 +94,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Box */
template<>
class GJKInitializer<Box>
{
......@@ -104,7 +105,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Capsule */
template<>
class GJKInitializer<Capsule>
{
......@@ -115,7 +116,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Cone */
template<>
class GJKInitializer<Cone>
{
......@@ -126,6 +127,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Convex */
template<>
class GJKInitializer<Convex>
{
......@@ -136,7 +138,7 @@ public:
static void deleteGJKObject(void* o);
};
/** \brief initialize GJK Triangle */
GJKSupportFunction triGetSupportFunction();
GJKCenterFunction triGetCenterFunction();
......@@ -147,15 +149,13 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
void triDeleteGJKObject(void* o);
/** GJK collision algorithm */
/** \brief GJK collision algorithm */
bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
void transformGJKObject(void* obj, const Vec3f R[3], const Vec3f& T);
/** collision algorithm between two shapes */
/** intersection checking between two shapes */
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
......@@ -170,42 +170,7 @@ bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BV
GJKInitializer<S2>::deleteGJKObject(o2);
}
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S1>::createGJKObject(s1);
void* o2 = GJKInitializer<S2>::createGJKObject(s2);
transformGJKObject(o1, R1, T1);
transformGJKObject(o2, R2, T2);
return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
contact_points, penetration_depth, normal);
GJKInitializer<S1>::deleteGJKObject(o1);
GJKInitializer<S2>::deleteGJKObject(o2);
}
template<typename S1, typename S2>
bool shapeIntersect(const S1& s1, const S2& s2, const Vec3f R[3], const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S1>::createGJKObject(s1);
void* o2 = GJKInitializer<S2>::createGJKObject(s2);
transformGJKObject(o2, R, T);
return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
contact_points, penetration_depth, normal);
GJKInitializer<S1>::deleteGJKObject(o1);
GJKInitializer<S2>::deleteGJKObject(o2);
}
/** \brief intersection checking between one shape and a triangle */
template<typename S>
bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
......@@ -220,25 +185,7 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
triDeleteGJKObject(o2);
}
template<typename S>
bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S>::createGJKObject(s);
void* o2 = triCreateGJKObject(P1, P2, P3, R2, T2);
transformGJKObject(o1, R1, T1);
return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
o2, triGetSupportFunction(), triGetCenterFunction(),
contact_points, penetration_depth, normal);
GJKInitializer<S>::deleteGJKObject(o1);
triDeleteGJKObject(o2);
}
/** \brief intersection checking between one shape and a triangle with transformation */
template<typename S>
bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Vec3f R[3], const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
......@@ -254,25 +201,6 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
triDeleteGJKObject(o2);
}
template<typename S>
bool shapeTriangleIntersect(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const S& s, const Vec3f R[3], const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = triCreateGJKObject(P1, P2, P3);
void* o2 = GJKInitializer<S>::createGJKObject(s);
transformGJKObject(o2, R, T);
return GJKCollide(o1, triGetSupportFunction(), triGetCenterFunction(),
o2, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
contact_points, penetration_depth, normal);
triDeleteGJKObject(o1);
GJKInitializer<S>::deleteGJKObject(o2);
}
}
#endif
......@@ -83,8 +83,8 @@ public:
const Vec3f R2[3], const Vec3f& T2,
const Vec3f& O)
{
t1 = SimpleTransform(R1, T1 - MxV(R1, O));
t2 = SimpleTransform(R2, T2 - MxV(R2, O));
t1 = SimpleTransform(R1, T1 - matMulVec(R1, O));
t2 = SimpleTransform(R2, T2 - matMulVec(R2, O));
t = t1;
/** Compute the velocities for the motion */
......
......@@ -59,7 +59,7 @@ struct Uncertainty
/** preprocess performs the eigen decomposition on the Sigma matrix */
void preprocess()
{
Meigen(Sigma, sigma, axis);
matEigen(Sigma, sigma, axis);
}
/** sqrt performs the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized
......
......@@ -61,13 +61,6 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
bool enable_contact = false)
{
for(int i = 0; i < 3; ++i)
{
node.R1[i] = R1[i];
node.R2[i] = R2[i];
}
node.T1 = T1;
node.T2 = T2;
node.enable_contact = enable_contact;
return true;
......@@ -109,7 +102,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -209,7 +202,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -218,7 +211,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = MxV(R2, p) + T2;
Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
......@@ -315,7 +308,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -324,7 +317,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = MxV(R2, p) + T2;
Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
......@@ -433,7 +426,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -442,7 +435,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = MxV(R2, p) + T2;
Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
......@@ -606,7 +599,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -615,7 +608,7 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = MxV(R2, p) + T2;
Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
......@@ -657,7 +650,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = MxV(R1, p) + T1;
Vec3f new_v = matMulVec(R1, p) + T1;
vertices_transformed1[i] = new_v;
}
......@@ -666,7 +659,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = MxV(R2, p) + T2;
Vec3f new_v = matMulVec(R2, p) + T2;
vertices_transformed2[i] = new_v;
}
......
......@@ -53,13 +53,6 @@ public:
model1 = NULL;
model2 = NULL;
R1[0] = Vec3f(1, 0, 0);
R1[1] = Vec3f(0, 1, 0);
R1[2] = Vec3f(0, 0, 1);
R2[0] = Vec3f(1, 0, 0);
R2[1] = Vec3f(0, 1, 0);
R2[2] = Vec3f(0, 0, 1);
enable_contact = false;
}
......@@ -71,9 +64,9 @@ public:
void leafTesting(int, int) const
{
if(enable_contact)
is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2, &contact_point, &penetration_depth, &normal);
is_collision = shapeIntersect(*model1, *model2, &contact_point, &penetration_depth, &normal);
else
is_collision = shapeIntersect(*model1, *model2, R1, T1, R2, T2);
is_collision = shapeIntersect(*model1, *model2);
}
const S1* model1;
......@@ -88,10 +81,6 @@ public:
bool enable_contact;
mutable bool is_collision;
Vec3f R1[3];
Vec3f R2[3];
Vec3f T1, T2;
};
}
......
......@@ -453,25 +453,25 @@ namespace fcl
#endif
/** \brief M * v */
Vec3f MxV(const Vec3f M[3], const Vec3f& v);
Vec3f matMulVec(const Vec3f M[3], const Vec3f& v);
/** \brief M' * v */
Vec3f