Work in progress

include/hpp/fcl/eigen/math_details.h

@@ -436,6 +436,7 @@ static inline bool equal(const eigen_v3<T>& x, const eigen_v3<T>& y, T epsilon)
 template<typename T>
 struct eigen_wrapper_m3
 {
+  typedef T meta_type;
   typedef eigen_wrapper_v3<T> vector_type;
   typedef Eigen::Matrix <T, 3, 3, Eigen::RowMajor> matrix_type;
   typedef Eigen::Matrix <T, 3, 1> inner_col_type;
@@ -477,19 +478,19 @@ struct eigen_wrapper_m3
   inline eigen_wrapper_m3<T> operator + (const eigen_wrapper_m3<T>& other) const { return eigen_wrapper_m3<T>(m + other.m); }
   inline eigen_wrapper_m3<T> operator - (const eigen_wrapper_m3<T>& other) const { return eigen_wrapper_m3<T>(m - other.m); }
 
-  inline eigen_wrapper_m3<T> operator + (meta_type c) const { return eigen_wrapper_m3<T>(m + c); }
-  inline eigen_wrapper_m3<T> operator - (meta_type c) const { return eigen_wrapper_m3<T>(m - c); }
-  inline eigen_wrapper_m3<T> operator * (meta_type c) const { return eigen_wrapper_m3<T>(m * c); }
-  inline eigen_wrapper_m3<T> operator / (meta_type c) const { return eigen_wrapper_m3<T>(m / c); }
+  inline eigen_wrapper_m3<T> operator + (T c) const { return eigen_wrapper_m3<T>(m + c); }
+  inline eigen_wrapper_m3<T> operator - (T c) const { return eigen_wrapper_m3<T>(m - c); }
+  inline eigen_wrapper_m3<T> operator * (T c) const { return eigen_wrapper_m3<T>(m * c); }
+  inline eigen_wrapper_m3<T> operator / (T c) const { return eigen_wrapper_m3<T>(m / c); }
 
   inline eigen_wrapper_m3<T>& operator *= (const eigen_wrapper_m3<T>& other) { m *= other.m; return *this; }
   inline eigen_wrapper_m3<T>& operator += (const eigen_wrapper_m3<T>& other) { m += other.m; return *this; }
   inline eigen_wrapper_m3<T>& operator -= (const eigen_wrapper_m3<T>& other) { m -= other.m; return *this; }
 
-  inline eigen_wrapper_m3<T>& operator += (meta_type c) { m.array() += c; return *this; }
-  inline eigen_wrapper_m3<T>& operator -= (meta_type c) { m.array() -= c; return *this; }
-  inline eigen_wrapper_m3<T>& operator *= (meta_type c) { m.array() *= c; return *this; }
-  inline eigen_wrapper_m3<T>& operator /= (meta_type c) { m.array() /= c; return *this; }
+  inline eigen_wrapper_m3<T>& operator += (T c) { m.array() += c; return *this; }
+  inline eigen_wrapper_m3<T>& operator -= (T c) { m.array() -= c; return *this; }
+  inline eigen_wrapper_m3<T>& operator *= (T c) { m.array() *= c; return *this; }
+  inline eigen_wrapper_m3<T>& operator /= (T c) { m.array() /= c; return *this; }
 
 
   void setIdentity() { m.setIdentity (); }
@@ -678,14 +679,14 @@ struct eigen_m3 :
   }
 
   template <typename OtherDerived>
-    const Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+    const typename Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
     transposeTimes(const Eigen::MatrixBase<OtherDerived>& other) const
   {
     return this->Base::transpose() * other;
   }
 
   template <typename OtherDerived>
