diff --git a/trunk/fcl/include/fcl/collision.h b/trunk/fcl/include/fcl/collision.h
index 2b53a291f464a6647bfd27547c88b3dbfc508606..985d7fe01e33c28bfdea762e6d7c3c408bb56f97 100644
--- a/trunk/fcl/include/fcl/collision.h
+++ b/trunk/fcl/include/fcl/collision.h
@@ -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,
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index f675ffe0e0e80c00cdafbbeab954e1846a4cf034..c8dfa442a1bd0a42ad10db3507753366f350effe 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -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();
diff --git a/trunk/fcl/include/fcl/distance.h b/trunk/fcl/include/fcl/distance.h
index f13fc532574e8a61b21390f8814fb69791ae9faf..5e5285b731e1e49b2cfbf1abe1065f085d31cff2 100644
--- a/trunk/fcl/include/fcl/distance.h
+++ b/trunk/fcl/include/fcl/distance.h
@@ -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
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index 75c899f6c0cba583783dff173ca901cc6df927f2..6f9e05035352e2cbbe4d26f3df64f532c78e7768 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -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();
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
index 8e67c7b301186a655d8f5e0f12891dfb181720a5..fc38a1199ff01fe873e24eac3c42f4eaf34e66d9 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -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);
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
index b14faa8ebc8d956cc511d5a72c1fe101bba67911..cc2658dc3164b36dbbc2b24694a976cae7d0c182 100644
--- a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -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);
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index 736ef0c7565810176bd588f51841ef9dd21f56a8..a0526a4a8931d7e3bc73871b747e08b1adc68397 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -40,259 +40,420 @@
#include "fcl/narrowphase/gjk.h"
#include "fcl/narrowphase/gjk_libccd.h"
+
namespace fcl
{
-/** intersection checking between two shapes */
-template<typename S1, typename S2>
-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)
+struct GJKSolver_libccd
{
- void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
+ /** intersection checking between two shapes */
+ template<typename S1, typename S2>
+ bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ {
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
- bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
- o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
+ max_collision_iterations, collision_tolerance,
+ contact_points, penetration_depth, normal);
- details::GJKInitializer<S1>::deleteGJKObject(o1);
- details::GJKInitializer<S2>::deleteGJKObject(o2);
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
- return res;
-}
+ return res;
+ }
-/** \brief intersection checking between one shape and a triangle */
-template<typename S>
-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 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3);
+ /** \brief intersection checking between one shape and a triangle */
+ template<typename S>
+ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
+ {
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3);
- bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
- o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ max_collision_iterations, collision_tolerance,
+ contact_points, penetration_depth, normal);
- details::GJKInitializer<S>::deleteGJKObject(o1);
- details::triDeleteGJKObject(o2);
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
- return res;
-}
+ return res;
+ }
-/** \brief intersection checking between one shape and a triangle with transformation */
-template<typename S>
-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 = details::GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
+ /** \brief intersection checking between one shape and a triangle with transformation */
+ template<typename S>
+ 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, BVH_REAL* penetration_depth, Vec3f* normal) const
+ {
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
- bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
- o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ max_collision_iterations, collision_tolerance,
+ contact_points, penetration_depth, normal);
- details::GJKInitializer<S>::deleteGJKObject(o1);
- details::triDeleteGJKObject(o2);
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
- return res;
-}
+ return res;
+ }
-/** \brief distance computation between two shapes */
-template<typename S1, typename S2>
-bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* dist)
-{
- void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
+ /** \brief distance computation between two shapes */
+ template<typename S1, typename S2>
+ bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ BVH_REAL* dist) const
+ {
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
- bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
- o2, details::GJKInitializer<S2>::getSupportFunction(),
- dist);
+ bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(),
+ max_distance_iterations, distance_tolerance,
+ dist);
- if(*dist > 0) *dist = std::sqrt(*dist);
+ if(*dist > 0) *dist = std::sqrt(*dist);
- details::GJKInitializer<S1>::deleteGJKObject(o1);
- details::GJKInitializer<S2>::deleteGJKObject(o2);
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
- return res;
-}
+ return res;
+ }
-}
+ /** \brief distance computation between one shape and a triangle */
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ BVH_REAL* dist) const
+ {
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3);
+
+ bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
+ o2, details::triGetSupportFunction(),
+ max_distance_iterations, distance_tolerance,
+ dist);
+ if(*dist > 0) *dist = std::sqrt(*dist);
+
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
-namespace fcl
-{
+ return res;
+ }
+
+ /** \brief distance computation between one shape and a triangle with transformation */
+ template<typename S>
+ bool shapeTriangleDistance(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ BVH_REAL* dist) const
+ {
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
+
+ bool res = details::GJKDistance(o1, details::GJKInitializer<S>::getSupportFunction(),
+ o2, details::triGetSupportFunction(),
+ max_distance_iterations, distance_tolerance,
+ dist);
+
+ if(*dist > 0) *dist = std::sqrt(*dist);
+
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
+
+ return res;
+ }
+
+
+ GJKSolver_libccd()
+ {
+ max_collision_iterations = 500;
+ max_distance_iterations = 500;
+ collision_tolerance = 1e-6;
+ distance_tolerance = 1e-6;
+ }
+
+ unsigned int max_collision_iterations;
+ unsigned int max_distance_iterations;
+ BVH_REAL collision_tolerance;
+ BVH_REAL distance_tolerance;
+
+};
-template<typename S1, typename S2>
-bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* distance)
+
+
+struct GJKSolver_indep
{
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- if(gjk_status == details::GJK::Valid)
+ template<typename S1, typename S2>
+ bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
{
- Vec3f w0, w1;
- for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
{
- BVH_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ case details::GJK::Inside:
+ {
+ details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
}
- *distance = (w0 - w1).length();
- return true;
+ return false;
}
- else
+
+
+ template<typename S>
+ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const
{
- *distance = -1;
+ Triangle2 tri(P1, P2, P3);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf.getRotation();
+ shape.toshape0 = tf.inverse();
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
return false;
}
-}
-template<typename S1, typename S2>
-bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
+ template<typename S>
+ 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, BVH_REAL* penetration_depth, Vec3f* normal) const
{
- case details::GJK::Inside:
+ Triangle2 tri(P1, P2, P3);
+ SimpleTransform tf2(R, T);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
+ shape.toshape0 = tf.inverseTimes(tf2);
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
{
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
+ case details::GJK::Inside:
{
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
+ details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
{
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
}
- if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
- return true;
+ else return false;
}
- else return false;
+ break;
+ default:
+ ;
}
- break;
- default:
- ;
+
+ return false;
}
- return false;
-}
+
+ template<typename S1, typename S2>
+ bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ BVH_REAL* distance) const
+ {
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(gjk_status == details::GJK::Valid)
+ {
+ Vec3f w0, w1;
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+ {
+ BVH_REAL p = gjk.getSimplex()->p[i];
+ w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ }
+
+ *distance = (w0 - w1).length();
+ return true;
+ }
+ else
+ {
+ *distance = -1;
+ return false;
+ }
+ }
-template<typename S>
-bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+
+
+ template<typename S>
+ bool shapeTriangleDistance(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)
-{
- Triangle2 tri(P1, P2, P3);
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s;
- shape.shapes[1] = &tri;
- shape.toshape1 = tf.getRotation();
- shape.toshape0 = tf.inverse();
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
+ BVH_REAL* distance) const
{
- case details::GJK::Inside:
+ Triangle2 tri(P1, P2, P3);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf.getRotation();
+ shape.toshape0 = tf.inverse();
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(gjk_status == details::GJK::Valid)
{
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
+ Vec3f w0, w1;
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
- return true;
+ BVH_REAL p = gjk.getSimplex()->p[i];
+ w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- else return false;
+
+ *distance = (w0 - w1).length();
+ return true;
+ }
+ else
+ {
+ *distance = -1;
+ return false;
}
- break;
- default:
- ;
}
- return false;
-}
-
-template<typename S>
-bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+ template<typename S>
+ bool shapeTriangleDistance(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)
-{
- Triangle2 tri(P1, P2, P3);
- SimpleTransform tf2(R, T);
- Vec3f guess(1, 0, 0);
- details::MinkowskiDiff shape;
- shape.shapes[0] = &s;
- shape.shapes[1] = &tri;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
- shape.toshape0 = tf.inverseTimes(tf2);
-
- details::GJK gjk;
- details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
+ BVH_REAL* distance) const
{
- case details::GJK::Inside:
+ Triangle2 tri(P1, P2, P3);
+ SimpleTransform tf2(R, T);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
+ shape.toshape0 = tf.inverseTimes(tf2);
+
+ details::GJK gjk(gjk_max_iterations, gjk_tolerance);
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(gjk_status == details::GJK::Valid)
{
- details::EPA epa;
- details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != details::EPA::Failed)
+ Vec3f w0, w1;
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
- return true;
+ BVH_REAL p = gjk.getSimplex()->p[i];
+ w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
}
- else return false;
+
+ *distance = (w0 - w1).length();
+ return true;
+ }
+ else
+ {
+ *distance = -1;
+ return false;
}
- break;
- default:
- ;
}
- return false;
-}
+
+ GJKSolver_indep()
+ {
+ gjk_max_iterations = 128;
+ gjk_tolerance = 1e-6;
+ epa_max_face_num = 128;
+ epa_max_vertex_num = 64;
+ epa_max_iterations = 255;
+ epa_tolerance = 1e-6;
+ }
+
+ unsigned int epa_max_face_num;
+ unsigned int epa_max_vertex_num;
+ unsigned int epa_max_iterations;
+ BVH_REAL epa_tolerance;
+ BVH_REAL gjk_tolerance;
+ BVH_REAL gjk_max_iterations;
+};
+
+
}
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 35369ea8d8f2edc1b0a09e8eb0e139d0d0bab29b..ee9647b35a0817c50c3be2c16a82819ae7992dcd 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -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, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf1.getRotation();
+ node.T = tf1.getTranslation();
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshDistanceTraversalNodeRSS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<RSS>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf2.getRotation();
+ node.T = tf2.getTranslation();
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(MeshShapeDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+ const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf1.getRotation();
+ node.T = tf1.getTranslation();
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshDistanceTraversalNodekIOS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf2.getRotation();
+ node.T = tf2.getTranslation();
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(MeshShapeDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const BVHModel<OBBRSS>& model1, const SimpleTransform& tf1,
+ const S& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf1.getRotation();
+ node.T = tf1.getTranslation();
+
+ return true;
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+bool initialize(ShapeMeshDistanceTraversalNodeOBBRSS<S, NarrowPhaseSolver>& node,
+ const S& model1, const SimpleTransform& tf1,
+ const BVHModel<OBBRSS>& model2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ 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.R = tf2.getRotation();
+ node.T = tf2.getTranslation();
+
+ return true;
+}
+
+
+
+
/** \brief Initialize traversal node for continuous collision detection between two meshes */
template<typename BV>
bool initialize(MeshContinuousCollisionTraversalNode<BV>& node,
diff --git a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
index 6a9ffbbbb893b9cb8f490b10f72810eafab21b8f..ccbeae927ecc1ea67acc9f03a64b32ba645be510 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvh_shape.h
@@ -50,7 +50,7 @@ template<typename BV, typename S>
class BVHShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- BVHShapeCollisionTraversalNode()
+ BVHShapeCollisionTraversalNode() : CollisionTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
@@ -95,7 +95,7 @@ template<typename S, typename BV>
class ShapeBVHCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
- ShapeBVHCollisionTraversalNode()
+ ShapeBVHCollisionTraversalNode() : CollisionTraversalNodeBase()
{
model1 = NULL;
model2 = NULL;
@@ -174,7 +174,7 @@ struct BVHShapeCollisionPairComp
};
-template<typename BV, typename S>
+template<typename BV, typename S, typename NarrowPhaseSolver>
class MeshShapeCollisionTraversalNode : public BVHShapeCollisionTraversalNode<BV, S>
{
public:
@@ -186,6 +186,8 @@ public:
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
+
+ nsolver = NULL;
}
void leafTesting(int b1, int b2) const
@@ -207,14 +209,14 @@ public:
if(!enable_contact) // only interested in collision or not
{
- if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3))
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, NULL, NULL, NULL))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id));
}
}
else
{
- if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
+ if(nsolver->shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, &contactp, &penetration, &normal))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
@@ -234,13 +236,62 @@ public:
bool enable_contact;
mutable std::vector<BVHShapeCollisionPair> pairs;
+
+ const NarrowPhaseSolver* nsolver;
};
-template<typename S>
-class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S>
+
+template<typename BV, typename S, typename NarrowPhaseSolver>
+static inline void meshShapeCollisionLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const S& model2,
+ Vec3f* vertices, Triangle* tri_indices,
+ const Matrix3f& R, const Vec3f& T,
+ const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ bool enable_statistics,
+ bool enable_contact,
+ bool exhaustive,
+ int num_max_contacts,
+ int& num_leaf_tests,
+ std::vector<BVHShapeCollisionPair>& pairs)
+
+{
+ if(enable_statistics) num_leaf_tests++;
+ const BVNode<BV>& node = model1->getBV(b1);
+
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = tri_indices[primitive_id];
+
+ const Vec3f& p1 = vertices[tri_id[0]];
+ const Vec3f& p2 = vertices[tri_id[1]];
+ const Vec3f& p3 = vertices[tri_id[2]];
+
+ BVH_REAL penetration;
+ Vec3f normal;
+ Vec3f contactp;
+
+ if(!enable_contact) // only interested in collision or not
+ {
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, NULL, NULL, NULL))
+ {
+ pairs.push_back(BVHShapeCollisionPair(primitive_id));
+ }
+ }
+ else
+ {
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
+ {
+ pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
+ }
+ }
+}
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeCollisionTraversalNodeOBB : public MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>
{
public:
- MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S>()
+ MeshShapeCollisionTraversalNodeOBB() : MeshShapeCollisionTraversalNode<OBB, S, NarrowPhaseSolver>()
{
R.setIdentity();
}
@@ -253,44 +304,98 @@ public:
void leafTesting(int b1, int b2) const
{
- if(this->enable_statistics) this->num_leaf_tests++;
- const BVNode<OBB>& node = this->model1->getBV(b1);
+ meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ }
- int primitive_id = node.primitiveId();
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
- const Triangle& tri_id = this->tri_indices[primitive_id];
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeCollisionTraversalNodeRSS : public MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeCollisionTraversalNodeRSS() : MeshShapeCollisionTraversalNode<RSS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
- const Vec3f& p1 = this->vertices[tri_id[0]];
- const Vec3f& p2 = this->vertices[tri_id[1]];
- const Vec3f& p3 = this->vertices[tri_id[2]];
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
- BVH_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ }
- if(!this->enable_contact) // only interested in collision or not
- {
- if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, R, T))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
- }
- }
- else
- {
- if(shapeTriangleIntersect(*(this->model2), this->tf2, p1, p2, p3, R, T, &contactp, &penetration, &normal))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
- }
- }
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeCollisionTraversalNodekIOS : public MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeCollisionTraversalNodekIOS() : MeshShapeCollisionTraversalNode<kIOS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
}
- // R, T is the transformation of bvh model
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ }
+
+ // R, T is the transform of bvh model
Matrix3f R;
Vec3f T;
};
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeCollisionTraversalNodeOBBRSS : public MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeCollisionTraversalNodeOBBRSS() : MeshShapeCollisionTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
-template<typename S, typename BV>
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+ }
+
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
class ShapeMeshCollisionTraversalNode : public ShapeBVHCollisionTraversalNode<S, BV>
{
public:
@@ -302,6 +407,8 @@ public:
num_max_contacts = 1;
exhaustive = false;
enable_contact = false;
+
+ nsolver = NULL;
}
void leafTesting(int b1, int b2) const
@@ -323,14 +430,14 @@ public:
if(!enable_contact) // only interested in collision or not
{
- if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3))
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, NULL, NULL, NULL))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id));
}
}
else
{
- if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
+ if(nsolver->shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, &contactp, &penetration, &normal))
{
pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
}
@@ -350,13 +457,15 @@ public:
bool enable_contact;
mutable std::vector<BVHShapeCollisionPair> pairs;
+
+ const NarrowPhaseSolver* nsolver;
};
-template<typename S>
-class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB>
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshCollisionTraversalNodeOBB : public ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>
{
public:
- ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB>()
+ ShapeMeshCollisionTraversalNodeOBB() : ShapeMeshCollisionTraversalNode<S, OBB, NarrowPhaseSolver>()
{
R.setIdentity();
}
@@ -369,45 +478,599 @@ public:
void leafTesting(int b1, int b2) const
{
- if(this->enable_statistics) this->num_leaf_tests++;
- const BVNode<OBB>& node = this->model2->getBV(b2);
+ meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
- int primitive_id = node.primitiveId();
+ // may need to change the order in pairs
+ }
- const Triangle& tri_id = this->tri_indices[primitive_id];
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
- const Vec3f& p1 = this->vertices[tri_id[0]];
- const Vec3f& p2 = this->vertices[tri_id[1]];
- const Vec3f& p3 = this->vertices[tri_id[2]];
- BVH_REAL penetration;
- Vec3f normal;
- Vec3f contactp;
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshCollisionTraversalNodeRSS : public ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshCollisionTraversalNodeRSS() : ShapeMeshCollisionTraversalNode<S, RSS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+
+ // may need to change the order in pairs
+ }
+
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
+
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshCollisionTraversalNodekIOS : public ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshCollisionTraversalNodekIOS() : ShapeMeshCollisionTraversalNode<S, kIOS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+
+ // may need to change the order in pairs
+ }
+
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
+
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshCollisionTraversalNodeOBBRSS : public ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshCollisionTraversalNodeOBBRSS() : ShapeMeshCollisionTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ bool BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return !overlap(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeCollisionLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->enable_contact,
+ this->exhaustive, this->num_max_contacts, this->num_leaf_tests, this->pairs);
+
+ // may need to change the order in pairs
+ }
+
+ // R, T is the transform of bvh model
+ Matrix3f R;
+ Vec3f T;
+};
+
+
+template<typename BV, typename S>
+class BVHShapeDistanceTraversalNode : public DistanceTraversalNodeBase
+{
+public:
+ BVHShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
+ {
+ model1 = NULL;
+ model2 = NULL;
+
+ num_bv_tests = 0;
+ num_leaf_tests = 0;
+ query_time_seconds = 0.0;
+ }
+
+ bool isFirstNodeLeaf(int b) const
+ {
+ return model1->getBV(b).isLeaf();
+ }
+
+ int getFirstLeftChild(int b) const
+ {
+ return model1->getBV(b).leftChild();
+ }
+
+ int getFirstRightChild(int b) const
+ {
+ return model1->getBV(b).rightChild();
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ return model1->getBV(b1).bv.distance(model2_bv);
+ }
+
+ const BVHModel<BV>* model1;
+ const S* model2;
+ BV model2_bv;
+
+ mutable int num_bv_tests;
+ mutable int num_leaf_tests;
+ mutable BVH_REAL query_time_seconds;
+};
+
+template<typename S, typename BV>
+class ShapeBVHDistanceTraversalNode : public DistanceTraversalNodeBase
+{
+public:
+ ShapeBVHDistanceTraversalNode() : DistanceTraversalNodeBase()
+ {
+ model1 = NULL;
+ model2 = NULL;
+
+ num_bv_tests = 0;
+ num_leaf_tests = 0;
+ query_time_seconds = 0.0;
+ }
+
+ bool isSecondNodeLeaf(int b) const
+ {
+ return model2->getBV(b).isLeaf();
+ }
+
+ int getSecondLeftChild(int b) const
+ {
+ return model2->getBV(b).leftChild();
+ }
+
+ int getSecondRightChild(int b) const
+ {
+ return model2->getBV(b).rightChild();
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ return model1_bv.distance(model2->getBV(b2).bv);
+ }
- if(!this->enable_contact) // only interested in collision or not
+ const S* model1;
+ const BVHModel<BV>* model2;
+ BV model1_bv;
+
+ mutable int num_bv_tests;
+ mutable int num_leaf_tests;
+ mutable BVH_REAL query_time_seconds;
+};
+
+
+
+template<typename BV, typename S, typename NarrowPhaseSolver>
+class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode<BV, S>
+{
+public:
+ MeshShapeDistanceTraversalNode() : BVHShapeDistanceTraversalNode<BV, S>()
+ {
+ vertices = NULL;
+ tri_indices = NULL;
+
+ last_tri_id = 0;
+
+ rel_err = 0;
+ abs_err = 0;
+
+ min_distance = std::numeric_limits<BVH_REAL>::max();
+
+ nsolver = NULL;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_leaf_tests++;
+
+ const BVNode<BV>& node = this->model1->getBV(b1);
+
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = tri_indices[primitive_id];
+
+ const Vec3f& p1 = vertices[tri_id[0]];
+ const Vec3f& p2 = vertices[tri_id[1]];
+ const Vec3f& p3 = vertices[tri_id[2]];
+
+ BVH_REAL d;
+ nsolver->shapeTriangleDistance(*(this->model2), this->tf2, p1, p2, p3, &d);
+
+ if(d < min_distance)
{
- if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, R, T))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id));
- }
+ min_distance = d;
+
+ last_tri_id = primitive_id;
}
- else
+ }
+
+ bool canStop(BVH_REAL c) const
+ {
+ if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
+ return true;
+ return false;
+ }
+
+ Vec3f* vertices;
+ Triangle* tri_indices;
+
+ BVH_REAL rel_err;
+ BVH_REAL abs_err;
+
+ mutable BVH_REAL min_distance;
+ mutable int last_tri_id;
+
+ const NarrowPhaseSolver* nsolver;
+};
+
+
+template<typename BV, typename S, typename NarrowPhaseSolver>
+void meshShapeDistanceLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const S& model2,
+ Vec3f* vertices, Triangle* tri_indices,
+ const Matrix3f&R, const Vec3f& T,
+ const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ bool enable_statistics,
+ int & num_leaf_tests,
+ int& last_tri_id,
+ BVH_REAL& min_distance)
+{
+ if(enable_statistics) num_leaf_tests++;
+
+ const BVNode<BV>& node = model1->getBV(b1);
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = tri_indices[primitive_id];
+ const Vec3f& p1 = vertices[tri_id[0]];
+ const Vec3f& p2 = vertices[tri_id[1]];
+ const Vec3f& p3 = vertices[tri_id[2]];
+
+ BVH_REAL dist;
+
+ nsolver->shapeTriangleDistance(model2, tf2, p1, p2, p3, R, T, &dist);
+
+ if(dist < min_distance)
+ {
+ min_distance = dist;
+
+ last_tri_id = primitive_id;
+ }
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+static inline void distance_preprocess(Vec3f* vertices, Triangle* tri_indices, int last_tri_id,
+ const Matrix3f& R, const Vec3f& T,
+ const S& s, const SimpleTransform& tf,
+ const NarrowPhaseSolver* nsolver,
+ BVH_REAL& min_distance)
+{
+ const Triangle& last_tri = tri_indices[last_tri_id];
+
+ const Vec3f& p1 = vertices[last_tri[0]];
+ const Vec3f& p2 = vertices[last_tri[1]];
+ const Vec3f& p3 = vertices[last_tri[2]];
+
+ Vec3f last_tri_P, last_tri_Q;
+
+ BVH_REAL dist;
+ nsolver->shapeTriangleDistance(s, tf, p1, p2, p3, R, T, &dist);
+
+ min_distance = dist;
+}
+
+
+static inline void distance_postprocess(const Matrix3f& R, const Vec3f& T, Vec3f& p2)
+{
+ Vec3f u = p2 - T;
+ p2 = R.transposeTimes(u);
+}
+
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeDistanceTraversalNodeRSS : public MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeDistanceTraversalNodeRSS() : MeshShapeDistanceTraversalNode<RSS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ }
+
+ void postprocess()
+ {
+
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
+ Matrix3f R;
+ Vec3f T;
+};
+
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeDistanceTraversalNodekIOS : public MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeDistanceTraversalNodekIOS() : MeshShapeDistanceTraversalNode<kIOS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ }
+
+ void postprocess()
+ {
+
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
+ Matrix3f R;
+ Vec3f T;
+};
+
+template<typename S, typename NarrowPhaseSolver>
+class MeshShapeDistanceTraversalNodeOBBRSS : public MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>
+{
+public:
+ MeshShapeDistanceTraversalNodeOBBRSS() : MeshShapeDistanceTraversalNode<OBBRSS, S, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model2), this->tf2, this->nsolver, this->min_distance);
+ }
+
+ void postprocess()
+ {
+
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model2_bv, this->model1->getBV(b1).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ R, T, this->tf2, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
+ Matrix3f R;
+ Vec3f T;
+};
+
+
+template<typename S, typename BV, typename NarrowPhaseSolver>
+class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode<S, BV>
+{
+public:
+ ShapeMeshDistanceTraversalNode() : ShapeBVHDistanceTraversalNode<S, BV>()
+ {
+ vertices = NULL;
+ tri_indices = NULL;
+
+ last_tri_id = 0;
+
+ rel_err = 0;
+ abs_err = 0;
+
+ min_distance = std::numeric_limits<BVH_REAL>::max();
+
+ nsolver = NULL;
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_leaf_tests++;
+
+ const BVNode<BV>& node = this->model2->getBV(b2);
+
+ int primitive_id = node.primitiveId();
+
+ const Triangle& tri_id = tri_indices[primitive_id];
+
+ const Vec3f& p1 = vertices[tri_id[0]];
+ const Vec3f& p2 = vertices[tri_id[1]];
+ const Vec3f& p3 = vertices[tri_id[2]];
+
+ BVH_REAL d;
+ nsolver->shapeTriangleDistance(*(this->model1), this->tf1, p1, p2, p3, &d);
+
+ if(d < min_distance)
{
- if(shapeTriangleIntersect(*(this->model1), this->tf1, p1, p2, p3, R, T, &contactp, &penetration, &normal))
- {
- this->pairs.push_back(BVHShapeCollisionPair(primitive_id, normal, contactp, penetration));
- }
+ min_distance = d;
+
+ last_tri_id = primitive_id;
}
}
- // R, T is the transformation of bvh model
+ bool canStop(BVH_REAL c) const
+ {
+ if((c >= min_distance - abs_err) && (c * (1 + rel_err) >= min_distance))
+ return true;
+ return false;
+ }
+
+ Vec3f* vertices;
+ Triangle* tri_indices;
+
+ BVH_REAL rel_err;
+ BVH_REAL abs_err;
+
+ mutable BVH_REAL min_distance;
+ mutable int last_tri_id;
+
+ const NarrowPhaseSolver* nsolver;
+};
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshDistanceTraversalNodeRSS : public ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshDistanceTraversalNodeRSS() : ShapeMeshDistanceTraversalNode<S, RSS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ }
+
+ void postprocess()
+ {
+
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
Matrix3f R;
Vec3f T;
};
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshDistanceTraversalNodekIOS : public ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshDistanceTraversalNodekIOS() : ShapeMeshDistanceTraversalNode<S, kIOS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ }
+ void postprocess()
+ {
+
+ }
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
+ Matrix3f R;
+ Vec3f T;
+};
+
+template<typename S, typename NarrowPhaseSolver>
+class ShapeMeshDistanceTraversalNodeOBBRSS : public ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>
+{
+public:
+ ShapeMeshDistanceTraversalNodeOBBRSS() : ShapeMeshDistanceTraversalNode<S, OBBRSS, NarrowPhaseSolver>()
+ {
+ R.setIdentity();
+ }
+
+ void preprocess()
+ {
+ distance_preprocess(this->vertices, this->tri_indices, this->last_tri_id, R, T, *(this->model1), this->tf1, this->nsolver, this->min_distance);
+ }
+
+ void postprocess()
+ {
+
+ }
+
+ BVH_REAL BVTesting(int b1, int b2) const
+ {
+ if(this->enable_statistics) this->num_bv_tests++;
+ return distance(R, T, this->model1_bv, this->model2->getBV(b2).bv);
+ }
+
+ void leafTesting(int b1, int b2) const
+ {
+ meshShapeDistanceLeafTesting(b2, b1, this->model2, *(this->model1), this->vertices, this->tri_indices,
+ R, T, this->tf1, this->nsolver, this->enable_statistics, this->num_leaf_tests, this->last_tri_id, this->min_distance);
+ }
+
+ Matrix3f R;
+ Vec3f T;
+};
}
#endif
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 0fde38b951fa446827aa81615752bfac6571b1b9..8063fea9f60ade1614d61642df6d4216152a1970 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -44,7 +44,7 @@
namespace fcl
{
-template<typename S1, typename S2>
+template<typename S1, typename S2, typename NarrowPhaseSolver>
class ShapeCollisionTraversalNode : public CollisionTraversalNodeBase
{
public:
@@ -55,6 +55,8 @@ public:
enable_contact = false;
is_collision = false;
+
+ nsolver = NULL;
}
bool BVTesting(int, int) const
@@ -65,9 +67,9 @@ public:
void leafTesting(int, int) const
{
if(enable_contact)
- is_collision = shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
+ is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, &contact_point, &penetration_depth, &normal);
else
- is_collision = shapeIntersect(*model1, tf1, *model2, tf2);
+ is_collision = nsolver->shapeIntersect(*model1, tf1, *model2, tf2, NULL, NULL, NULL);
}
const S1* model1;
@@ -83,7 +85,45 @@ public:
bool enable_contact;
mutable bool is_collision;
- };
+
+ const NarrowPhaseSolver* nsolver;
+};
+
+template<typename S1, typename S2, typename NarrowPhaseSolver>
+class ShapeDistanceTraversalNode : public DistanceTraversalNodeBase
+{
+public:
+ ShapeDistanceTraversalNode() : DistanceTraversalNodeBase()
+ {
+ model1 = NULL;
+ model2 = NULL;
+
+ nsolver = NULL;
+ }
+
+ BVH_REAL BVTesting(int, int) const
+ {
+ return -1; // should not be used
+ }
+
+ void leafTesting(int, int) const
+ {
+ is_collision = !nsolver->shapeDistance(*model1, tf1, *model2, tf2, &min_distance);
+ }
+
+ const S1* model1;
+ const S2* model2;
+
+ mutable BVH_REAL min_distance;
+ mutable Vec3f p1;
+ mutable Vec3f p2;
+
+ mutable bool is_collision;
+
+ const NarrowPhaseSolver* nsolver;
+};
+
+
}
#endif
diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp
index fc2d2542053771e8b421ad1704295683508529c8..c741d8c850cc22d8c3180954628efbcdcbeec5c8 100644
--- a/trunk/fcl/src/collision.cpp
+++ b/trunk/fcl/src/collision.cpp
@@ -37,62 +37,118 @@
#include "fcl/collision.h"
#include "fcl/collision_func_matrix.h"
+#include "fcl/narrowphase/narrowphase.h"
#include <iostream>
namespace fcl
{
-static CollisionFunctionMatrix CollisionFunctionLookTable;
-
+static CollisionFunctionMatrix<GJKSolver_libccd> CollisionFunctionLookTable_libccd;
+static CollisionFunctionMatrix<GJKSolver_indep> CollisionFunctionLookTable_indep;
+template<typename NarrowPhaseSolver>
int collide(const CollisionObject* o1, const CollisionObject* o2,
+ const NarrowPhaseSolver* nsolver,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts)
{
return collide(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(),
+ nsolver,
num_max_contacts, exhaustive, enable_contact, contacts);
}
+template<typename NarrowPhaseSolver>
+void* getCollisionLookTable() { return NULL; }
+
+template<>
+void* getCollisionLookTable<GJKSolver_libccd>()
+{
+ return &CollisionFunctionLookTable_libccd;
+}
+
+template<>
+void* getCollisionLookTable<GJKSolver_indep>()
+{
+ return &CollisionFunctionLookTable_indep;
+}
+
+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)
{
+ const NarrowPhaseSolver* nsolver = nsolver_;
+ if(!nsolver_)
+ nsolver = new NarrowPhaseSolver();
+
+ const CollisionFunctionMatrix<NarrowPhaseSolver>* looktable = static_cast<const CollisionFunctionMatrix<NarrowPhaseSolver>*>(getCollisionLookTable<NarrowPhaseSolver>());
+
+ int res;
if(num_max_contacts <= 0 && !exhaustive)
{
std::cerr << "Warning: should stop early as num_max_contact is " << num_max_contacts << " !" << std::endl;
- return 0;
- }
-
- OBJECT_TYPE object_type1 = o1->getObjectType();
- OBJECT_TYPE object_type2 = o2->getObjectType();
- NODE_TYPE node_type1 = o1->getNodeType();
- NODE_TYPE node_type2 = o2->getNodeType();
-
- if(object_type1 == OT_GEOM & object_type2 == OT_BVH)
- {
- if(!CollisionFunctionLookTable.collision_matrix[node_type2][node_type1])
- {
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
- }
-
- return CollisionFunctionLookTable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, num_max_contacts, exhaustive, enable_contact, contacts);
+ res = 0;
}
else
{
- if(!CollisionFunctionLookTable.collision_matrix[node_type1][node_type2])
+ OBJECT_TYPE object_type1 = o1->getObjectType();
+ OBJECT_TYPE object_type2 = o2->getObjectType();
+ NODE_TYPE node_type1 = o1->getNodeType();
+ NODE_TYPE node_type2 = o2->getNodeType();
+
+ if(object_type1 == OT_GEOM & object_type2 == OT_BVH)
+ {
+ if(!looktable->collision_matrix[node_type2][node_type1])
+ {
+ std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+ res = 0;
+ }
+ else
+ res = looktable->collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, num_max_contacts, exhaustive, enable_contact, contacts);
+ }
+ else
{
- std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
- return 0;
+ if(!looktable->collision_matrix[node_type1][node_type2])
+ {
+ std::cerr << "Warning: collision function between node type " << node_type1 << " and node type " << node_type2 << " is not supported"<< std::endl;
+ res = 0;
+ }
+ else
+ res = looktable->collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact, contacts);
}
-
- return CollisionFunctionLookTable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
}
+ if(!nsolver_)
+ delete nsolver;
+
+ return res;
+}
+
+template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+template int collide(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
+template int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* 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)
+{
+ GJKSolver_libccd solver;
+ return collide<GJKSolver_libccd>(o1, o2, &solver, num_max_contacts, exhaustive, enable_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)
+{
+ GJKSolver_libccd solver;
+ return collide<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver, num_max_contacts, exhaustive, enable_contact, contacts);
+}
}
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 001102eaca56abbe1d05b1ffbd0ac78369ceb90f..669d0101f27aeb4c61dc9dd3a92474f55124c2c0 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -48,13 +48,15 @@
namespace fcl
{
-template<typename T_SH1, typename T_SH2>
-int ShapeShapeCollide(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 T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+int ShapeShapeCollide(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)
{
- ShapeCollisionTraversalNode<T_SH1, T_SH2> node;
+ ShapeCollisionTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, enable_contact);
collide(&node);
if(!node.is_collision) return 0;
contacts.resize(1);
@@ -89,18 +91,20 @@ static inline int BVHShapeContactCollection(const std::vector<BVHShapeCollisionP
}
-template<typename T_BVH, typename T_SH>
+template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
struct BVHShapeCollider
{
- static 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)
+ static 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)
{
- MeshShapeCollisionTraversalNode<T_BVH, T_SH> node;
+ MeshShapeCollisionTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
SimpleTransform tf1_tmp = tf1;
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
fcl::collide(&node);
int num_contacts = BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
@@ -111,33 +115,37 @@ struct BVHShapeCollider
};
-template<typename T_SH>
-struct BVHShapeCollider<OBB, T_SH>
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeCollider<OBB, T_SH, NarrowPhaseSolver>
{
- static 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)
+ static 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)
{
- MeshShapeCollisionTraversalNodeOBB<T_SH> node;
+ MeshShapeCollisionTraversalNodeOBB<T_SH, NarrowPhaseSolver> node;
const BVHModel<OBB>* obj1 = static_cast<const BVHModel<OBB>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
fcl::collide(&node);
return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
}
};
-/*
-template<typename T_SH>
-struct BVHShapeCollider<RSS, T_SH>
+
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeCollider<RSS, T_SH, NarrowPhaseSolver>
{
- static 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)
+ static 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)
{
- MeshShapeCollisionTraversalNodeRSS<T_SH> node;
+ MeshShapeCollisionTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
fcl::collide(&node);
return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
@@ -145,23 +153,43 @@ struct BVHShapeCollider<RSS, T_SH>
};
-template<typename T_SH>
-struct BVHShapeCollider<kIOS, T_SH>
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeCollider<kIOS, T_SH, NarrowPhaseSolver>
{
- static 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)
+ static 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)
{
- MeshShapeCollisionTraversalNodekIOS<T_SH> node;
+ MeshShapeCollisionTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
const T_SH* obj2 = static_cast<const T_SH*>(o2);
- initialize(node, *obj1, tf1, *obj2, tf2, num_max_contacts, exhaustive, enable_contact);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
+ fcl::collide(&node);
+
+ return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
+ }
+};
+
+
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeCollider<OBBRSS, T_SH, NarrowPhaseSolver>
+{
+ static 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)
+ {
+ MeshShapeCollisionTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
+ const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact);
fcl::collide(&node);
return BVHShapeContactCollection(node.pairs, obj1, obj2, num_max_contacts, exhaustive, enable_contact, contacts);
}
};
-*/
template<typename T_BVH>
static inline int BVHContactCollection(const std::vector<BVHCollisionPair>& pairs, const BVHModel<T_BVH>* obj1, const BVHModel<T_BVH>* obj2, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
@@ -277,7 +305,17 @@ int BVHCollide<kIOS>(const CollisionGeometry* o1, const SimpleTransform& tf1, co
}
-CollisionFunctionMatrix::CollisionFunctionMatrix()
+template<typename T_BVH, typename NarrowPhaseSolver>
+int BVHCollide(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)
+{
+ return BVHCollide<T_BVH>(o1, tf1, o2, tf2, num_max_contacts, exhaustive, enable_contact, contacts);
+}
+
+
+template<typename NarrowPhaseSolver>
+CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
{
for(int i = 0; i < 16; ++i)
{
@@ -285,134 +323,139 @@ CollisionFunctionMatrix::CollisionFunctionMatrix()
collision_matrix[i][j] = NULL;
}
- collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box>;
- collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere>;
- collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule>;
- collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone>;
- collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder>;
- collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex>;
- collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane>;
-
- collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box>;
- collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere>;
- collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule>;
- collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone>;
- collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder>;
- collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex>;
- collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane>;
-
- collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box>;
- collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere>;
- collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule>;
- collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone>;
- collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder>;
- collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex>;
- collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane>;
-
- collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box>;
- collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere>;
- collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule>;
- collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone>;
- collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder>;
- collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex>;
- collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane>;
-
- collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box>;
- collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere>;
- collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule>;
- collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone>;
- collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder>;
- collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex>;
- collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane>;
-
- collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box>;
- collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere>;
- collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule>;
- collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone>;
- collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder>;
- collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex>;
- collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane>;
-
- collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box>;
- collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere>;
- collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule>;
- collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone>;
- collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder>;
- collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex>;
- collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane>;
-
- collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box>::collide;
- collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere>::collide;
- collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule>::collide;
- collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone>::collide;
- collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder>::collide;
- collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex>::collide;
- collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane>::collide;
-
- collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box>::collide;
- collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere>::collide;
- collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule>::collide;
- collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone>::collide;
- collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder>::collide;
- collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex>::collide;
- collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane>::collide;
-
- collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box>::collide;
- collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere>::collide;
- collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule>::collide;
- collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone>::collide;
- collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder>::collide;
- collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex>::collide;
- collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane>::collide;
-
- collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box>::collide;
- collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere>::collide;
- collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule>::collide;
- collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone>::collide;
- collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder>::collide;
- collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex>::collide;
- collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane>::collide;
-
- collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box>::collide;
- collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere>::collide;
- collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule>::collide;
- collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone>::collide;
- collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder>::collide;
- collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex>::collide;
- collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane>::collide;
-
- collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box>::collide;
- collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere>::collide;
- collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule>::collide;
- collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone>::collide;
- collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder>::collide;
- collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex>::collide;
- collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane>::collide;
-
- collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box>::collide;
- collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere>::collide;
- collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule>::collide;
- collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone>::collide;
- collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder>::collide;
- collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex>::collide;
- collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane>::collide;
-
- collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box>::collide;
- collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder>::collide;
- collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex>::collide;
- collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane>::collide;
-
- collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB>;
- collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB>;
- collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS>;
- collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16> >;
- collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18> >;
- collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24> >;
- collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS>;
- collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS>;
+ collision_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeCollide<Box, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeCollide<Box, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeCollide<Box, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeCollide<Box, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeCollide<Box, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeCollide<Box, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeCollide<Box, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeCollide<Sphere, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeCollide<Sphere, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeCollide<Sphere, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeCollide<Sphere, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeCollide<Sphere, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeCollide<Sphere, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeCollide<Sphere, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeCollide<Capsule, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeCollide<Capsule, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeCollide<Capsule, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeCollide<Capsule, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeCollide<Capsule, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeCollide<Capsule, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeCollide<Capsule, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeCollide<Cone, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeCollide<Cone, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeCollide<Cone, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeCollide<Cone, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeCollide<Cone, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeCollide<Cone, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeCollide<Cone, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeCollide<Cylinder, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeCollide<Cylinder, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeCollide<Cylinder, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeCollide<Cylinder, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeCollide<Cylinder, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeCollide<Cylinder, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeCollide<Cylinder, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeCollide<Convex, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeCollide<Convex, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeCollide<Convex, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeCollide<Convex, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeCollide<Convex, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeCollide<Convex, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeCollide<Convex, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeCollide<Plane, Box, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeCollide<Plane, Sphere, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeCollide<Plane, Capsule, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeCollide<Plane, Cone, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeCollide<Plane, Cylinder, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeCollide<Plane, Convex, NarrowPhaseSolver>;
+ collision_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeCollide<Plane, Plane, NarrowPhaseSolver>;
+
+ collision_matrix[BV_AABB][GEOM_BOX] = &BVHShapeCollider<AABB, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeCollider<AABB, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeCollider<AABB, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CONE] = &BVHShapeCollider<AABB, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeCollider<AABB, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeCollider<AABB, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeCollider<AABB, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_OBB][GEOM_BOX] = &BVHShapeCollider<OBB, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeCollider<OBB, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeCollider<OBB, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CONE] = &BVHShapeCollider<OBB, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeCollider<OBB, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeCollider<OBB, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeCollider<OBB, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_RSS][GEOM_BOX] = &BVHShapeCollider<RSS, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeCollider<RSS, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeCollider<RSS, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CONE] = &BVHShapeCollider<RSS, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeCollider<RSS, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeCollider<RSS, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeCollider<RSS, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeCollider<KDOP<16>, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeCollider<KDOP<16>, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<16>, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeCollider<KDOP<16>, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<16>, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeCollider<KDOP<16>, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeCollider<KDOP<16>, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeCollider<KDOP<18>, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeCollider<KDOP<18>, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<18>, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeCollider<KDOP<18>, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<18>, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeCollider<KDOP<18>, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeCollider<KDOP<18>, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeCollider<KDOP<24>, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeCollider<KDOP<24>, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeCollider<KDOP<24>, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeCollider<KDOP<24>, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeCollider<KDOP<24>, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeCollider<KDOP<24>, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeCollider<KDOP<24>, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeCollider<kIOS, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeCollider<kIOS, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeCollider<kIOS, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeCollider<kIOS, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeCollider<kIOS, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeCollider<kIOS, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeCollider<kIOS, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeCollider<OBBRSS, Box, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeCollider<OBBRSS, Sphere, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeCollider<OBBRSS, Capsule, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeCollider<OBBRSS, Cone, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeCollider<OBBRSS, Cylinder, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeCollider<OBBRSS, Convex, NarrowPhaseSolver>::collide;
+ collision_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeCollider<OBBRSS, Plane, NarrowPhaseSolver>::collide;
+
+ collision_matrix[BV_AABB][BV_AABB] = &BVHCollide<AABB, NarrowPhaseSolver>;
+ collision_matrix[BV_OBB][BV_OBB] = &BVHCollide<OBB, NarrowPhaseSolver>;
+ collision_matrix[BV_RSS][BV_RSS] = &BVHCollide<RSS, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP16][BV_KDOP16] = &BVHCollide<KDOP<16>, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP18][BV_KDOP18] = &BVHCollide<KDOP<18>, NarrowPhaseSolver>;
+ collision_matrix[BV_KDOP24][BV_KDOP24] = &BVHCollide<KDOP<24>, NarrowPhaseSolver>;
+ collision_matrix[BV_kIOS][BV_kIOS] = &BVHCollide<kIOS, NarrowPhaseSolver>;
+ collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide<OBBRSS, NarrowPhaseSolver>;
}
+
+template struct CollisionFunctionMatrix<GJKSolver_libccd>;
+template struct CollisionFunctionMatrix<GJKSolver_indep>;
+
+
}
diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp
index 2d85f2856e7b0c8ba5d5d2a20a116361694a4cbd..8d942bcd56216e20acab8db61f3c46af9cfe10ac 100644
--- a/trunk/fcl/src/distance.cpp
+++ b/trunk/fcl/src/distance.cpp
@@ -36,47 +36,103 @@
#include "fcl/distance.h"
#include "fcl/distance_func_matrix.h"
+#include "fcl/narrowphase/narrowphase.h"
#include <iostream>
namespace fcl
{
-static DistanceFunctionMatrix DistanceFunctionLookTable;
+static DistanceFunctionMatrix<GJKSolver_libccd> DistanceFunctionLookTable_libccd;
+static DistanceFunctionMatrix<GJKSolver_indep> DistanceFunctionLookTable_indep;
-BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
+template<typename NarrowPhaseSolver>
+BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver)
+{
+ return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver);
+}
+
+template<typename NarrowPhaseSolver>
+void* getDistanceLookTable() { return NULL; }
+
+template<>
+void* getDistanceLookTable<GJKSolver_libccd>()
+{
+ return &DistanceFunctionLookTable_libccd;
+}
+
+template<>
+void* getDistanceLookTable<GJKSolver_indep>()
{
- return distance(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform());
+ return &DistanceFunctionLookTable_indep;
}
+template<typename NarrowPhaseSolver>
BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
- const CollisionGeometry* o2, const SimpleTransform& tf2)
+ const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver_)
{
+ const NarrowPhaseSolver* nsolver = nsolver_;
+ if(!nsolver_)
+ nsolver = new NarrowPhaseSolver();
+
+ const DistanceFunctionMatrix<NarrowPhaseSolver>* looktable = static_cast<const DistanceFunctionMatrix<NarrowPhaseSolver>*>(getDistanceLookTable<NarrowPhaseSolver>());
+
OBJECT_TYPE object_type1 = o1->getObjectType();
NODE_TYPE node_type1 = o1->getNodeType();
OBJECT_TYPE object_type2 = o2->getObjectType();
NODE_TYPE node_type2 = o2->getNodeType();
+ BVH_REAL res;
+
if(object_type1 == OT_GEOM && object_type2 == OT_BVH)
{
- if(!DistanceFunctionLookTable.distance_matrix[node_type2][node_type1])
+ if(!looktable->distance_matrix[node_type2][node_type1])
{
std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
- return 0;
+ res = 0;
+ }
+ else
+ {
+ res = looktable->distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver);
}
-
- return DistanceFunctionLookTable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1);
}
else
{
- if(!DistanceFunctionLookTable.distance_matrix[node_type1][node_type2])
+ if(!looktable->distance_matrix[node_type1][node_type2])
{
std::cerr << "Warning: distance function between node type " << node_type1 << " and node type " << node_type2 << " is not supported" << std::endl;
- return 0;
+ res = 0;
+ }
+ else
+ {
+ res = looktable->distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver);
}
-
- return DistanceFunctionLookTable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2);
}
+
+ if(!nsolver_)
+ delete nsolver;
+
+ return res;
}
+template BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_libccd* nsolver);
+template BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const GJKSolver_indep* nsolver);
+template BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_libccd* nsolver);
+template BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const GJKSolver_indep* nsolver);
+
+BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2)
+{
+ GJKSolver_libccd solver;
+ return distance<GJKSolver_libccd>(o1, o2, &solver);
+}
+
+BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1,
+ const CollisionGeometry* o2, const SimpleTransform& tf2)
+{
+ GJKSolver_libccd solver;
+ return distance<GJKSolver_libccd>(o1, tf1, o2, tf2, &solver);
+}
+
+
}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index 9264c8fc635e11790de5eb7f7730a79d61f16b65..5bdc6ac494fbbfee4b13d1aeaf6601980ca37d15 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -38,10 +38,92 @@
#include "fcl/collision_node.h"
#include "fcl/simple_setup.h"
+#include "fcl/narrowphase/narrowphase.h"
namespace fcl
{
+template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
+BVH_REAL ShapeShapeDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+{
+ ShapeDistanceTraversalNode<T_SH1, T_SH2, NarrowPhaseSolver> node;
+ const T_SH1* obj1 = static_cast<const T_SH1*>(o1);
+ const T_SH2* obj2 = static_cast<const T_SH2*>(o2);
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver);
+ distance(&node);
+
+ return node.min_distance;
+}
+
+template<typename T_BVH, typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeDistancer
+{
+ static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ {
+ MeshShapeDistanceTraversalNode<T_BVH, T_SH, NarrowPhaseSolver> node;
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
+ BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
+ SimpleTransform tf1_tmp = tf1;
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1_tmp, tf1_tmp, *obj2, tf2, nsolver);
+ fcl::distance(&node);
+
+ delete obj1_tmp;
+ return node.min_distance;
+ }
+};
+
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeDistancer<RSS, T_SH, NarrowPhaseSolver>
+{
+ static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ {
+ MeshShapeDistanceTraversalNodeRSS<T_SH, NarrowPhaseSolver> node;
+ const BVHModel<RSS>* obj1 = static_cast<const BVHModel<RSS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver);
+ fcl::distance(&node);
+
+ return node.min_distance;
+ }
+};
+
+
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeDistancer<kIOS, T_SH, NarrowPhaseSolver>
+{
+ static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ {
+ MeshShapeDistanceTraversalNodekIOS<T_SH, NarrowPhaseSolver> node;
+ const BVHModel<kIOS>* obj1 = static_cast<const BVHModel<kIOS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver);
+ fcl::distance(&node);
+
+ return node.min_distance;
+ }
+};
+
+template<typename T_SH, typename NarrowPhaseSolver>
+struct BVHShapeDistancer<OBBRSS, T_SH, NarrowPhaseSolver>
+{
+ static BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver)
+ {
+ MeshShapeDistanceTraversalNodeOBBRSS<T_SH, NarrowPhaseSolver> node;
+ const BVHModel<OBBRSS>* obj1 = static_cast<const BVHModel<OBBRSS>* >(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+
+ initialize(node, *obj1, tf1, *obj2, tf2, nsolver);
+ fcl::distance(&node);
+
+ return node.min_distance;
+ }
+};
+
+
template<typename T_BVH>
BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2)
{
@@ -99,7 +181,16 @@ BVH_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const SimpleTransform&
return node.min_distance;
}
-DistanceFunctionMatrix::DistanceFunctionMatrix()
+
+template<typename T_BVH, typename NarrowPhaseSolver>
+BVH_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver)
+{
+ return BVHDistance<T_BVH>(o1, tf1, o2, tf2);
+}
+
+template<typename NarrowPhaseSolver>
+DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
{
for(int i = 0; i < 16; ++i)
{
@@ -107,11 +198,138 @@ DistanceFunctionMatrix::DistanceFunctionMatrix()
distance_matrix[i][j] = NULL;
}
- distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB>;
- distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS>;
- distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS>;
- distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS>;
+ distance_matrix[GEOM_BOX][GEOM_BOX] = &ShapeShapeDistance<Box, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_SPHERE] = &ShapeShapeDistance<Box, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CAPSULE] = &ShapeShapeDistance<Box, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CONE] = &ShapeShapeDistance<Box, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CYLINDER] = &ShapeShapeDistance<Box, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_CONVEX] = &ShapeShapeDistance<Box, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_BOX][GEOM_PLANE] = &ShapeShapeDistance<Box, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_SPHERE][GEOM_BOX] = &ShapeShapeDistance<Sphere, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_SPHERE] = &ShapeShapeDistance<Sphere, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CAPSULE] = &ShapeShapeDistance<Sphere, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONE] = &ShapeShapeDistance<Sphere, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CYLINDER] = &ShapeShapeDistance<Sphere, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_CONVEX] = &ShapeShapeDistance<Sphere, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_SPHERE][GEOM_PLANE] = &ShapeShapeDistance<Sphere, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_CAPSULE][GEOM_BOX] = &ShapeShapeDistance<Capsule, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_SPHERE] = &ShapeShapeDistance<Capsule, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CAPSULE] = &ShapeShapeDistance<Capsule, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONE] = &ShapeShapeDistance<Capsule, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CYLINDER] = &ShapeShapeDistance<Capsule, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_CONVEX] = &ShapeShapeDistance<Capsule, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CAPSULE][GEOM_PLANE] = &ShapeShapeDistance<Capsule, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_CONE][GEOM_BOX] = &ShapeShapeDistance<Cone, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_SPHERE] = &ShapeShapeDistance<Cone, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CAPSULE] = &ShapeShapeDistance<Cone, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CONE] = &ShapeShapeDistance<Cone, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CYLINDER] = &ShapeShapeDistance<Cone, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_CONVEX] = &ShapeShapeDistance<Cone, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONE][GEOM_PLANE] = &ShapeShapeDistance<Cone, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_CYLINDER][GEOM_BOX] = &ShapeShapeDistance<Cylinder, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_SPHERE] = &ShapeShapeDistance<Cylinder, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CAPSULE] = &ShapeShapeDistance<Cylinder, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONE] = &ShapeShapeDistance<Cylinder, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CYLINDER] = &ShapeShapeDistance<Cylinder, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_CONVEX] = &ShapeShapeDistance<Cylinder, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CYLINDER][GEOM_PLANE] = &ShapeShapeDistance<Cylinder, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_CONVEX][GEOM_BOX] = &ShapeShapeDistance<Convex, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_SPHERE] = &ShapeShapeDistance<Convex, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CAPSULE] = &ShapeShapeDistance<Convex, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONE] = &ShapeShapeDistance<Convex, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CYLINDER] = &ShapeShapeDistance<Convex, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_CONVEX] = &ShapeShapeDistance<Convex, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_CONVEX][GEOM_PLANE] = &ShapeShapeDistance<Convex, Plane, NarrowPhaseSolver>;
+
+ distance_matrix[GEOM_PLANE][GEOM_BOX] = &ShapeShapeDistance<Plane, Box, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_SPHERE] = &ShapeShapeDistance<Plane, Sphere, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CAPSULE] = &ShapeShapeDistance<Plane, Capsule, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CONE] = &ShapeShapeDistance<Plane, Cone, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CYLINDER] = &ShapeShapeDistance<Plane, Cylinder, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_CONVEX] = &ShapeShapeDistance<Plane, Convex, NarrowPhaseSolver>;
+ distance_matrix[GEOM_PLANE][GEOM_PLANE] = &ShapeShapeDistance<Plane, Plane, NarrowPhaseSolver>;
+
+ /*
+ distance_matrix[BV_AABB][GEOM_BOX] = &BVHShapeDistancer<AABB, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_SPHERE] = &BVHShapeDistancer<AABB, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CAPSULE] = &BVHShapeDistancer<AABB, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CONE] = &BVHShapeDistancer<AABB, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CYLINDER] = &BVHShapeDistancer<AABB, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_CONVEX] = &BVHShapeDistancer<AABB, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_AABB][GEOM_PLANE] = &BVHShapeDistancer<AABB, Plane, NarrowPhaseSolver>::distance;
+
+ distance_matrix[BV_OBB][GEOM_BOX] = &BVHShapeDistancer<OBB, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_SPHERE] = &BVHShapeDistancer<OBB, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CAPSULE] = &BVHShapeDistancer<OBB, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CONE] = &BVHShapeDistancer<OBB, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CYLINDER] = &BVHShapeDistancer<OBB, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_CONVEX] = &BVHShapeDistancer<OBB, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBB][GEOM_PLANE] = &BVHShapeDistancer<OBB, Plane, NarrowPhaseSolver>::distance;
+ */
+
+ distance_matrix[BV_RSS][GEOM_BOX] = &BVHShapeDistancer<RSS, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_SPHERE] = &BVHShapeDistancer<RSS, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CAPSULE] = &BVHShapeDistancer<RSS, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CONE] = &BVHShapeDistancer<RSS, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CYLINDER] = &BVHShapeDistancer<RSS, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_CONVEX] = &BVHShapeDistancer<RSS, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_RSS][GEOM_PLANE] = &BVHShapeDistancer<RSS, Plane, NarrowPhaseSolver>::distance;
+
+ /*
+ distance_matrix[BV_KDOP16][GEOM_BOX] = &BVHShapeDistancer<KDOP<16>, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<16>, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<16>, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONE] = &BVHShapeDistancer<KDOP<16>, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<16>, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<16>, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP16][GEOM_PLANE] = &BVHShapeDistancer<KDOP<16>, Plane, NarrowPhaseSolver>::distance;
+
+ distance_matrix[BV_KDOP18][GEOM_BOX] = &BVHShapeDistancer<KDOP<18>, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<18>, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<18>, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONE] = &BVHShapeDistancer<KDOP<18>, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<18>, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<18>, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP18][GEOM_PLANE] = &BVHShapeDistancer<KDOP<18>, Plane, NarrowPhaseSolver>::distance;
+
+ distance_matrix[BV_KDOP24][GEOM_BOX] = &BVHShapeDistancer<KDOP<24>, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_SPHERE] = &BVHShapeDistancer<KDOP<24>, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CAPSULE] = &BVHShapeDistancer<KDOP<24>, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONE] = &BVHShapeDistancer<KDOP<24>, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CYLINDER] = &BVHShapeDistancer<KDOP<24>, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_CONVEX] = &BVHShapeDistancer<KDOP<24>, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_KDOP24][GEOM_PLANE] = &BVHShapeDistancer<KDOP<24>, Plane, NarrowPhaseSolver>::distance;
+ */
+
+ distance_matrix[BV_kIOS][GEOM_BOX] = &BVHShapeDistancer<kIOS, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_SPHERE] = &BVHShapeDistancer<kIOS, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CAPSULE] = &BVHShapeDistancer<kIOS, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONE] = &BVHShapeDistancer<kIOS, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CYLINDER] = &BVHShapeDistancer<kIOS, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_CONVEX] = &BVHShapeDistancer<kIOS, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_kIOS][GEOM_PLANE] = &BVHShapeDistancer<kIOS, Plane, NarrowPhaseSolver>::distance;
+
+ distance_matrix[BV_OBBRSS][GEOM_BOX] = &BVHShapeDistancer<OBBRSS, Box, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_SPHERE] = &BVHShapeDistancer<OBBRSS, Sphere, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CAPSULE] = &BVHShapeDistancer<OBBRSS, Capsule, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONE] = &BVHShapeDistancer<OBBRSS, Cone, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CYLINDER] = &BVHShapeDistancer<OBBRSS, Cylinder, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_CONVEX] = &BVHShapeDistancer<OBBRSS, Convex, NarrowPhaseSolver>::distance;
+ distance_matrix[BV_OBBRSS][GEOM_PLANE] = &BVHShapeDistancer<OBBRSS, Plane, NarrowPhaseSolver>::distance;
+
+ distance_matrix[BV_AABB][BV_AABB] = &BVHDistance<AABB, NarrowPhaseSolver>;
+ distance_matrix[BV_RSS][BV_RSS] = &BVHDistance<RSS, NarrowPhaseSolver>;
+ distance_matrix[BV_kIOS][BV_kIOS] = &BVHDistance<kIOS, NarrowPhaseSolver>;
+ distance_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHDistance<OBBRSS, NarrowPhaseSolver>;
}
+template struct DistanceFunctionMatrix<GJKSolver_libccd>;
+template struct DistanceFunctionMatrix<GJKSolver_indep>;
+
}
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/geometric_shapes.cpp
index 5309517571555a379173d5021b7f97868e5e54e8..ecef677a45dd47ca13ba3d0074291de3afc271cb 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/geometric_shapes.cpp
@@ -113,4 +113,70 @@ void Plane::unitNormalTest()
}
}
+
+void Box::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Sphere::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = radius;
+}
+
+void Capsule::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Cone::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Cylinder::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Convex::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Plane::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+void Triangle2::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
+
}
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 045641a038c7fdab972ee938c7dce9ca9fc12b70..4e52249c5044e5e8780fc5b7a7ddaefc9e2cbb49 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -168,7 +168,7 @@ std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf)
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransform& tf)
{
- std::vector<Vec3f> result;
+ std::vector<Vec3f> result(12);
BVH_REAL hl = cylinder.lz * 0.5;
BVH_REAL r2 = cylinder.radius * 2 / sqrt(3.0);
@@ -474,69 +474,6 @@ void computeBV<KDOP<24>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<
{
}
-void Box::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Sphere::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = radius;
-}
-
-void Capsule::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Cone::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Cylinder::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Convex::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Plane::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
-
-void Triangle2::computeLocalAABB()
-{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
-}
diff --git a/trunk/fcl/src/gjk.cpp b/trunk/fcl/src/gjk.cpp
index d19f2f49963e28f1bb8b0c41f6de187ed9697354..29f5fcea2de2bbf2c2e66bfd74251aa9aa41ac8f 100644
--- a/trunk/fcl/src/gjk.cpp
+++ b/trunk/fcl/src/gjk.cpp
@@ -317,7 +317,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
Simplex& next_simplex = simplices[next];
BVH_REAL rl = ray.sqrLength();
- if(rl < GJK_EPS) // means origin is near the face of original simplex, return touch
+ if(rl < tolerance) // means origin is near the face of original simplex, return touch
{
status = Inside;
break;
@@ -329,7 +329,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
bool found = false;
for(size_t i = 0; i < 4; ++i)
{
- if((w - lastw[i]).sqrLength() < GJK_EPS)
+ if((w - lastw[i]).sqrLength() < tolerance)
{
found = true; break;
}
@@ -348,7 +348,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
// check for termination, from bullet
BVH_REAL omega = ray.dot(w) / rl;
alpha = std::max(alpha, omega);
- if((rl - alpha) - GJK_EPS * rl <= 0)
+ if((rl - alpha) - tolerance * rl <= 0)
{
removeVertex(simplices[current]);
break;
@@ -391,7 +391,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
break;
}
- status = ((++iterations) < GJK_MAX_ITERATIONS) ? status : Failed;
+ status = ((++iterations) < max_iterations) ? status : Failed;
} while(status == Valid);
@@ -489,12 +489,14 @@ bool GJK::encloseOrigin()
void EPA::initialize()
{
+ sv_store = new SimplexV[max_vertex_num];
+ fc_store = new SimplexF[max_face_num];
status = Failed;
normal = Vec3f(0, 0, 0);
depth = 0;
nextsv = 0;
- for(size_t i = 0; i < EPA_MAX_FACES; ++i)
- stock.append(&fc_store[EPA_MAX_FACES-i-1]);
+ for(size_t i = 0; i < max_face_num; ++i)
+ stock.append(&fc_store[max_face_num-i-1]);
}
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist)
@@ -542,7 +544,7 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
face->n = (b->w - a->w).cross(c->w - a->w);
BVH_REAL l = face->n.length();
- if(l > EPA_EPS)
+ if(l > tolerance)
{
if(!(getEdgeDist(face, a, b, face->d) ||
getEdgeDist(face, b, c, face->d) ||
@@ -552,7 +554,7 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
}
face->n /= l;
- if(forced || face->d >= -EPA_EPS)
+ if(forced || face->d >= -tolerance)
return face;
else
status = NonConvex;
@@ -633,9 +635,9 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
bind(tetrahedron[2], 2, tetrahedron[3], 1);
status = Valid;
- for(; iterations < EPA_MAX_ITERATIONS; ++iterations)
+ for(; iterations < max_iterations; ++iterations)
{
- if(nextsv < EPA_MAX_VERTICES)
+ if(nextsv < max_vertex_num)
{
SimplexHorizon horizon;
SimplexV* w = &sv_store[nextsv++];
@@ -643,7 +645,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
best->pass = ++pass;
gjk.getSupport(best->n, *w);
BVH_REAL wdist = best->n.dot(w->w) - best->d;
- if(wdist > EPA_EPS)
+ if(wdist > tolerance)
{
for(size_t j = 0; (j < 3) && valid; ++j)
{
@@ -719,7 +721,7 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
const size_t e1 = nexti[e];
// case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
- if(f->n.dot(w->w) - f->d < -EPA_EPS)
+ if(f->n.dot(w->w) - f->d < -tolerance)
{
SimplexF* nf = newFace(f->c[e1], f->c[e], w, false);
if(nf)
diff --git a/trunk/fcl/src/gjk_libccd.cpp b/trunk/fcl/src/gjk_libccd.cpp
index fc8e1f24cae3d9e71c596a35da90dd2e0e47511c..c5515e626da467284cae5a3c4efc8111c8eb3d5a 100644
--- a/trunk/fcl/src/gjk_libccd.cpp
+++ b/trunk/fcl/src/gjk_libccd.cpp
@@ -361,7 +361,8 @@ static int doSimplexDist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* di
static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
- const ccd_t *ccd, ccd_simplex_t *simplex)
+ const ccd_t *ccd, ccd_simplex_t *simplex,
+ ccd_real_t tolerance)
{
unsigned long iterations;
ccd_vec3_t dir; // direction vector
@@ -383,8 +384,10 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
ccdVec3Copy(&dir, &last.v);
ccdVec3Scale(&dir, -CCD_ONE);
+ bool collision_free = false;
+
// start iterations
- for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
+ for(iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
{
// obtain support point
__ccdSupport(obj1, obj2, &dir, ccd, &last);
@@ -394,10 +397,10 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
// - because if it is, objects are not intersecting at all.
if(ccdVec3Dot(&last.v, &dir) < CCD_ZERO)
{
+ collision_free = true;
ccd_real_t dist = ccdVec3Len2(&last.v);
if(min_dist < 0) min_dist = dist;
- else
- min_dist = std::min(min_dist, dist);
+ else min_dist = std::min(min_dist, dist);
}
// add last support vector to simplex
@@ -408,26 +411,23 @@ static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
ccd_real_t dist;
do_simplex_res = doSimplexDist(simplex, &dir, &dist);
- if(do_simplex_res == 1)
+ if((do_simplex_res == 1) && (!collision_free))
{
return -1; // intersection found
}
- else if(do_simplex_res == -1)
- {
- return min_dist;
- }
+ else if(do_simplex_res == -1) collision_free = true;
if(ccdIsZero(ccdVec3Len2(&dir)))
- {
- return min_dist; // intersection not found
- }
+ collision_free = true;
+
if(min_dist > 0)
{
- if(fabs(min_dist - dist) < 0.000001 && iterations > 0)
+ ccd_real_t old_min_dist = min_dist;
+ min_dist = std::min(min_dist, dist);
+
+ if((fabs(min_dist - old_min_dist) < tolerance) && (iterations > 0))
break;
- else
- min_dist = std::min(min_dist, dist);
}
else min_dist = dist;
}
@@ -686,6 +686,7 @@ static void centerTriangle(const void* obj, ccd_vec3_t* c)
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)
{
ccd_t ccd;
@@ -699,8 +700,8 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
ccd.support2 = supp2;
ccd.center1 = cen1;
ccd.center2 = cen2;
- ccd.max_iterations = 500;
- ccd.mpr_tolerance = 1e-6;
+ ccd.max_iterations = max_iterations;
+ ccd.mpr_tolerance = tolerance;
if(!contact_points)
{
@@ -723,6 +724,7 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
+ unsigned int max_iterations, BVH_REAL tolerance_,
BVH_REAL* res)
{
ccd_t ccd;
@@ -732,10 +734,12 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
ccd.support1 = supp1;
ccd.support2 = supp2;
- ccd.max_iterations = 500;
+ ccd.max_iterations = max_iterations;
+ ccd_real_t tolerance = tolerance_;
+
ccd_simplex_t simplex;
- dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex);
+ dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex, tolerance);
*res = dist;
if(dist < 0) return false;
else return true;
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 347fd46d084c20e99f01f08cbf5e071559c9cd32..1549e1779ba60e28fbb38f9b4a39717c48e4bc2d 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -39,7 +39,6 @@
namespace fcl
{
-
bool initialize(MeshCollisionTraversalNodeOBB& node,
const BVHModel<OBB>& model1, const SimpleTransform& tf1,
const BVHModel<OBB>& model2, const SimpleTransform& tf2,
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 8e31e4999de87ab49348fd6e7e83243d177b6584..86cec884fd9e4939bf350d72eb72353da701b182 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -41,17 +41,17 @@ namespace fcl
{
template<typename BV>
-static inline void BVHCollisionLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const BVHModel<BV>* model2,
- Vec3f* vertices1, Vec3f* vertices2,
- Triangle* tri_indices1, Triangle* tri_indices2,
- const Matrix3f& R, const Vec3f& T,
- bool enable_statistics,
- bool enable_contact,
- bool exhaustive,
- int num_max_contacts,
- int& num_leaf_tests,
- std::vector<BVHCollisionPair>& pairs)
+static inline void meshCollisionLeafTesting(int b1, int b2,
+ const BVHModel<BV>* model1, const BVHModel<BV>* model2,
+ Vec3f* vertices1, Vec3f* vertices2,
+ Triangle* tri_indices1, Triangle* tri_indices2,
+ const Matrix3f& R, const Vec3f& T,
+ bool enable_statistics,
+ bool enable_contact,
+ bool exhaustive,
+ int num_max_contacts,
+ int& num_leaf_tests,
+ std::vector<BVHCollisionPair>& pairs)
{
if(enable_statistics) num_leaf_tests++;
@@ -102,7 +102,7 @@ static inline void BVHCollisionLeafTesting(int b1, int b2,
template<typename BV>
-static inline void BVHDistanceLeafTesting(int b1, int b2,
+static inline void meshDistanceLeafTesting(int b1, int b2,
const BVHModel<BV>* model1, const BVHModel<BV>* model2,
Vec3f* vertices1, Vec3f* vertices2,
Triangle* tri_indices1, Triangle* tri_indices2,
@@ -169,13 +169,13 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
{
- fcl::BVHCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -187,13 +187,13 @@ bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
{
- fcl::BVHCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -212,13 +212,13 @@ bool MeshCollisionTraversalNodeRSS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
{
- fcl::BVHCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -238,13 +238,13 @@ bool MeshCollisionTraversalNodekIOS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
{
- fcl::BVHCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -263,13 +263,13 @@ bool MeshCollisionTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
void MeshCollisionTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
- fcl::BVHCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
- tri_indices1, tri_indices2,
- R, T,
- enable_statistics, enable_contact, exhaustive,
- num_max_contacts,
- num_leaf_tests,
- pairs);
+ fcl::meshCollisionLeafTesting(b1, b2, model1, model2, vertices1, vertices2,
+ tri_indices1, tri_indices2,
+ R, T,
+ enable_statistics, enable_contact, exhaustive,
+ num_max_contacts,
+ num_leaf_tests,
+ pairs);
}
@@ -487,9 +487,9 @@ BVH_REAL MeshDistanceTraversalNodeRSS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodeRSS::leafTesting(int b1, int b2) const
{
- fcl::BVHDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}
MeshDistanceTraversalNodekIOS::MeshDistanceTraversalNodekIOS() : MeshDistanceTraversalNode<kIOS>()
@@ -516,9 +516,9 @@ BVH_REAL MeshDistanceTraversalNodekIOS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodekIOS::leafTesting(int b1, int b2) const
{
- fcl::BVHDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}
MeshDistanceTraversalNodeOBBRSS::MeshDistanceTraversalNodeOBBRSS() : MeshDistanceTraversalNode<OBBRSS>()
@@ -545,9 +545,9 @@ BVH_REAL MeshDistanceTraversalNodeOBBRSS::BVTesting(int b1, int b2) const
void MeshDistanceTraversalNodeOBBRSS::leafTesting(int b1, int b2) const
{
- fcl::BVHDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
- R, T, enable_statistics, num_leaf_tests,
- p1, p2, last_tri_id1, last_tri_id2, min_distance);
+ fcl::meshDistanceLeafTesting(b1, b2, model1, model2, vertices1, vertices2, tri_indices1, tri_indices2,
+ R, T, enable_statistics, num_leaf_tests,
+ p1, p2, last_tri_id1, last_tri_id2, min_distance);
}