change looktable using trick learned from Ioan's template cache.

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@110 253336fb-580f-4252-a368-f3cef5a2a82b

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);
     }
   }
 

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);    
     }
   }