-    const Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+    const typename Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
     timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
   {
     return this->operator* (other.Base::transpose());

include/hpp/fcl/eigen/matrix_3fx.h

@@ -43,35 +43,52 @@ namespace fcl
 
 /// @brief Matrix2 class wrapper. the core data is in the template parameter class
 template<typename T>
-class Matrix3fX
-  : Eigen::Matrix <T, 3, 1>
+class Matrix3fX :
+  public Eigen::Matrix <T, 3, 3, Eigen::RowMajor>
 {
 public:
-  // typedef typename T::vector_type S;
-  
-  Matrix3fX() {}
-  Matrix3fX(U xx, U xy, U xz,
-            U yx, U yy, U yz,
-            U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
-  {}
+  typedef Eigen::Matrix <T, 3, 3, Eigen::RowMajor> Base;
+  typedef typename Base::ColXpr ColXpr;
+  typedef typename Base::ConstColXpr ConstColXpr;
+  typedef typename Base::RowXpr RowXpr;
+  typedef typename Base::ConstRowXpr ConstRowXpr;
 
-  Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
-    : data(v1.data, v2.data, v3.data) {}
   
-  Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
+  Matrix3fX(void): Base() {}
 
-  Matrix3fX(const T& data_) : data(data_) {}
-  
-  inline Vec3fX<S> getColumn(size_t i) const
-  {
-    return Vec3fX<S>(data.getColumn(i));
-  }
+  // This constructor allows you to construct MyVectorType from Eigen expressions
+  template<typename OtherDerived>
+    Matrix3fX(const Eigen::MatrixBase<OtherDerived>& other)
+    : Base(other)
+    {}
 
-  inline Vec3fX<S> getRow(size_t i) const
-  {
-    return Vec3fX<S>(data.getRow(i));
-  }
+  // This method allows you to assign Eigen expressions to MyVectorType
+  template<typename OtherDerived>
+    Matrix3fX& operator=(const Eigen::MatrixBase <OtherDerived>& other)
+    {
+      this->Base::operator=(other);
+      return *this;
+    }
 
+  Matrix3fX(T xx, T xy, T xz,
+            T yx, T yy, T yz,
+            T zx, T zy, T zz)
+    : Base((Base () << xx, xy, xz, yx, yy, yz, zx, zy, zz).finished ())
+  {}
+
+  template <typename OtherDerived>
+  Matrix3fX(const Eigen::MatrixBase<OtherDerived>& v1,
+            const Eigen::MatrixBase<OtherDerived>& v2,
+            const Eigen::MatrixBase<OtherDerived>& v3)
+    : Base((Base () << v1, v2, v3).finished ())
+  {}
+
+  inline ColXpr getColumn(size_t i) { return this->col(i); }
+  inline RowXpr getRow(size_t i)    { return this->row(i); }
+  inline ConstColXpr getColumn(size_t i) const { return this->col(i); }
+  inline ConstRowXpr getRow(size_t i)    const { return this->row(i); }
+
+/*
   inline U operator () (size_t i, size_t j) const
   {
     return data(i, j);
@@ -168,6 +185,7 @@ public:
   {
     data.setIdentity();
   }
+  // */
 
   inline bool isIdentity () const
   {
@@ -177,10 +195,12 @@ public:
       data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
   }
 
+  /*
   inline void setZero()
   {
     data.setZero();
   }
+  // */
 
   /// @brief Set the matrix from euler angles YPR around ZYX axes
   /// @param eulerX Roll about X axis
@@ -218,150 +238,175 @@ public:
     setEulerZYX(roll, pitch, yaw);
   }
 
+  /*
   inline U determinant() const
   {
     return data.determinant();
   }
+  // */
 
   Matrix3fX<T>& transpose() 
   {
-    data.transpose();
+    this->transposeInPlace();
     return *this;
   }
 
   Matrix3fX<T>& inverse()
   {
-    data.inverse();
+    *this = this->inverse().eval ();
     return *this;
   }
 
   Matrix3fX<T>& abs()
   {
-    data.abs();
+    *this = this->cwiseAbs ();
     return *this;
   }
 
   static const Matrix3fX<T>& getIdentity()
   {
-    static const Matrix3fX<T> I((U)1, (U)0, (U)0,
-                                (U)0, (U)1, (U)0,
-                                (U)0, (U)0, (U)1);
+    static const Matrix3fX<T> I((T)1, (T)0, (T)0,
+                                (T)0, (T)1, (T)0,
+                                (T)0, (T)0, (T)1);
     return I;
   }
 
-  Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+  template<typename OtherDerived>
+    const typename Eigen::ProductReturnType< Eigen::Transpose <Matrix3fX<T> >,
+                                             OtherDerived>::Type
+   transposeTimes(const Eigen::MatrixBase<OtherDerived>& other) const
   {
-    return Matrix3fX<T>(data.transposeTimes(other.data));
+    return this->Base::transpose() * other;
   }
 
-  Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+  template<typename OtherDerived>
+    const typename Eigen::ProductReturnType< Matrix3fX<T>,
+                                             Eigen::Transpose <OtherDerived> >
+    ::Type timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
   {
-    return Matrix3fX<T>(data.timesTranspose(other.data));
+    return *this * other.transpose ();
   }
 
+  /*
   Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
   {
     return Vec3fX<S>(data.transposeTimes(v.data));
   }
+  // */
 
-  Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+  template<typename OtherDerived>
+    const Eigen::ProductReturnType<
+      Eigen::ProductReturnType< Matrix3fX<T>,
+                                OtherDerived>::Type,
+      Eigen::Transpose < Matrix3fX<T> >
+        > ::Type
+  tensorTransform(const Eigen::MatrixBase<OtherDerived>& m) const
   {
-    Matrix3fX<T> res(*this);
-    res *= m;
-    return res.timesTranspose(*this);
+    return (*this * m) * this->Base::transpose ();
+    // Matrix3fX<T> res(*this);
+    // res *= m;
+    // return res.timesTranspose(*this);
   }
 
   // (1 0 0)^T (*this)^T v
-  inline U transposeDotX(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T transposeDotX(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.transposeDot(0, v.data);
+    return transposeDot(0, v);
   }
 
   // (0 1 0)^T (*this)^T v
-  inline U transposeDotY(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T transposeDotY(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.transposeDot(1, v.data);
+    return transposeDot(1, v);
   }
 
   // (0 0 1)^T (*this)^T v
-  inline U transposeDotZ(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T transposeDotZ(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.transposeDot(2, v.data);
+    return transposeDot(2, v);
   }
 
   // (\delta_{i3})^T (*this)^T v
-  inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T transposeDot(size_t i, const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.transposeDot(i, v.data);
+    return this->col(i).dot(v);
   }
 
   // (1 0 0)^T (*this) v
-  inline U dotX(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T dotX(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.dot(0, v.data);
+    return dot(0, v);
   }
 
   // (0 1 0)^T (*this) v
-  inline U dotY(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T dotY(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.dot(1, v.data);
+    return dot(1, v);
   }
 
   // (0 0 1)^T (*this) v
-  inline U dotZ(const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T dotZ(const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.dot(2, v.data);
+    return dot(2, v);
   }
 
   // (\delta_{i3})^T (*this) v
-  inline U dot(size_t i, const Vec3fX<S>& v) const
+  template<typename OtherDerived>
+  inline T dot(size_t i, const Eigen::MatrixBase<OtherDerived>& v) const
   {
-    return data.dot(i, v.data);
+    return this->row(i).dot(v);
   }
 
-  inline void setValue(U xx, U xy, U xz,
-                       U yx, U yy, U yz,
-                       U zx, U zy, U zz)
+  inline void setValue(T xx, T xy, T xz,
+                       T yx, T yy, T yz,
+                       T zx, T zy, T zz)
   {
-    data.setValue(xx, xy, xz, 
-                  yx, yy, yz,
-                  zx, zy, zz);
+    (*this)(0,0) = xx; (*this)(0,1) = xy; (*this)(0,2) = xz;
+    (*this)(1,0) = yx; (*this)(1,1) = yy; (*this)(1,2) = yz;
+    (*this)(2,0) = zx; (*this)(2,1) = zy; (*this)(2,2) = zz;
   }
 
-  inline void setValue(U x)
+  inline void setValue(T x)
   {
-    data.setValue(x);
+    this->setConstant (x);
   }
 };
 
-template<typename T>
-void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
+template<typename T, typename OtherDerived>
+void hat(Matrix3fX<T>& mat, const Eigen::MatrixBase<OtherDerived>& vec)
 {
   mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
 }
 
-template<typename T>
-void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
-                       const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
-                       Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
+template<typename T, typename OtherDerived>
+void relativeTransform(const Matrix3fX<T>& R1, const Eigen::MatrixBase<OtherDerived>& t1,
+                       const Matrix3fX<T>& R2, const Eigen::MatrixBase<OtherDerived>& t2,
+                       Matrix3fX<T>& R, Eigen::MatrixBase<OtherDerived>& t)
 {
   R = R1.transposeTimes(R2);
   t = R1.transposeTimes(t2 - t1);
 }
 
 /// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
-template<typename T>
-void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
+template<typename T, typename Derived>
+void eigen(const Matrix3fX<T>& m, T dout[3], const Eigen::MatrixBase<Derived> vout[3])
 {
   Matrix3fX<T> R(m);
   int n = 3;
   int j, iq, ip, i;
-  typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
+  T tresh, theta, tau, t, sm, s, h, g, c;
   int nrot;
-  typename T::meta_type b[3];
-  typename T::meta_type z[3];
-  typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
-  typename T::meta_type d[3];
+  T b[3];
+  T z[3];
+  T v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+  T d[3];
 
   for(ip = 0; ip < n; ++ip)
   {
@@ -439,25 +484,26 @@ void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename
 }
 
 template<typename T>
-Matrix3fX<T> abs(const Matrix3fX<T>& R) 
+const typename CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Matrix3fX<T> >
+abs(const Matrix3fX<T>& R) 
 {
-  return Matrix3fX<T>(abs(R.data));
+  return R.cwiseAbs ();
 }
 
 template<typename T>
-Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+typename Eigen::Transpose <Matrix3fX<T> > transpose(const Matrix3fX<T>& R)
 {
-  return Matrix3fX<T>(transpose(R.data));
+  return R.Matrix3fX<T>::Base::transpose ();
 }
 
 template<typename T>
 Matrix3fX<T> inverse(const Matrix3fX<T>& R)
 {
-  return Matrix3fX<T>(inverse(R.data));
+  return R.Matrix3fX<T>::Base::inverse ().eval ();
 }
 
-template<typename T>
-typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+template<typename T, typename Derived>
+T quadraticForm(const Matrix3fX<T>& R, const Eigen::MatrixBase<Derived>& v)
 {
   return v.dot(R * v);
 }

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 FclOp<typename internal::deduce_fcl_type<Derived, OtherDerived, COEFWISE>::Type> ProductType;
+  typedef FclOp<typename ProductReturnType<Transpose<Derived>, OtherDerived >::Type> TransposeTimesType;
+  typedef FclOp<typename ProductReturnType<Derived, Transpose<OtherDerived> >::Type> TimesTransposeType;
+  typedef FclOp<typename ProductReturnType<ProductType, Transpose<Derived>  >::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 FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::ProductType \
+      operator*(const MatrixBase<OtherDerived>& other) const \
+      { \
+        return FclProduct<const FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::run (*this, other.derived()); \
+      }

include/hpp/fcl/math/math_details.h

@@ -36,6 +36,9 @@
 #ifndef FCL_MATH_DETAILS_H
 #define FCL_MATH_DETAILS_H
 
+#if FCL_HAVE_EIGEN
+# error "This file should not be included when compiling with Eigen library"
+#endif
 
 #include <cmath>
 #include <algorithm>

include/hpp/fcl/math/matrix_3f.h

@@ -51,9 +51,9 @@ namespace fcl
 
 #if FCL_HAVE_EIGEN
 # if FCL_USE_NATIVE_EIGEN
-  typedef Matrix3fX<FCL_REAL> Matrix3f;
+  typedef Eigen::FclMatrix<FCL_REAL, 3, 0> Matrix3f;
 # else
-  typedef Matrix3fX<details::eigen_m3<FCL_REAL> > Matrix3f;
+  typedef Matrix3fX<details::eigen_wrapper_m3<FCL_REAL> > Matrix3f;
 # endif
 #elif FCL_HAVE_SSE
 typedef Matrix3fX<details::sse_meta_f12> Matrix3f;

include/hpp/fcl/math/vec_3f.h

@@ -41,12 +41,6 @@
 #include <hpp/fcl/config-fcl.hh>
 #include <hpp/fcl/data_types.h>
 
-#if FCL_USE_NATIVE_EIGEN
-# include <hpp/fcl/eigen/vec_3fx.h>
-#else
-# include <hpp/fcl/math/vec_3fx.h>
-#endif
-
 #if FCL_HAVE_EIGEN
 # if ! FCL_USE_NATIVE_EIGEN
 #  include <hpp/fcl/eigen/math_details.h>
@@ -57,6 +51,12 @@
 # include <hpp/fcl/math/math_details.h>
 #endif
 
+#if FCL_USE_NATIVE_EIGEN
+# include <hpp/fcl/eigen/vec_3fx.h>
+#else
+# include <hpp/fcl/math/vec_3fx.h>
+#endif
+
 #include <cmath>
 #include <iostream>
 #include <limits>
@@ -67,9 +67,9 @@ namespace fcl
 
 #if FCL_HAVE_EIGEN
 # if FCL_USE_NATIVE_EIGEN
-  typedef Vec3fX<FCL_REAL> Vec3f;
+  typedef Eigen::FclMatrix<FCL_REAL, 1, 0> Vec3f;
 # else
-  typedef Vec3fX<details::eigen_v3<FCL_REAL> > Vec3f;
+  typedef Vec3fX<details::eigen_wrapper_v3<FCL_REAL> > Vec3f;
 # endif
 #elif FCL_HAVE_SSE
   typedef Vec3fX<details::sse_meta_f4> Vec3f;

include/hpp/fcl/math/vec_3fx.h

@@ -41,8 +41,6 @@
 #include <hpp/fcl/config-fcl.hh>
 #include <hpp/fcl/data_types.h>
 
-#include <hpp/fcl/math/math_details.h>
-
 #include <cmath>
 #include <iostream>
 #include <limits>

test/CMakeLists.txt

 config_files(fcl_resources/config.h)
 
+macro(add_fcl_template_test test_name)
+  add_executable(${ARGV})
+  target_link_libraries(${test_name}
+    ${Boost_LIBRARIES}
+    )
+  add_test(${test_name} ${EXECUTABLE_OUTPUT_PATH}/${test_name})
+endmacro(add_fcl_template_test)
+
 macro(add_fcl_test test_name)
   add_executable(${ARGV})
   target_link_libraries(${test_name}
@@ -37,5 +45,5 @@ add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
 if (FCL_HAVE_OCTOMAP)
   add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
 endif()
-add_fcl_test(test_fcl_math test_fcl_math.cpp)
 
+add_fcl_template_test(test_fcl_eigen test_fcl_eigen.cpp)

test/test_fcl_eigen.cpp

+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011-2014, Willow Garage, Inc.
+ *  Copyright (c) 2014-2015, Open Source Robotics Foundation
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define BOOST_TEST_MODULE "FCL_EIGEN"
+#include <boost/test/included/unit_test.hpp>
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/eigen/vec_3fx.h>
+
+using namespace fcl;
+
+#define PRINT_VECTOR(a) std::cout << #a": " << a.base().transpose() << std::endl;
+#define PRINT_MATRIX(a) std::cout << #a": " << a << std::endl;
+
+BOOST_AUTO_TEST_CASE(fcl_eigen_vec3fx)
+{
+  typedef Eigen::FclMatrix <double, 1, 0> Vec3f;
+  Vec3f a (0, 1, 2);
+  Vec3f b (1, 2, 3);
+
+  std::cout << (Vec3f::Base&) a - (Vec3f::Base&) b << std::endl;
+  std::cout << a - b << std::endl;
+  Vec3f c = a - b;
+  std::cout << c << std::endl;
+  c.normalize ();
+
+  Vec3f l = (a - b).normalize ();
+
+  std::cout << l << std::endl;
+  std::cout << c << std::endl;
+
+  Vec3f d = -b;
+  std::cout << d << std::endl;
+
+  d += b;
+  std::cout << d << std::endl;
+
+  d += 1;
+  std::cout << d << std::endl;
+
+  d *= b;
+  std::cout << d << std::endl;
+
+  // std::cout << d * b << std::endl;
+  // std::cout << d / b << std::endl;
+  // std::cout << d + 3.4 << std::endl;
+  // std::cout << d - 3.4 << std::endl;
+  // std::cout << d * 2 << std::endl;
+  // std::cout << d / 3 << std::endl;
+  // std::cout << (d - 3.4).abs().sqrLength() << std::endl;
+
+  // std::cout << (((d - 3.4).abs() + 1) + 3).sqrLength() << std::endl;
+  PRINT_VECTOR(a)
+  PRINT_VECTOR(b)
+  PRINT_VECTOR(min(a,b))
+  PRINT_VECTOR(max(a,b))
+  a.lbound(b);
+  PRINT_VECTOR(a)
+  std::cout << (a+1).lbound(b) << std::endl;
+  std::cout << (a+1).ubound(b) << std::endl;
+
+  std::cout << a.getRow(1) << std::endl;
+}
+
+BOOST_AUTO_TEST_CASE(fcl_eigen_matrix3fx)
+{
+  typedef Eigen::FclMatrix <double, 3, 0> Matrix3fX;
+  Matrix3fX a (0, 1, 2, 3, 4, 5, 6, 7, 8);
+
+  PRINT_MATRIX(a);
+  a.transpose ();
+  PRINT_MATRIX(a);
+  a += a;
+  PRINT_MATRIX(a);
+  a *= a;
+  PRINT_MATRIX(a);
+
+  Matrix3fX b = a; b.inverse ();
+  a += a + a * b;
+  PRINT_MATRIX(a);
+
+  b = a; b.inverse ();
+  a.transpose ();
+  PRINT_MATRIX(a);
+  PRINT_MATRIX(a.transposeTimes (b));
+  PRINT_MATRIX(a.timesTranspose (b));
+  PRINT_MATRIX(a.tensorTransform (b));
+}