Make Eigen a required dependency and add fcl::Vec3fX::derived() (same

for Matrix3fX

CMakeLists.txt

@@ -48,13 +48,10 @@ 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)
+# add_optional_dependency("eigen3 >= 3.0.0")
+add_required_dependency("eigen3 >= 3.0.0")
+set(FCL_HAVE_EIGEN TRUE CACHE BOOL "Use eigen matrix type when possible")
+include_directories(${EIGEN3_INCLUDE_DIRS})
 
 # Required dependencies
 add_required_dependency("ccd >= 1.4")

include/hpp/fcl/math/matrix_3fx.h

@@ -39,6 +39,7 @@
 #define FCL_MATRIX_3FX_H
 
 #include <hpp/fcl/math/vec_3f.h>
+#include <Eigen/Core>
 
 namespace fcl
 {
@@ -50,6 +51,7 @@ class Matrix3fX
 public:
   typedef typename T::meta_type U;
   typedef typename T::vector_type S;
+  typedef Eigen::Matrix<U, 3, 3> EigenType;
   T data;
   
   Matrix3fX() {}
@@ -75,6 +77,15 @@ public:
     return Vec3fX<S>(data.getRow(i));
   }
 
+  inline EigenType derived() const
+  {
+    EigenType ret;
+    for(int i = 0; i < 3; ++i)
+      for(int j = 0; j < 3; ++j)
+        ret(i,j) = data(i,j);
+    return ret;
+  }
+
   inline U operator () (size_t i, size_t j) const
   {
     return data(i, j);

include/hpp/fcl/math/vec_3fx.h

@@ -45,6 +45,7 @@
 #include <iostream>
 #include <limits>
 
+#include <Eigen/Core>
 
 namespace fcl
 {
@@ -55,6 +56,7 @@ class Vec3fX
 {
 public:
   typedef typename T::meta_type U;
+  typedef Eigen::Matrix<U, 3, 1> EigenType;
 
   /// @brief interval vector3 data
   T data;
@@ -71,6 +73,11 @@ public:
   /// @brief create vector using the internal data type
   Vec3fX(const T& data_) : data(data_) {}
 
+  inline EigenType derived() const
+  {
+    return EigenType (data[0],data[1],data[2]);
+  }
+
   inline U operator [] (size_t i) const { return data[i]; }
   inline U& operator [] (size_t i) { return data[i]; }