core: use functor for distance and collision request

src/algorithm/geometry.hxx

@@ -59,7 +59,7 @@ namespace pinocchio
     PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.first  < geom_model.ngeoms );
     PINOCCHIO_CHECK_INPUT_ARGUMENT( pair.second < geom_model.ngeoms );
 
-    fcl::CollisionResult& collisionResult = geom_data.collisionResults[pair_id];
+    fcl::CollisionResult & collisionResult = geom_data.collisionResults[pair_id];
     collisionResult.clear();
 
     fcl::Transform3f oM1 (toFclTransform3f(geom_data.oMg[pair.first ])),
@@ -67,10 +67,10 @@ namespace pinocchio
 
     try
     {
-      fcl::collide(geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
-                   geom_model.geometryObjects[pair.second].geometry.get(), oM2,
-                   geom_data.collisionRequests[pair_id],
-                   collisionResult);
+      const GeometryData::ComputeCollision & do_computations = geom_data.collision_functors[pair_id];
+      do_computations(oM1, oM2,
+                      geom_data.collisionRequests[pair_id],
+                      collisionResult);
     }
     catch(std::invalid_argument & e)
     {
@@ -149,10 +149,10 @@ namespace pinocchio
     
     try
     {
-      fcl::distance( geom_model.geometryObjects[pair.first ].geometry.get(), oM1,
-                     geom_model.geometryObjects[pair.second].geometry.get(), oM2,
-                     geom_data.distanceRequests[pair_id],
-                     geom_data.distanceResults[pair_id]);
+      const GeometryData::ComputeDistance & do_computations = geom_data.distance_functors[pair_id];
+      do_computations(oM1, oM2,
+                      geom_data.distanceRequests[pair_id],
+                      geom_data.distanceResults[pair_id]);
     }
     catch(std::invalid_argument & e)
     {

src/multibody/geometry.hpp

@@ -188,6 +188,9 @@ namespace pinocchio
     typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXb;
     typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
     
+    typedef ::hpp::fcl::ComputeCollision ComputeCollision;
+    typedef ::hpp::fcl::ComputeDistance ComputeDistance;
+    
     ///
     /// \brief Vector gathering the SE3 placements of the geometry objects relative to the world.
     ///        See updateGeometryPlacements to update the placements.
@@ -237,7 +240,14 @@ namespace pinocchio
     /// the algo computeCollisions() sets it to the first colliding pair.
     ///
     PairIndex collisionPairIndex;
-#endif // PINOCCHIO_WITH_HPP_FCL   
+    
+    /// \brief Functoqr associated to the computation of collisions.
+    PINOCCHIO_ALIGNED_STD_VECTOR(ComputeCollision) collision_functors;
+    
+    /// \brief Functoqr associated to the computation of distances.
+    PINOCCHIO_ALIGNED_STD_VECTOR(ComputeDistance) distance_functors;
+    
+#endif // PINOCCHIO_WITH_HPP_FCL
 
     /// \brief Map over vector GeomModel::geometryObjects, indexed by joints.
     ///

src/multibody/geometry.hxx

@@ -47,6 +47,19 @@ namespace pinocchio
       dreq.enable_cached_gjk_guess = true;
     }
 #endif
+    collision_functors.reserve(geom_model.collisionPairs.size());
+    distance_functors.reserve(geom_model.collisionPairs.size());
+    
+    for(size_t cp_index = 0;
+        cp_index < geom_model.collisionPairs.size(); ++cp_index)
+    {
+      const CollisionPair & cp = geom_model.collisionPairs[cp_index];
+      const GeometryObject & obj_1 = geom_model.geometryObjects[cp.first];
+      const GeometryObject & obj_2 = geom_model.geometryObjects[cp.second];
+      
+      collision_functors.push_back(ComputeCollision(obj_1.geometry.get(),obj_2.geometry.get()));
+      distance_functors.push_back(ComputeDistance(obj_1.geometry.get(),obj_2.geometry.get()));
+    }
 #endif
     fillInnerOuterObjectMaps(geom_model);
   }
@@ -61,6 +74,8 @@ namespace pinocchio
   , collisionResults (other.collisionResults)
   , radius (other.radius)
   , collisionPairIndex (other.collisionPairIndex)
+  , collision_functors (other.collision_functors)
+  , distance_functors (other.distance_functors)
 #endif // PINOCCHIO_WITH_HPP_FCL
   , innerObjects (other.innerObjects)
   , outerObjects (other.outerObjects)