Commit f6bfb30b authored by jpan's avatar jpan
Browse files

shape-mesh and shape-shape distance finished.

now the user can choose solver for the basic shape distance/collision. 



git-svn-id: https://kforge.ros.org/fcl/fcl_ros@104 253336fb-580f-4252-a368-f3cef5a2a82b
parent 09b6b7e4
......@@ -51,9 +51,24 @@ namespace fcl
* performs the collision between them.
* Return value is the number of contacts returned
*/
template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
template<typename NarrowPhaseSolver>
int collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts);
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,
......
......@@ -45,12 +45,11 @@
namespace fcl
{
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);
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
CollisionFunc collision_matrix[16][16];
CollisionFunctionMatrix();
......
......@@ -41,11 +41,21 @@
namespace fcl
{
template<typename NarrowPhaseSolver>
BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver);
template<typename NarrowPhaseSolver>
BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver);
BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2);
BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2);
}
#endif
......@@ -42,10 +42,12 @@
namespace fcl
{
typedef BVH_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2);
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
typedef BVH_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
DistanceFunc distance_matrix[16][16];
DistanceFunctionMatrix();
......
......@@ -89,10 +89,6 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL*
BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m);
static const BVH_REAL GJK_EPS = 0.000001;
static const size_t GJK_MAX_ITERATIONS = 128;
struct GJK
{
struct SimplexV
......@@ -116,7 +112,13 @@ struct GJK
Simplex simplices[2];
GJK() { initialize(); }
GJK(unsigned int max_iterations_, BVH_REAL tolerance_)
{
max_iterations = max_iterations_;
tolerance = tolerance_;
initialize();
}
void initialize();
......@@ -142,6 +144,9 @@ private:
size_t current;
Simplex* simplex;
Status status;
BVH_REAL tolerance;
unsigned int max_iterations;
};
......@@ -202,6 +207,12 @@ private:
SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
};
private:
unsigned int max_face_num;
unsigned int max_vertex_num;
unsigned int max_iterations;
BVH_REAL tolerance;
public:
enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
......@@ -210,16 +221,27 @@ public:
GJK::Simplex result;
Vec3f normal;
BVH_REAL depth;
SimplexV sv_store[EPA_MAX_VERTICES];
SimplexF fc_store[EPA_MAX_FACES];
SimplexV* sv_store;
SimplexF* fc_store;
size_t nextsv;
SimplexList hull, stock;
EPA()
EPA(unsigned int max_face_num_, unsigned int max_vertex_num_, unsigned int max_iterations_, BVH_REAL tolerance_)
{
max_face_num = max_face_num_;
max_vertex_num = max_vertex_num_;
max_iterations = max_iterations_;
tolerance = tolerance_;
initialize();
}
~EPA()
{
delete [] sv_store;
delete [] fc_store;
}
void initialize();
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist);
......
......@@ -155,10 +155,12 @@ void triDeleteGJKObject(void* o);
/** \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,
unsigned int max_iterations, BVH_REAL tolerance,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, BVH_REAL tolerance,
BVH_REAL* dist);
......
......@@ -48,10 +48,11 @@ namespace fcl
{
/** \brief Initialize traversal node for collision between two geometric shapes */
template<typename S1, typename S2>
bool initialize(ShapeCollisionTraversalNode<S1, S2>& node,
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeCollisionTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const SimpleTransform& tf1,
const S2& shape2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool enable_contact = false)
{
node.enable_contact = enable_contact;
......@@ -59,14 +60,16 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node,
node.tf1 = tf1;
node.model2 = &shape2;
node.tf2 = tf2;
node.nsolver = nsolver;
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,
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
bool use_refit = false, bool refit_bottomup = false)
{
......@@ -94,6 +97,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model2, tf2, node.model2_bv);
......@@ -108,10 +112,11 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node,
/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
template<typename S, typename BV>
bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node,
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
bool use_refit = false, bool refit_bottomup = false)
{
......@@ -139,6 +144,7 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node,
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model1, tf1, node.model1_bv);
......@@ -154,10 +160,11 @@ bool initialize(ShapeMeshCollisionTraversalNode<S, BV>& node,
/** \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,
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const BVHModel<OBB>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -167,6 +174,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node,
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model2, tf2, node.model2_bv);
......@@ -184,10 +192,11 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node,
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
template<typename S>
bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node,
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBB<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
......@@ -197,6 +206,7 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node,
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model1, tf1, node.model1_bv);
......@@ -212,6 +222,201 @@ bool initialize(ShapeMeshCollisionTraversalNodeOBB<S>& node,
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model2, tf2, node.model2_bv);
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf1.getRotation();
node.T = tf1.getTranslation();
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for RSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model1, tf1, node.model1_bv);
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model2, tf2, node.model2_bv);
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf1.getRotation();
node.T = tf1.getTranslation();
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for kIOS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodekIOS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model1, tf1, node.model1_bv);
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model2, tf2, node.model2_bv);
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf1.getRotation();
node.T = tf1.getTranslation();
return true;
}
/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBBRSS type */
template<typename S, typename NarrowPhaseSolver>
bool initialize(ShapeMeshCollisionTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
computeBV(model1, tf1, node.model1_bv);
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
node.num_max_contacts = num_max_contacts;
node.exhaustive = exhaustive;
node.enable_contact = enable_contact;
node.R = tf2.getRotation();
node.T = tf2.getTranslation();
return true;
}
/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshCollisionTraversalNode<BV>& node,
......@@ -496,6 +701,22 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node,
#endif
/** \brief Initialize traversal node for distance between two geometric shapes */
template<typename S1, typename S2, typename NarrowPhaseSolver>
bool initialize(ShapeDistanceTraversalNode<S1, S2, NarrowPhaseSolver>& node,
const S1& shape1, const SimpleTransform& tf1,
const S2& shape2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver)
{
node.model1 = &shape1;
node.tf1 = tf1;
node.model2 = &shape2;
node.tf2 = tf2;
node.nsolver = nsolver;
return true;
}
/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
template<typename BV>
bool initialize(MeshDistanceTraversalNode<BV>& node,
......@@ -570,6 +791,249 @@ bool initialize(MeshDistanceTraversalNodeOBBRSS& node,
const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2);
template<typename BV, typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNode<BV, S, NarrowPhaseSolver>& node,
BVHModel<BV>& model1, SimpleTransform& tf1,
const S& model2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool use_refit = false, bool refit_bottomup = false)
{
if(model1.getModelType() != BVH_MODEL_TRIANGLES)
return false;
if(!tf1.isIdentity())
{
std::vector<Vec3f> vertices_transformed1(model1.num_vertices);
for(int i = 0; i < model1.num_vertices; ++i)
{
Vec3f& p = model1.vertices[i];
Vec3f new_v = tf1.transform(p);
vertices_transformed1[i] = new_v;
}
model1.beginReplaceModel();
model1.replaceSubModel(vertices_transformed1);
model1.endReplaceModel(use_refit, refit_bottomup);
tf1.setIdentity();
}
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
node.vertices = model1.vertices;
node.tri_indices = model1.tri_indices;
computeBV(model2, tf2, node.model2_bv);
return true;
}
template<typename S, typename BV, typename NarrowPhaseSolver>
bool initialize(ShapeMeshDistanceTraversalNode<S, BV, NarrowPhaseSolver>& node,
const S& model1, const SimpleTransform& tf1,
BVHModel<BV>& model2, SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
bool use_refit = false, bool refit_bottomup = false)
{
if(model2.getModelType() != BVH_MODEL_TRIANGLES)
return false;
if(!tf2.isIdentity())
{
std::vector<Vec3f> vertices_transformed(model2.num_vertices);
for(int i = 0; i < model2.num_vertices; ++i)
{
Vec3f& p = model2.vertices[i];
Vec3f new_v = tf2.transform(p);
vertices_transformed[i] = new_v;
}
model2.beginReplaceModel();
model2.replaceSubModel(vertices_transformed);
model2.endReplaceModel(use_refit, refit_bottomup);
tf2.setIdentity();
}
node.model1 = &model1;
node.tf1 = tf1;
node.model2 = &model2;
node.tf2 = tf2;
node.nsolver = nsolver;
node.vertices = model2.vertices;
node.tri_indices = model2.tri_indices;
computeBV(model1, tf1, node.model1_bv);
return true;
}
template<typename S, typename NarrowPhaseSolver>
bool initialize(MeshShapeDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
const BVHModel<RSS>& model1,