Merge pull request #10 from jmirabel/devel

Add Eigen wrapping

CMakeLists.txt

@@ -2,7 +2,7 @@
 # Software License Agreement (BSD License)
 #
 #  Copyright (c) 2014 CNRS-LAAS
-#  Author: Florent Lamiraux
+#  Author: Florent Lamiraux, Joseph Mirabel
 #  All rights reserved.
 #
 #  Redistribution and use in source and binary forms, with or without
@@ -35,6 +35,7 @@
 cmake_minimum_required(VERSION 2.8)
 set(CXX_DISABLE_WERROR TRUE)
 include(cmake/base.cmake)
+include(cmake/eigen.cmake)
 include(cmake/boost.cmake)
 
 set(PROJECT_NAME hpp-fcl)
@@ -45,6 +46,16 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
 
 setup_project()
 
+set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
+
+add_optional_dependency("eigen3 >= 3.0.0")
+set(FCL_HAVE_EIGEN  ${EIGEN3_FOUND} CACHE BOOL "Use eigen matrix type when possible")
+if (EIGEN3_FOUND)
+  if (FCL_HAVE_EIGEN)
+    include_directories(${EIGEN3_INCLUDE_DIRS})
+  endif (FCL_HAVE_EIGEN)
+endif (EIGEN3_FOUND)
+
 # Required dependencies
 add_required_dependency("ccd >= 1.4")
 set(BOOST_COMPONENTS
@@ -129,7 +140,9 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/continuous_collision.h
   include/hpp/fcl/math/vec_nf.h
   include/hpp/fcl/math/matrix_3f.h
+  include/hpp/fcl/math/matrix_3fx.h
   include/hpp/fcl/math/vec_3f.h
+  include/hpp/fcl/math/vec_3fx.h
   include/hpp/fcl/math/sampling.h
   include/hpp/fcl/math/math_details.h
   include/hpp/fcl/math/transform.h
@@ -156,6 +169,12 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/fcl/collision_object.h
   include/hpp/fcl/octree.h
   include/hpp/fcl/fwd.hh
+  include/hpp/fcl/eigen/math_details.h
+  include/hpp/fcl/eigen/vec_3fx.h
+  include/hpp/fcl/eigen/product.h
+  include/hpp/fcl/eigen/taylor_operator.h
+  include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
+  include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
   )
 
 add_subdirectory(src)

cmake @ 5663a002

-Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd
+Subproject commit 5663a002a905b5c8d84a69d8e74044a8e34a48ed

include/hpp/fcl/BV/BV.h

@@ -283,7 +283,7 @@ public:
 
     const Matrix3f& R = tf1.getRotation();
     bool left_hand = (id[0] == (id[1] + 1) % 3);
-    bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
+    if (left_hand) bv2.axis[0] = -R.getColumn(id[0]); else bv2.axis[0] = R.getColumn(id[0]);
     bv2.axis[1] = R.getColumn(id[1]);
     bv2.axis[2] = R.getColumn(id[2]);    
   }

include/hpp/fcl/BV/OBB.h

@@ -71,7 +71,7 @@ public:
 
   
   /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. 
-  bool overlap(const OBB& other, OBB& overlap_part) const
+  bool overlap(const OBB& other, OBB& /*overlap_part*/) const
   {
     return overlap(other);
   }

include/hpp/fcl/BV/OBBRSS.h

@@ -72,7 +72,7 @@ public:
   }
 
   /// @brief Check collision between two OBBRSS and return the overlap part.
-  bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
+  bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
   {
     return overlap(other);
   }

include/hpp/fcl/BV/RSS.h

@@ -67,7 +67,7 @@ public:
   bool overlap(const RSS& other) const;
 
   /// Not implemented
