Commit cd66479c authored by jpan's avatar jpan
Browse files

change the interface of collide: separate geometry and pose. See collide.h and related files


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@55 253336fb-580f-4252-a368-f3cef5a2a82b
parent 7fb021ba
......@@ -266,9 +266,92 @@ fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape
ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
}
return g;
fcl::CollisionObject* co = new fcl::CollisionObject(g);
return co;
}
/*
fcl::CollisionObject* EnvironmentModelFCL::createGeom(const shapes::Shape *shape, double scale, double padding)
{
fcl::CollisionObject* g = NULL;
switch(shape->type)
{
case shapes::SPHERE:
{
g = new fcl::Sphere(static_cast<const shapes::Sphere*>(shape)->radius * scale + padding);
}
break;
case shapes::BOX:
{
const double *size = static_cast<const shapes::Box*>(shape)->size;
g = new fcl::Box(size[0] * scale + padding * 2.0, size[1] * scale + padding * 2.0, size[2] * scale + padding * 2.0);
}
break;
case shapes::CYLINDER:
{
g = new fcl::Cylinder(static_cast<const shapes::Cylinder*>(shape)->radius * scale + padding,
static_cast<const shapes::Cylinder*>(shape)->length * scale + padding * 2.0);
}
break;
case shapes::MESH:
{
fcl::BVHModel<fcl::OBB>* gobb = new fcl::BVHModel<fcl::OBB>();
const shapes::Mesh *mesh = static_cast<const shapes::Mesh*>(shape);
if(mesh->vertexCount > 0 && mesh->triangleCount > 0)
{
std::vector<fcl::Triangle> tri_indices(mesh->triangleCount);
for(unsigned int i = 0; i < mesh->triangleCount; ++i)
tri_indices[i] = fcl::Triangle(mesh->triangles[3 * i], mesh->triangles[3 * i + 1], mesh->triangles[3 * i + 2]);
std::vector<fcl::Vec3f> points(mesh->vertexCount);
double sx = 0.0, sy = 0.0, sz = 0.0;
for(unsigned int i = 0; i < mesh->vertexCount; ++i)
{
points[i] = fcl::Vec3f(mesh->vertices[3 * i], mesh->vertices[3 * i + 1], mesh->vertices[3 * i + 2]);
sx += points[i][0];
sy += points[i][1];
sz += points[i][2];
}
// the center of the mesh
sx /= (double)mesh->vertexCount;
sy /= (double)mesh->vertexCount;
sz /= (double)mesh->vertexCount;
// scale the mesh
for(unsigned int i = 0; i < mesh->vertexCount; ++i)
{
// vector from center to the vertex
double dx = points[i][0] - sx;
double dy = points[i][1] - sy;
double dz = points[i][2] - sz;
// length of vector
//double norm = sqrt(dx * dx + dy * dy + dz * dz);
double ndx = ((dx > 0) ? dx+padding : dx-padding);
double ndy = ((dy > 0) ? dy+padding : dy-padding);
double ndz = ((dz > 0) ? dz+padding : dz-padding);
// the new distance of the vertex from the center
//double fact = scale + padding/norm;
points[i] = fcl::Vec3f(sx + ndx, sy + ndy, sz + ndz);
}
gobb->beginModel();
gobb->addSubModel(points, tri_indices);
gobb->endModel();
gobb->computeLocalAABB();
g = gobb;
}
}
break;
default:
ROS_FATAL_STREAM("This shape type is not supported using FCL yet");
}
return g;
}
*/
void EnvironmentModelFCL::updateGeom(fcl::CollisionObject* geom, const btTransform &pose) const
{
......
......@@ -54,7 +54,7 @@ namespace fcl
/** \brief A class describing the bounding hierarchy of a mesh model */
template<typename BV>
class BVHModel : public CollisionObject
class BVHModel : public CollisionGeometry
{
private:
int num_tris_allocated;
......
......@@ -54,6 +54,11 @@ namespace fcl
int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
}
#endif
......@@ -14,8 +14,8 @@ struct Contact
BVH_REAL penetration_depth;
Vec3f normal;
Vec3f pos;
const CollisionObject* o1;
const CollisionObject* o2;
const CollisionGeometry* o1;
const CollisionGeometry* o2;
int b1;
int b2;
......@@ -27,7 +27,7 @@ struct Contact
b2 = -1;
}
Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_)
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
{
o1 = o1_;
o2 = o2_;
......@@ -35,7 +35,7 @@ struct Contact
b2 = b2_;
}
Contact(const CollisionObject* o1_, const CollisionObject* o2_, int b1_, int b2_,
Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
const Vec3f& pos_, const Vec3f& normal_, BVH_REAL depth_)
{
o1 = o1_;
......
......@@ -46,7 +46,7 @@ namespace fcl
{
typedef int (*CollisionFunc)(const CollisionObject* o1, const CollisionObject* o2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
struct CollisionFunctionMatrix
......
......@@ -40,6 +40,7 @@
#include "fcl/AABB.h"
#include "fcl/transform.h"
#include <boost/shared_ptr.hpp>
namespace fcl
{
......@@ -49,11 +50,181 @@ enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
/** \brief Base class for all objects participating in collision */
class CollisionGeometry
{
public:
virtual ~CollisionGeometry() {}
virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
virtual void computeLocalAABB() = 0;
/** AABB center in local coordinate */
Vec3f aabb_center;
/** AABB radius */
BVH_REAL aabb_radius;
};
class CollisionObject
{
public:
virtual ~CollisionObject() {}
CollisionObject(CollisionGeometry* cgeom_)
{
cgeom.reset(cgeom_);
computeAABB();
}
CollisionObject(CollisionGeometry* cgeom_, const SimpleTransform& tf)
{
cgeom.reset(cgeom_);
t = tf;
computeAABB();
}
CollisionObject(CollisionGeometry* cgeom_, const Matrix3f& R, const Vec3f& T)
{
cgeom.reset(cgeom_);
t = SimpleTransform(R, T);
computeAABB();
}
CollisionObject()
{
}
~CollisionObject()
{
}
OBJECT_TYPE getObjectType() const
{
return cgeom->getObjectType();
}
NODE_TYPE getNodeType() const
{
return cgeom->getNodeType();
}
inline const AABB& getAABB() const
{
return aabb;
}
inline void computeAABB()
{
Vec3f center = t.transform(cgeom->aabb_center);
Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
aabb.min_ = center - delta;
aabb.max_ = center + delta;
}
inline void computeAABB2()
{
Vec3f center = t.transform(cgeom->aabb_center);
// compute new r1, r2, r3
}
void* getUserData() const
{
return user_data;
}
void setUserData(void *data)
{
user_data = data;
}
inline const Vec3f& getTranslation() const
{
return t.getTranslation();
}
inline const Matrix3f& getRotation() const
{
return t.getRotation();
}
inline const SimpleQuaternion& getQuatRotation() const
{
return t.getQuatRotation();
}
inline const SimpleTransform& getTransform() const
{
return t;
}
void setRotation(const Matrix3f& R)
{
t.setRotation(R);
}
void setTranslation(const Vec3f& T)
{
t.setTranslation(T);
}
void setQuatRotation(const SimpleQuaternion& q)
{
t.setQuatRotation(q);
}
void setTransform(const Matrix3f& R, const Vec3f& T)
{
t.setTransform(R, T);
}
void setTransform(const SimpleQuaternion& q, const Vec3f& T)
{
t.setTransform(q, T);
}
void setTransform(const SimpleTransform& tf)
{
t = tf;
}
bool isIdentityTransform() const
{
return t.isIdentity();
}
void setIdentityTransform()
{
t.setIdentity();
}
const CollisionGeometry* getCollisionGeometry() const
{
return cgeom.get();
}
protected:
// const CollisionGeometry* cgeom;
boost::shared_ptr<CollisionGeometry> cgeom;
SimpleTransform t;
/** AABB in global coordinate */
mutable AABB aabb;
/** pointer to user defined data specific to this object */
void *user_data;
};
/** \brief Base class for all objects participating in collision */
class CollisionObject2
{
public:
virtual ~CollisionObject2() {}
virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
......
......@@ -48,9 +48,9 @@ namespace fcl
{
template<typename BV>
int conservativeAdvancement(const CollisionObject* o1,
int conservativeAdvancement(const CollisionGeometry* o1,
MotionBase<BV>* motion1,
const CollisionObject* o2,
const CollisionGeometry* o2,
MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
......
......@@ -48,7 +48,7 @@ namespace fcl
/** \brief Generate BVH model from box */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape)
void generateBVHModel(BVHModel<BV>& model, const Box& shape, const SimpleTransform& pose = SimpleTransform())
{
double a = shape.side[0];
double b = shape.side[1];
......@@ -80,7 +80,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......@@ -92,7 +92,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
/** Generate BVH model from sphere */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int seg = 16, unsigned int ring = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -147,7 +147,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......@@ -162,7 +162,7 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
* then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
*/
template<typename BV>
void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_faces_for_unit_sphere)
void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int n_faces_for_unit_sphere = 100)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -222,7 +222,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......@@ -235,7 +235,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, unsigned int n_
/** \brief Generate BVH model from cylinder */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -299,7 +299,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......@@ -314,7 +314,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int t
* larger radius, the number of circle split number is r * tot.
*/
template<typename BV>
void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot_for_unit_cylinder)
void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot_for_unit_cylinder = 100)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -379,7 +379,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......@@ -392,7 +392,7 @@ void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, unsigned int
/** \brief Generate BVH model from cone */
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot = 16)
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const SimpleTransform& pose = SimpleTransform(), unsigned int tot = 16)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -454,7 +454,7 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, unsigned int tot =
for(unsigned int i = 0; i < points.size(); ++i)
{
Vec3f v = shape.getLocalRotation() * points[i] + shape.getLocalTranslation();
v = shape.getRotation() * v + shape.getTranslation();
v = pose.transform(v);
points[i] = v;
}
......
......@@ -47,7 +47,7 @@ namespace fcl
{
/** \brief Base class for all basic geometric shapes */
class ShapeBase : public CollisionObject
class ShapeBase : public CollisionGeometry
{
public:
/** \brief Default Constructor */
......
......@@ -66,7 +66,7 @@ public:
* Notice that only local transformation is applied.
* Gloal transformation are considered later
*/
static void* createGJKObject(const T& s) { return NULL; }
static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
/** \brief Delete GJK object */
static void deleteGJKObject(void* o) {}
......@@ -79,7 +79,7 @@ class GJKInitializer<Cylinder>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Cylinder& s);
static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -90,7 +90,7 @@ class GJKInitializer<Sphere>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Sphere& s);
static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -101,7 +101,7 @@ class GJKInitializer<Box>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Box& s);
static void* createGJKObject(const Box& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -112,7 +112,7 @@ class GJKInitializer<Capsule>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Capsule& s);
static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -123,7 +123,7 @@ class GJKInitializer<Cone>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Cone& s);
static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -134,7 +134,7 @@ class GJKInitializer<Convex>
public:
static GJKSupportFunction getSupportFunction();
static GJKCenterFunction getCenterFunction();
static void* createGJKObject(const Convex& s);
static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
static void deleteGJKObject(void* o);
};
......@@ -157,10 +157,12 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
/** 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)
bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S1>::createGJKObject(s1);
void* o2 = GJKInitializer<S2>::createGJKObject(s2);
void* o1 = GJKInitializer<S1>::createGJKObject(s1, tf1);
void* o2 = GJKInitializer<S2>::createGJKObject(s2, tf2);
return GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
......@@ -172,9 +174,10 @@ bool shapeIntersect(const S1& s1, const S2& s2, Vec3f* contact_points = NULL, BV
/** \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)
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S>::createGJKObject(s);
void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = triCreateGJKObject(P1, P2, P3);
return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
......@@ -187,10 +190,11 @@ bool shapeTriangleIntersect(const S& s, const Vec3f& P1, const Vec3f& P2, const
/** \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 Matrix3f& R, const Vec3f& T,
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
void* o1 = GJKInitializer<S>::createGJKObject(s);
void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
void* o2 = triCreateGJKObject(P1, P2, P3, R, T);
return GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
......
......@@ -44,71 +44,71 @@
namespace fcl
{
template<typename BV>
void computeBV(const Box& s, BV& bv) {}
void computeBV(const Box& s, const SimpleTransform& tf, BV& bv) {}
template<typename BV>
void computeBV(const Sphere& s, BV& bv) {}
void computeBV(const Sphere& s, const SimpleTransform& tf, BV& bv) {}
template<typename BV>
void computeBV(const Capsule& s, BV& bv) {}
void computeBV(const Capsule& s, const SimpleTransform& tf