geometry: introduce Compute{Collision,Distance}

src/multibody/fcl.hpp

@@ -271,7 +271,64 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP
 
   friend std::ostream & operator<< (std::ostream & os, const GeometryObject & geomObject);
 };
+
+#ifdef PINOCCHIO_WITH_HPP_FCL
+
+  struct ComputeCollision
+  : ::hpp::fcl::ComputeCollision
+  {
+    typedef ::hpp::fcl::ComputeCollision Base;
+    
+    ComputeCollision(const GeometryObject & o1, const GeometryObject & o2)
+    : Base(o1.geometry.get(),o2.geometry.get())
+    , o1(o1)
+    , o2(o2)
+    {}
+    
+    virtual ~ComputeCollision() {};
+    
+  protected:
+    const GeometryObject & o1;
+    const GeometryObject & o2;
+    
+    virtual std::size_t run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
+                            const fcl::CollisionRequest& request, fcl::CollisionResult& result) const
+    {
+      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
+      const_cast<Pointer&>(Base::o1) = o1.geometry.get();
+      const_cast<Pointer&>(Base::o2) = o2.geometry.get();
+      return Base::run(tf1, tf2, request, result);
+    }
+  };
+
+  struct ComputeDistance
+  : ::hpp::fcl::ComputeDistance
+  {
+    typedef ::hpp::fcl::ComputeDistance Base;
+    
+    ComputeDistance(const GeometryObject & o1, const GeometryObject & o2)
+    : Base(o1.geometry.get(),o2.geometry.get())
+    , o1(o1)
+    , o2(o2)
+    {}
+    
+    virtual ~ComputeDistance() {};
+    
+  protected:
+    const GeometryObject & o1;
+    const GeometryObject & o2;
+    
+    virtual hpp::fcl::FCL_REAL run(const fcl::Transform3f& tf1, const fcl::Transform3f& tf2,
+                                   const fcl::DistanceRequest& request, fcl::DistanceResult& result) const
+    {
+      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
+      const_cast<Pointer&>(Base::o1) = o1.geometry.get();
+      const_cast<Pointer&>(Base::o2) = o2.geometry.get();
+      return Base::run(tf1, tf2, request, result);
+    }
+  };
   
+#endif
 
 } // namespace pinocchio
 

src/multibody/geometry.hpp

@@ -189,8 +189,8 @@ namespace pinocchio
     typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> MatrixXs;
     
 #ifdef PINOCCHIO_WITH_HPP_FCL
-    typedef ::hpp::fcl::ComputeCollision ComputeCollision;
-    typedef ::hpp::fcl::ComputeDistance ComputeDistance;
+    typedef ComputeCollision ComputeCollision;
+    typedef ComputeDistance ComputeDistance;
 #endif
     
     ///

src/multibody/geometry.hxx

@@ -57,8 +57,8 @@ namespace pinocchio
       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()));
+      collision_functors.push_back(ComputeCollision(obj_1,obj_2));
+      distance_functors.push_back(ComputeDistance(obj_1,obj_2));
     }
 #endif
     fillInnerOuterObjectMaps(geom_model);