Merge pull request #387 from jmirabel/devel

Update to last changes in hpp-fcl

src/spatial/fcl-pinocchio-conversions.hpp

@@ -24,6 +24,18 @@
 namespace se3
 {
 
+#if FCL_HAVE_EIGEN
+  // TODO When not supporting the version of hpp-fcl (v0.5) not using plain eigen types
+  // the following functions can be removed because
+  //  - fcl::Matrix3f = Eigen::Matrix3d
+  //  - fcl::Vec3f    = Eigen::Vector3d
+  // Currently, they are mandatory to support both v0.5 with FCL_HAVE_EIGEN and
+  // future version
+  inline fcl::Matrix3f   toFclMatrix3f (Eigen::Matrix3d const & mat) { return mat; }
+  inline Eigen::Matrix3d toMatrix3d    (fcl::Matrix3f   const & mat) { return mat; }
+  inline fcl::Vec3f      toFclVec3f    (Eigen::Vector3d const & vec) { return vec; }
+  inline Eigen::Vector3d toVector3d    (fcl::Vec3f      const & vec) { return vec; }
+#else  // FCL_HAVE_EIGEN
   inline fcl::Matrix3f toFclMatrix3f(const Eigen::Matrix3d & mat)
   {
     return fcl::Matrix3f( mat(0,0),mat(0,1), mat(0,2),
@@ -50,6 +62,7 @@ namespace se3
   {
     return Eigen::Vector3d(vec[0],vec[1],vec[2]);
   }
+#endif // FCL_HAVE_EIGEN
 
   inline fcl::Transform3f toFclTransform3f(const SE3 & m)
   {

unittest/geom.cpp

@@ -156,13 +156,13 @@ BOOST_AUTO_TEST_CASE ( simple_boxes )
   model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
   model.addBodyFrame("planar2_body", idx, SE3::Identity());
   
-  boost::shared_ptr<fcl::Box> sample(new fcl::Box(1));
+  boost::shared_ptr<fcl::Box> sample(new fcl::Box(1, 1, 1));
   geomModel.addGeometryObject(GeometryObject("ff1_collision_object",
                                              model.getBodyId("planar1_body"),0,
                                              sample,SE3::Identity(), "", Eigen::Vector3d::Ones()),
                               model,true);
   
-  boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1));
+  boost::shared_ptr<fcl::Box> sample2(new fcl::Box(1, 1, 1));
   geomModel.addGeometryObject(GeometryObject("ff2_collision_object",
                                              model.getBodyId("planar2_body"),0,
                                              sample2,SE3::Identity(), "", Eigen::Vector3d::Ones()),