-  bool overlap(const RSS& other, FCL_REAL& sqrDistLowerBound) const
+  bool overlap(const RSS& /*other*/, FCL_REAL& /*sqrDistLowerBound*/) const
   {
     throw std::runtime_error ("Not implemented.");
     return false;
@@ -75,7 +75,7 @@ public:
 
   /// @brief Check collision between two RSS and return the overlap part.
   /// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
-  bool overlap(const RSS& other, RSS& overlap_part) const
+  bool overlap(const RSS& other, RSS& /*overlap_part*/) const
   {
     return overlap(other);
   }

include/hpp/fcl/BV/kDOP.h

@@ -97,7 +97,7 @@ public:
   bool overlap(const KDOP<N>& other) const;
 
   /// Not implemented
-  bool overlap(const KDOP<N>& other, FCL_REAL&) const
+  bool overlap(const KDOP<N>& /*other*/, FCL_REAL&) const
   {
     throw std::runtime_error ("Not implemented");
   }

include/hpp/fcl/BV/kIOS.h

@@ -69,7 +69,7 @@ class kIOS
     }
     else /** spheres partially overlapping or disjoint */
     {
-      float dist = std::sqrt(dist2);
+      float dist = (float)std::sqrt(dist2);
       kIOS_Sphere s;
       s.r = dist + s0.r + s1.r;
       if(dist > 0)
@@ -99,7 +99,7 @@ public:
   /// @brief Check collision between two kIOS and return the overlap part.
   /// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
   /// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
-  bool overlap(const kIOS& other, kIOS& overlap_part) const
+  bool overlap(const kIOS& other, kIOS& /*overlap_part*/) const
   {
     return overlap(other);
   }

include/hpp/fcl/BVH/BVH_internal.h

@@ -54,7 +54,7 @@ enum BVHBuildState
     BVH_BUILD_STATE_PROCESSED,        /// after tree has been build, ready for cd use
     BVH_BUILD_STATE_UPDATE_BEGUN,     /// after beginUpdateModel(), state for updating geometry primitives
     BVH_BUILD_STATE_UPDATED,          /// after tree has been build for updated geometry, ready for ccd use
-    BVH_BUILD_STATE_REPLACE_BEGUN,    /// after beginReplaceModel(), state for replacing geometry primitives
+    BVH_BUILD_STATE_REPLACE_BEGUN     /// after beginReplaceModel(), state for replacing geometry primitives
   };
 
 /// @brief Error code for BVH 

include/hpp/fcl/broadphase/broadphase.h

@@ -85,13 +85,13 @@ public:
   virtual void update() = 0;
 
   /// @brief update the manager by explicitly given the object updated
-  virtual void update(CollisionObject* updated_obj)
+  virtual void update(CollisionObject* /*updated_obj*/)
   {
     update();
   }
 
   /// @brief update the manager by explicitly given the set of objects update
-  virtual void update(const std::vector<CollisionObject*>& updated_objs)
+  virtual void update(const std::vector<CollisionObject*>& /*updated_objs*/)
   {
     update();
   }
@@ -184,13 +184,13 @@ public:
   virtual void update() = 0;
 
   /// @brief update the manager by explicitly given the object updated
-  virtual void update(ContinuousCollisionObject* updated_obj)
+  virtual void update(ContinuousCollisionObject* /*updated_obj*/)
   {
     update();
   }
 
   /// @brief update the manager by explicitly given the set of objects update
-  virtual void update(const std::vector<ContinuousCollisionObject*>& updated_objs)
+  virtual void update(const std::vector<ContinuousCollisionObject*>& /*updated_objs*/)
   {
     update();
   }

include/hpp/fcl/ccd/interval_matrix.h

@@ -99,6 +99,10 @@ struct IMatrix3
   IMatrix3& rotationConstrain();
 
   void print() const;
+
+#ifdef FCL_CCD_INTERVALMATRIX_PLUGIN
+# include FCL_CCD_INTERVALMATRIX_PLUGIN
+#endif
 };
 
 IMatrix3 rotationConstrain(const IMatrix3& m);

include/hpp/fcl/ccd/interval_vector.h

@@ -156,6 +156,10 @@ struct IVector3
 
   bool overlap(const IVector3& v) const;
   bool contain(const IVector3& v) const;
+
+#ifdef FCL_CCD_INTERVALVECTOR_PLUGIN
+# include FCL_CCD_INTERVALVECTOR_PLUGIN
+#endif
 };
 
 IVector3 bound(const IVector3& i, const Vec3f& v);

include/hpp/fcl/ccd/motion.h

@@ -88,7 +88,7 @@ public:
     tf_ = tf;
   }
 
-  void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+  void getTaylorModel(TMatrix3& /*tm*/, TVector3& /*tv*/) const
   {
   }
 
@@ -113,14 +113,14 @@ public:
                const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
 
   // @brief Construct motion from initial and goal transform
-  SplineMotion(const Matrix3f& R1, const Vec3f& T1,
-               const Matrix3f& R2, const Vec3f& T2) : MotionBase()
+  SplineMotion(const Matrix3f& /*R1*/, const Vec3f& /*T1*/,
+               const Matrix3f& /*R2*/, const Vec3f& /*T2*/) : MotionBase()
   {
     // TODO
   }
 
