diff --git a/trunk/fcl/src/collision.cpp b/trunk/fcl/src/collision.cpp index c741d8c850cc22d8c3180954628efbcdcbeec5c8..46be2c231e4861e4df05fbd65270b483d207da70 100644 --- a/trunk/fcl/src/collision.cpp +++ b/trunk/fcl/src/collision.cpp @@ -44,9 +44,12 @@ namespace fcl { -static CollisionFunctionMatrix<GJKSolver_libccd> CollisionFunctionLookTable_libccd; -static CollisionFunctionMatrix<GJKSolver_indep> CollisionFunctionLookTable_indep; - +template<typename GJKSolver> +CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable() +{ + static CollisionFunctionMatrix<GJKSolver> table; + return table; +}; template<typename NarrowPhaseSolver> int collide(const CollisionObject* o1, const CollisionObject* o2, @@ -59,21 +62,6 @@ int collide(const CollisionObject* o1, const CollisionObject* o2, 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, @@ -85,7 +73,7 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, if(!nsolver_) nsolver = new NarrowPhaseSolver(); - const CollisionFunctionMatrix<NarrowPhaseSolver>* looktable = static_cast<const CollisionFunctionMatrix<NarrowPhaseSolver>*>(getCollisionLookTable<NarrowPhaseSolver>()); + const CollisionFunctionMatrix<NarrowPhaseSolver>& looktable = getCollisionFunctionLookTable<NarrowPhaseSolver>(); int res; if(num_max_contacts <= 0 && !exhaustive) @@ -102,23 +90,23 @@ int collide(const CollisionGeometry* o1, const SimpleTransform& tf1, if(object_type1 == OT_GEOM & object_type2 == OT_BVH) { - if(!looktable->collision_matrix[node_type2][node_type1]) + 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); + res = looktable.collision_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver, num_max_contacts, exhaustive, enable_contact, contacts); } else { - if(!looktable->collision_matrix[node_type1][node_type2]) + 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); + res = looktable.collision_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver, num_max_contacts, exhaustive, enable_contact, contacts); } } diff --git a/trunk/fcl/src/distance.cpp b/trunk/fcl/src/distance.cpp index 8d942bcd56216e20acab8db61f3c46af9cfe10ac..b3883a89faa9f86ebadf5de76ac280b38c9b08fb 100644 --- a/trunk/fcl/src/distance.cpp +++ b/trunk/fcl/src/distance.cpp @@ -43,28 +43,17 @@ namespace fcl { -static DistanceFunctionMatrix<GJKSolver_libccd> DistanceFunctionLookTable_libccd; -static DistanceFunctionMatrix<GJKSolver_indep> DistanceFunctionLookTable_indep; - -template<typename NarrowPhaseSolver> -BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver) +template<typename GJKSolver> +DistanceFunctionMatrix<GJKSolver>& getDistanceFunctionLookTable() { - return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver); + static DistanceFunctionMatrix<GJKSolver> table; + return table; } template<typename NarrowPhaseSolver> -void* getDistanceLookTable() { return NULL; } - -template<> -void* getDistanceLookTable<GJKSolver_libccd>() -{ - return &DistanceFunctionLookTable_libccd; -} - -template<> -void* getDistanceLookTable<GJKSolver_indep>() +BVH_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver) { - return &DistanceFunctionLookTable_indep; + return distance<NarrowPhaseSolver>(o1->getCollisionGeometry(), o1->getTransform(), o2->getCollisionGeometry(), o2->getTransform(), nsolver); } template<typename NarrowPhaseSolver> @@ -76,7 +65,7 @@ BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, if(!nsolver_) nsolver = new NarrowPhaseSolver(); - const DistanceFunctionMatrix<NarrowPhaseSolver>* looktable = static_cast<const DistanceFunctionMatrix<NarrowPhaseSolver>*>(getDistanceLookTable<NarrowPhaseSolver>()); + const DistanceFunctionMatrix<NarrowPhaseSolver>& looktable = getDistanceFunctionLookTable<NarrowPhaseSolver>(); OBJECT_TYPE object_type1 = o1->getObjectType(); NODE_TYPE node_type1 = o1->getNodeType(); @@ -87,26 +76,26 @@ BVH_REAL distance(const CollisionGeometry* o1, const SimpleTransform& tf1, if(object_type1 == OT_GEOM && object_type2 == OT_BVH) { - if(!looktable->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; res = 0; } else { - res = looktable->distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver); + res = looktable.distance_matrix[node_type2][node_type1](o2, tf2, o1, tf1, nsolver); } } else { - if(!looktable->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; res = 0; } else { - res = looktable->distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver); + res = looktable.distance_matrix[node_type1][node_type2](o1, tf1, o2, tf2, nsolver); } }