-  SplineMotion(const Transform3f& tf1,
-               const Transform3f& tf2) : MotionBase()
+  SplineMotion(const Transform3f& /*tf1*/,
+               const Transform3f& /*tf2*/) : MotionBase()
   {
     // TODO
   }

include/hpp/fcl/ccd/motion_base.h

@@ -72,11 +72,11 @@ class TBVMotionBoundVisitor : public BVMotionBoundVisitor
 public:
   TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
 
-  virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
-  virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
-  virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
-  virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
-  virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
+  virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
+  virtual FCL_REAL visit(const SplineMotion& /*motion*/) const { return 0; }
+  virtual FCL_REAL visit(const ScrewMotion& /*motion*/) const { return 0; }
+  virtual FCL_REAL visit(const InterpMotion& /*motion*/) const { return 0; }
+  virtual FCL_REAL visit(const TranslationMotion& /*motion*/) const { return 0; }
 
 protected:
   BV bv;
@@ -102,7 +102,7 @@ public:
   TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
     a(a_), b(b_), c(c_), n(n_) {}
 
-  virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
+  virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
   virtual FCL_REAL visit(const SplineMotion& motion) const;
   virtual FCL_REAL visit(const ScrewMotion& motion) const;
   virtual FCL_REAL visit(const InterpMotion& motion) const;

include/hpp/fcl/ccd/taylor_vector.h

@@ -41,6 +41,10 @@
 #include <hpp/fcl/ccd/interval_vector.h>
 #include <hpp/fcl/ccd/taylor_model.h>
 
+#if FCL_HAVE_EIGEN
+#include <hpp/fcl/eigen/taylor_operator.h>
+#endif
+
 namespace fcl
 {
 

include/hpp/fcl/config-fcl.hh.in

@@ -37,6 +37,8 @@
 
 # include "config.h"
 
+#cmakedefine01 FCL_HAVE_EIGEN
+
 #cmakedefine01 FCL_HAVE_SSE
 #cmakedefine01 FCL_HAVE_OCTOMAP
 #cmakedefine01 FCL_HAVE_FLANN

include/hpp/fcl/eigen/plugins/ccd/interval-vector.h

+template <typename Derived>
+IVector3& operator=(const FclType<Derived>& other)
+{
+  const Vec3f& tmp = other.fcl();
+  setValue (tmp);
+  return *this;
+}

include/hpp/fcl/eigen/product.h

+namespace internal {
+
+template<typename Derived, typename OtherDerived, bool coefwise> struct deduce_fcl_type {};
+
+template<typename Derived, typename OtherDerived>
+struct deduce_fcl_type<Derived, OtherDerived, false> {
+  typedef typename ProductReturnType<Derived, OtherDerived>::Type Type;
+};
+template<typename Derived, typename OtherDerived>
+struct deduce_fcl_type<Derived, OtherDerived, true> {
+  typedef CwiseBinaryOp<scalar_multiple_op<typename Derived::Scalar>, const Derived, const OtherDerived> Type;
+};
+
+}
+
+template<typename Derived, typename OtherDerived>
+struct FclProduct
+{
+  enum {
+    COEFWISE = Derived::ColsAtCompileTime == 1 && OtherDerived::ColsAtCompileTime == 1
+  };
+
+  typedef typename internal::remove_fcl<Derived>::type EDerived;
+  typedef typename internal::remove_fcl<OtherDerived>::type EOtherDerived;
+
+  typedef FclOp<typename internal::deduce_fcl_type<EDerived, EOtherDerived, COEFWISE>::Type> ProductType;
+  typedef FclOp<typename ProductReturnType<Transpose<Derived>, EOtherDerived >::Type> TransposeTimesType;
+  typedef FclOp<typename ProductReturnType<EDerived, Transpose<EOtherDerived> >::Type> TimesTransposeType;
+  typedef FclOp<typename ProductReturnType<ProductType, Transpose<EDerived>  >::Type> TensorTransformType;
+  static EIGEN_STRONG_INLINE ProductType run (const Derived& l, const OtherDerived& r) { return ProductType (l, r); }
+};
+
+#define FCL_EIGEN_MAKE_PRODUCT_OPERATOR() \
+      template <typename OtherDerived> \
+      EIGEN_STRONG_INLINE const typename FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::ProductType \
+      operator*(const MatrixBase<OtherDerived>& other) const \
+      { \
+        return FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::run (*this, other.derived()); \
+      }