diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4ddca1f9b1dfc8e9852199f244c1b767eb1b2f2b..80adcfdf123ee6db15f378ab6dff8979a0504f87 100644
--- a/CMakeLists.txt
+++ b/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,17 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
 
 setup_project()
 
+set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
+set(FCL_HAVE_EIGEN FALSE CACHE BOOL "Use eigen wrappers")
+
+add_optional_dependency("eigen3 >= 3.0.0")
+if (EIGEN3_FOUND)
+  set (FCL_HAVE_EIGEN TRUE)
+  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
@@ -156,6 +168,7 @@ 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
   )
 
 add_subdirectory(src)
diff --git a/include/hpp/fcl/config-fcl.hh.in b/include/hpp/fcl/config-fcl.hh.in
index 14cc27ddfd5960d453ca509ef6444dc2e60e292d..fa302b1b802558eb6c5a2676397b43d404444024 100644
--- a/include/hpp/fcl/config-fcl.hh.in
+++ b/include/hpp/fcl/config-fcl.hh.in
@@ -37,6 +37,7 @@
 
 # include "config.h"
 
+#cmakedefine01 FCL_HAVE_EIGEN
 #cmakedefine01 FCL_HAVE_SSE
 #cmakedefine01 FCL_HAVE_OCTOMAP
 #cmakedefine01 FCL_HAVE_FLANN
diff --git a/include/hpp/fcl/eigen/math_details.h b/include/hpp/fcl/eigen/math_details.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd90acd71ddd2619f20c8cd101cc5c2044af12c3
--- /dev/null
+++ b/include/hpp/fcl/eigen/math_details.h
@@ -0,0 +1,481 @@
+/*
+ * 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.
+ */
+
+#ifndef FCL_EIGEN_DETAILS_H
+#define FCL_EIGEN_DETAILS_H
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+namespace fcl
+{
+
+namespace details
+{
+
+template <typename T>
+struct eigen_wrapper_v3
+{
+  typedef T meta_type;
+  typedef Eigen::Matrix <T, 3, 1> vector_type;
+
+  vector_type v;
+
+  eigen_wrapper_v3() { setValue(0); }
+
+  template <typename Derived>
+    eigen_wrapper_v3(const Eigen::MatrixBase <Derived>& value) :
+      v (value)
+  {}
+
+  eigen_wrapper_v3(T x) :
+    v (vector_type::Constant (x))
+  {}
+
+  eigen_wrapper_v3(T* x) :
+    v (vector_type::Constant (*x))
+  {}
+
+  eigen_wrapper_v3(T x, T y, T z) :
+    v (x, y, z)
+  {}
+
+  inline void setValue(T x, T y, T z)
+  {
+    v << x, y, z;
+  }
+
+  inline void setValue(T x)
+  {
+    v.setConstant (x);
+  }
+
+  inline void negate()
+  {
+    v *= -1;
+  }
+
+  inline eigen_wrapper_v3<T>& ubound(const eigen_wrapper_v3<T>& u)
+  {
+    v = v.cwiseMin (u.v);
+    return *this;
+  }
+
+  inline eigen_wrapper_v3<T>& lbound(const eigen_wrapper_v3<T>& l)
+  {
+    v = v.cwiseMax (l.v);
+    return *this;
+  }
+
+  T operator [] (size_t i) const { return v[i]; }
+  T& operator [] (size_t i) { return v[i]; }
+
+  inline eigen_wrapper_v3<T> operator + (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v + other.v); }
+  inline eigen_wrapper_v3<T> operator - (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v - other.v); }
+  inline eigen_wrapper_v3<T> operator * (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v.cwiseProduct (other.v)); }
+  inline eigen_wrapper_v3<T> operator / (const eigen_wrapper_v3<T>& other) const { return (eigen_wrapper_v3<T>(v) /= other); }
+  inline eigen_wrapper_v3<T>& operator += (const eigen_wrapper_v3<T>& other) { v += other.v; return *this; }
+  inline eigen_wrapper_v3<T>& operator -= (const eigen_wrapper_v3<T>& other) { v -= other.v; return *this; }
+  inline eigen_wrapper_v3<T>& operator *= (const eigen_wrapper_v3<T>& other) { v = v.cwiseProduct  (other.v); return *this; }
+  inline eigen_wrapper_v3<T>& operator /= (const eigen_wrapper_v3<T>& other) { v = v.cwiseQuotient (other.v); return *this; }
+  inline eigen_wrapper_v3<T> operator + (T t) const { return eigen_wrapper_v3<T>((v.array() + t).matrix()); }
+  inline eigen_wrapper_v3<T> operator - (T t) const { return eigen_wrapper_v3<T>((v.array() - t).matrix()); }
+  inline eigen_wrapper_v3<T> operator * (T t) const { return eigen_wrapper_v3<T>(v * t); }
+  inline eigen_wrapper_v3<T> operator / (T t) const { return eigen_wrapper_v3<T>(v / t); }
+  inline eigen_wrapper_v3<T>& operator += (T t) { v.array() += t; return *this; }
+  inline eigen_wrapper_v3<T>& operator -= (T t) { v.array() -= t; return *this; }
+  inline eigen_wrapper_v3<T>& operator *= (T t) { v.array() *= t; return *this; }
+  inline eigen_wrapper_v3<T>& operator /= (T t) { v.array() /= t; return *this; }
+  inline eigen_wrapper_v3<T> operator - () const { return eigen_wrapper_v3<T>(-v); }
+};
+
+
+template <typename T>
+static inline eigen_wrapper_v3<T> cross_prod(const eigen_wrapper_v3<T>& l, const eigen_wrapper_v3<T>& r)
+{
+  return eigen_wrapper_v3<T>(l.v.cross (r.v));
+}
+
+template <typename T>
+static inline T dot_prod3(const eigen_wrapper_v3<T>& l, const eigen_wrapper_v3<T>& r)
+{
+  return l.v.dot(r.v);
+}
+
+template <typename T>
+static inline eigen_wrapper_v3<T> min(const eigen_wrapper_v3<T>& x, const eigen_wrapper_v3<T>& y)
+{
+  return eigen_wrapper_v3<T>(x.v.cwiseMin (y.v));
+}
+
+template <typename T>
+static inline eigen_wrapper_v3<T> max(const eigen_wrapper_v3<T>& x, const eigen_wrapper_v3<T>& y)
+{
+  return eigen_wrapper_v3<T>(x.v.cwiseMax(y.v));
+}
+
+template <typename T>
+static inline eigen_wrapper_v3<T> abs(const eigen_wrapper_v3<T>& x)
+{
+  return eigen_wrapper_v3<T>(x.v.cwiseAbs());
+}
+
+template <typename T>
+static inline bool equal(const eigen_wrapper_v3<T>& x, const eigen_wrapper_v3<T>& y, T epsilon)
+{
+  return ((x.v - y.v).cwiseAbs ().array () < epsilon).all();
+}
+
+namespace internal {
+  template <typename Derived, int Size> struct assign {
+    static Eigen::Matrix<typename Derived::Scalar, 4, 1> run (const Eigen::MatrixBase <Derived>& value)
+    {
+      assert (false);
+    }
+  };
+
+  template <typename Derived> struct assign <Derived, 3> {
+    static Eigen::Matrix<typename Derived::Scalar, 4, 1> run (const Eigen::MatrixBase <Derived>& value)
+    {
+      return (Eigen::Matrix<typename Derived::Scalar, 4, 1> () << value, 0).finished ();
+    }
+  };
+
+  template <typename Derived> struct assign <Derived, 4> {
+    static Eigen::Matrix<typename Derived::Scalar, 4, 1> run (const Eigen::MatrixBase <Derived>& value)
+    {
+      return value;
+    }
+  };
+}
+
+template <typename T>
+struct eigen_wrapper_v4
+{
+  typedef T meta_type;
+  typedef Eigen::Matrix <T, 4, 1> vector_type;
+
+  vector_type v;
+
+  eigen_wrapper_v4() { setValue(0); }
+
+  template <typename Derived>
+    eigen_wrapper_v4(const Eigen::MatrixBase <Derived>& value) :
+      v (internal::assign <Derived, Derived::SizeAtCompileTime>::run (value))
+  {}
+
+  eigen_wrapper_v4(T x) :
+    v (vector_type::Constant (x))
+  {
+    v[3] = 0;
+  }
+
+  eigen_wrapper_v4(T* x) :
+    v (vector_type::Constant (*x))
+  {
+    v[3] = 0;
+  }
+
+  eigen_wrapper_v4(T x, T y, T z) :
+    v (x, y, z, 0)
+  {}
+
+  inline typename vector_type::template FixedSegmentReturnType<3>::Type d () { return v.template head <3> (); }
+
+  inline typename vector_type::template ConstFixedSegmentReturnType<3>::Type d () const { return v.template head <3> (); }
+
+  inline void setValue(T x, T y, T z, T w = 0)
+  {
+    v << x, y, z, w;
+  }
+
+  inline void setValue(T x)
+  {
+    d().setConstant (x);
+    v[3] = 0;
+  }
+
+  inline void negate()
+  {
+    v *= -1;
+  }
+
+  inline eigen_wrapper_v4<T>& ubound(const eigen_wrapper_v4<T>& u)
+  {
+    v = v.cwiseMin (u.v);
+    return *this;
+  }
+
+  inline eigen_wrapper_v4<T>& lbound(const eigen_wrapper_v4<T>& l)
+  {
+    v = v.cwiseMax (l.v);
+    return *this;
+  }
+
+  T operator [] (size_t i) const { return v[i]; }
+  T& operator [] (size_t i) { return v[i]; }
+
+  inline eigen_wrapper_v4<T> operator + (const eigen_wrapper_v4<T>& other) const { return eigen_wrapper_v4<T>(v + other.v); }
+  inline eigen_wrapper_v4<T> operator - (const eigen_wrapper_v4<T>& other) const { return eigen_wrapper_v4<T>(v - other.v); }
+  inline eigen_wrapper_v4<T> operator * (const eigen_wrapper_v4<T>& other) const { return eigen_wrapper_v4<T>(v.cwiseProduct (other.v)); }
+  inline eigen_wrapper_v4<T> operator / (const eigen_wrapper_v4<T>& other) const { return (eigen_wrapper_v4<T>(v) /= other); }
+  inline eigen_wrapper_v4<T>& operator += (const eigen_wrapper_v4<T>& other) { v += other.v; return *this; }
+  inline eigen_wrapper_v4<T>& operator -= (const eigen_wrapper_v4<T>& other) { v -= other.v; return *this; }
+  inline eigen_wrapper_v4<T>& operator *= (const eigen_wrapper_v4<T>& other) { v = v.cwiseProduct  (other.v); return *this; }
+  inline eigen_wrapper_v4<T>& operator /= (const eigen_wrapper_v4<T>& other) { d() = d().cwiseQuotient (other.d()); return *this; }
+  inline eigen_wrapper_v4<T> operator + (T t) const { return eigen_wrapper_v4<T>((d().array() + t).matrix()); }
+  inline eigen_wrapper_v4<T> operator - (T t) const { return eigen_wrapper_v4<T>((d().array() - t).matrix()); }
+  inline eigen_wrapper_v4<T> operator * (T t) const { return eigen_wrapper_v4<T>(v * t); }
+  inline eigen_wrapper_v4<T> operator / (T t) const { return eigen_wrapper_v4<T>(v / t); }
+  inline eigen_wrapper_v4<T>& operator += (T t) { d().array() += t; return *this; }
+  inline eigen_wrapper_v4<T>& operator -= (T t) { d().array() -= t; return *this; }
+  inline eigen_wrapper_v4<T>& operator *= (T t) { v.array() *= t; return *this; }
+  inline eigen_wrapper_v4<T>& operator /= (T t) { v.array() /= t; return *this; }
+  inline eigen_wrapper_v4<T> operator - () const { return eigen_wrapper_v4<T>(-v); }
+} __attribute__ ((aligned));
+
+
+template <typename T>
+static inline eigen_wrapper_v4<T> cross_prod(const eigen_wrapper_v4<T>& l, const eigen_wrapper_v4<T>& r)
+{
+  return eigen_wrapper_v4<T>(l.v.cross3 (r.v));
+}
+
+template <typename T>
+static inline T dot_prod3(const eigen_wrapper_v4<T>& l, const eigen_wrapper_v4<T>& r)
+{
+  return l.v.dot(r.v);
+}
+
+template <typename T>
+static inline eigen_wrapper_v4<T> min(const eigen_wrapper_v4<T>& x, const eigen_wrapper_v4<T>& y)
+{
+  return eigen_wrapper_v4<T>(x.v.cwiseMin (y.v));
+}
+
+template <typename T>
+static inline eigen_wrapper_v4<T> max(const eigen_wrapper_v4<T>& x, const eigen_wrapper_v4<T>& y)
+{
+  return eigen_wrapper_v4<T>(x.v.cwiseMax(y.v));
+}
+
+template <typename T>
+static inline eigen_wrapper_v4<T> abs(const eigen_wrapper_v4<T>& x)
+{
+  return eigen_wrapper_v4<T>(x.v.cwiseAbs());
+}
+
+template <typename T>
+static inline bool equal(const eigen_wrapper_v4<T>& x, const eigen_wrapper_v4<T>& y, T epsilon)
+{
+  return ((x.v - y.v).cwiseAbs ().array () < epsilon).all();
+}
+
+
+template<typename T>
+struct eigen_wrapper_m3
+{
+  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;
+  typedef typename matrix_type::ConstRowXpr ConstRowXpr;
+
+  matrix_type m;
+  eigen_wrapper_m3() {};
+
+  template <typename Derived>
+    eigen_wrapper_m3(const Eigen::MatrixBase <Derived>& matrix) :
+      m (matrix)
+  {}
+
+  eigen_wrapper_m3(T xx, T xy, T xz,
+                   T yx, T yy, T yz,
+                   T zx, T zy, T zz)
+  {
+    setValue(xx, xy, xz,
+             yx, yy, yz,
+             zx, zy, zz);
+  }
+
+  eigen_wrapper_m3(const vector_type& v1, const vector_type& v2, const vector_type& v3)
+  {
+    m << v1.v, v2.v, v3.v;
+  }
+
+  eigen_wrapper_m3(const eigen_wrapper_m3<T>& other) { m = other.m; }
+
+  inline inner_col_type getColumn(size_t i) const { return m.col (i); }
+  inline ConstRowXpr getRow(size_t i) const { return m.row (i); }
+
+  inline T operator() (size_t i, size_t j) const { return m(i, j); }
+  inline T& operator() (size_t i, size_t j) { return m(i, j); }
+
+  inline vector_type operator * (const vector_type& v) const { return vector_type(m * v.v); }
+
+  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 - (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 *= (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; }
+
+
+  void setIdentity() { m.setIdentity (); }
+
+  void setZero() { m.setZero(); }
+
+  static const eigen_wrapper_m3<T>& getIdentity()
+  {
+    static const eigen_wrapper_m3<T> I(matrix_type::Identity ());
+    return I;
+  }
+
+  T determinant() const { return m.determinant (); }
+
+  eigen_wrapper_m3<T>& transpose()
+  {
+    m.transposeInPlace ();
+    return *this;
+  }
+
+  eigen_wrapper_m3<T>& inverse()
+  {
+    m = m.inverse().eval();
+    return *this;
+  }
+
+  eigen_wrapper_m3<T> transposeTimes(const eigen_wrapper_m3<T>& other) const
+  {
+    return eigen_wrapper_m3<T>(m.transpose () * other.m);
+  }
+
+  eigen_wrapper_m3<T> timesTranspose(const eigen_wrapper_m3<T>& other) const
+  {
+    return eigen_wrapper_m3<T>(m * other.transpose ());
+  }
+
+  vector_type transposeTimes(const vector_type& other) const
+  {
+    return vector_type(m.transpose () * other.v);
+  }
+
+  inline T transposeDotX(const vector_type& other) const
+  {
+    return m.col(0).dot(other.v);
+  }
+
+  inline T transposeDotY(const vector_type& other) const
+  {
+    return m.col(1).dot(other.v);
+  }
+
+  inline T transposeDotZ(const vector_type& other) const
+  {
+    return m.col(2).dot(other.v);
+  }
+
+  inline T transposeDot(size_t i, const vector_type& other) const
+  {
+    return m.col (i).dot(other.v);
+  }
+
+  inline T dotX(const vector_type& other) const
+  {
+    return m.row (0).dot (other.v);
+  }
+
+  inline T dotY(const vector_type& other) const
+  {
+    return m.row (1).dot (other.v);
+  }
+
+  inline T dotZ(const vector_type& other) const
+  {
+    return m.row (2).dot (other.v);
+  }
+
+  inline T dot(size_t i, const vector_type& other) const
+  {
+    return m.row (i).dot (other.v);
+  }
+
+  inline void setValue(T xx, T xy, T xz,
+                       T yx, T yy, T yz,
+                       T zx, T zy, T zz)
+  {
+    m << xx, xy, xz,
+         yx, yy, yz,
+         zx, zy, zz;
+  }
+
+  inline void setValue(T x) { m.setConstant (x); }
+};
+
+
+template<typename T>
+eigen_wrapper_m3<T> abs(const eigen_wrapper_m3<T>& m)
+{
+  return eigen_wrapper_m3<T>(m.m.cwiseAbs ());
+}
+
+template<typename T>
+eigen_wrapper_m3<T> transpose(const eigen_wrapper_m3<T>& m)
+{
+  return eigen_wrapper_m3<T>(m.m.transpose ());
+}
+
+
+template<typename T>
+eigen_wrapper_m3<T> inverse(const eigen_wrapper_m3<T>& m)
+{
+  return eigen_wrapper_m3<T> (m.m.inverse().eval ());
+}
+
+}
+
+}
+
+#endif
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index f67934e9c22e962897c3b48649bd9d70efae5b0d..fea3affae128edd389a16bc99780e44ba1344de8 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -466,7 +466,9 @@ typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename
 }
 
 
-#if FCL_HAVE_SSE
+#if FCL_HAVE_EIGEN
+typedef Matrix3fX<details::eigen_wrapper_m3<FCL_REAL> > Matrix3f;
+#elif FCL_HAVE_SSE
 typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
 #else
 typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index 555d12b6249f3d447ef86ff7ca22d36a8add2277..a959c2b47d8c58c9ecc258d87e36daa309ddea1a 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -42,7 +42,9 @@
 #include <hpp/fcl/data_types.h>
 #include <hpp/fcl/math/math_details.h>
 
-#if FCL_HAVE_SSE
+#if FCL_HAVE_EIGEN
+#include <hpp/fcl/eigen/math_details.h>
+#elif FCL_HAVE_SSE
 #include <hpp/fcl/simd/math_simd_details.h>
 #endif
 
@@ -233,7 +235,9 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
   }
 }
 
-#if FCL_HAVE_SSE
+#if FCL_HAVE_EIGEN
+  typedef Vec3fX<details::eigen_wrapper_v3<FCL_REAL> > Vec3f;
+#elif FCL_HAVE_SSE
   typedef Vec3fX<details::sse_meta_f4> Vec3f;
 #else
   typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5ee7213a634a5967e13b8e0046af3b9ee99856aa..e3e480108a512ccea88e526a9428e331b56fbfbf 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -37,4 +37,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)
 
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index e35c21d09755461ecafcb9c3f09df7d67dc8b5a9..18ee66bb85251e3c17665ae62172d21294053f79 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -37,6 +37,9 @@
 #define BOOST_TEST_MODULE "FCL_MATH"
 #include <boost/test/included/unit_test.hpp>
 
+#if FCL_HAVE_EIGEN
+#include <hpp/fcl/eigen/math_details.h>
+#endif
 #if FCL_HAVE_SSE
 #include <hpp/fcl/simd/math_simd_details.h>
 #endif
@@ -163,6 +166,471 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
   BOOST_CHECK(v1.dot(v2) == 26);
 }
 
+#if FCL_HAVE_EIGEN
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32)
+{
+  typedef Vec3fX<details::eigen_wrapper_v3 <float> > Vec3f32;
+  Vec3f32 v1(1.0f, 2.0f, 3.0f);
+  BOOST_CHECK(v1[0] == 1.0f);
+  BOOST_CHECK(v1[1] == 2.0f);
+  BOOST_CHECK(v1[2] == 3.0f);
+
+  Vec3f32 v2 = v1;
+  Vec3f32 v3(3.3f, 4.3f, 5.3f);
+  v1 += v3;
+  BOOST_CHECK(v1.equal(v2 + v3));
+  v1 -= v3;
+  BOOST_CHECK(v1.equal(v2));
+  v1 -= v3;
+  BOOST_CHECK(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  BOOST_CHECK(v1.equal(v2 * v3));
+  v1 /= v3;
+  BOOST_CHECK(v1.equal(v2));
+  v1 /= v3;
+  BOOST_CHECK(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0f;
+  BOOST_CHECK(v1.equal(v2 * 2.0f));
+  v1 /= 2.0f;
+  BOOST_CHECK(v1.equal(v2));
+  v1 /= 2.0f;
+  BOOST_CHECK(v1.equal(v2 / 2.0f));
+  v1 *= 2.0f;
+
+  v1 += 2.0f;
+  BOOST_CHECK(v1.equal(v2 + 2.0f));
+  v1 -= 2.0f;
+  BOOST_CHECK(v1.equal(v2));
+  v1 -= 2.0f;
+  BOOST_CHECK(v1.equal(v2 - 2.0f));
+  v1 += 2.0f;
+  
+  BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
+
+  v1 = Vec3f32(1.0f, 2.0f, 3.0f);
+  v2 = Vec3f32(3.0f, 4.0f, 5.0f);
+  BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
+  BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f32(3.0f, 4.0f, 5.0f);
+  BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
+  BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
+  BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
+{
+  typedef Vec3fX<details::eigen_wrapper_v3<double> > Vec3f64;
+  Vec3f64 v1(1.0, 2.0, 3.0);
+  BOOST_CHECK(v1[0] == 1.0);
+  BOOST_CHECK(v1[1] == 2.0);
+  BOOST_CHECK(v1[2] == 3.0);
+
+  Vec3f64 v2 = v1;
+  Vec3f64 v3(3.3, 4.3, 5.3);
+  v1 += v3;
+  BOOST_CHECK(v1.equal(v2 + v3));
+  v1 -= v3;
+  BOOST_CHECK(v1.equal(v2));
+  v1 -= v3;
+  BOOST_CHECK(v1.equal(v2 - v3));
+  v1 += v3;
+
+  v1 *= v3;
+  BOOST_CHECK(v1.equal(v2 * v3));
+  v1 /= v3;
+  BOOST_CHECK(v1.equal(v2));
+  v1 /= v3;
+  BOOST_CHECK(v1.equal(v2 / v3));
+  v1 *= v3;
+
+  v1 *= 2.0;
+  BOOST_CHECK(v1.equal(v2 * 2.0));
+  v1 /= 2.0;
+  BOOST_CHECK(v1.equal(v2));
+  v1 /= 2.0;
+  BOOST_CHECK(v1.equal(v2 / 2.0));
+  v1 *= 2.0;
+
+  v1 += 2.0;
+  BOOST_CHECK(v1.equal(v2 + 2.0));
+  v1 -= 2.0;
+  BOOST_CHECK(v1.equal(v2));
+  v1 -= 2.0;
+  BOOST_CHECK(v1.equal(v2 - 2.0));
+  v1 += 2.0;
+
+  BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
+
+  v1 = Vec3f64(3.0, 4.0, 5.0);
+  BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
+  BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
+  BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+
+
+  v1 = Vec3f64(1.0, 2.0, 3.0);
+  v2 = Vec3f64(3.0, 4.0, 5.0);
+  BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+  BOOST_CHECK(v1.dot(v2) == 26);
+}
+
+BOOST_AUTO_TEST_CASE(eigen_mat32_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+  typedef Vec3fX<details::eigen_wrapper_v3 <float> > Vec3f32Eigen;
+
+  typedef Matrix3fX<details::Matrix3Data<details::Vec3Data<float> > > Matrix3f32;
+  typedef Matrix3fX<details::eigen_wrapper_m3<details::eigen_wrapper_v3<float> > > Matrix3f32Eigen;
+
+  Vec3f32 v1(1, 2, 3);
+  Vec3f32Eigen v2(1, 2, 3);
+
+  Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+  Matrix3f32Eigen m2(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      BOOST_CHECK((m1(i, j) - m2(i, j) < 1e-1));
+  
+  Matrix3f32 m3(transpose(m1));
+  Matrix3f32Eigen m4(transpose(m2));
+        
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = m1; m3.transpose();
+  m4 = m2; m4.transpose();
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = inverse(m1);
+  m4 = inverse(m2);
+  
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+  m3 = m1; m3.inverse();
+  m4 = m2; m4.inverse();
+
+  for(size_t i = 0; i < 3; ++i)
+    for(size_t j = 0; j < 3; ++j)
+      BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+  typedef Vec3fX<details::eigen_wrapper_v3<float> > Vec3f32Eigen;
+
+  Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f);
+  Vec3f32Eigen v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f);
+  Vec3f32 v12 = v1 + v2;
+  Vec3f32Eigen v34 = v3 + v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - v2;
+  v34 = v3 - v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * v2;
+  v34 = v3 * v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / v2;
+  v34 = v3 / v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  float t = 1234.433f;
+  v12 = v1 + t;
+  v34 = v3 + t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - t;
+  v34 = v3 - t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * t;
+  v34 = v3 * t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / t;
+  v34 = v3 / t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12 += v2;
+  v34 = v3; v34 += v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= v2;
+  v34 = v3; v34 -= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= v2;
+  v34 = v3; v34 *= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= v2;
+  v34 = v3; v34 /= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 += t;
+  v34 = v3; v34 += t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= t;
+  v34 = v3; v34 -= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= t;
+  v34 = v3; v34 *= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= t;
+  v34 = v3; v34 /= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = -v1;
+  v34 = -v3;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1.cross(v2);
+  v34 = v3.cross(v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  // Difference if about 6e-5
+  BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-4);
+
+  v12 = min(v1, v2);
+  v34 = min(v3, v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = max(v1, v2);
+  v34 = max(v3, v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = abs(v2);
+  v34 = abs(v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f);
+  Vec3f32Eigen delta2(1e-9f, 1e-9f, 1e-9f);
+  BOOST_CHECK((v1 + delta1).equal(v1));
+  BOOST_CHECK((v3 + delta2).equal(v3));
+
+  BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
+  BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ 
+  v12 = v1; v12.negate();
+  v34 = v3; v34.negate();
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12.normalize();
+  v34 = v3; v34.normalize();
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  
+  v12 = normalize(v1);
+  v34 = normalize(v3);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);  
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64_consistent)
+{
+  typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
+  typedef Vec3fX<details::eigen_wrapper_v3<double> > Vec3f64Eigen;
+
+  Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4);
+  Vec3f64Eigen v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4);
+  Vec3f64 v12 = v1 + v2;
+  Vec3f64Eigen v34 = v3 + v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - v2;
+  v34 = v3 - v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * v2;
+  v34 = v3 * v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / v2;
+  v34 = v3 / v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  double t = 1234.433;
+  v12 = v1 + t;
+  v34 = v3 + t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 - t;
+  v34 = v3 - t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 * t;
+  v34 = v3 * t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1 / t;
+  v34 = v3 / t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12 += v2;
+  v34 = v3; v34 += v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= v2;
+  v34 = v3; v34 -= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= v2;
+  v34 = v3; v34 *= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= v2;
+  v34 = v3; v34 /= v4;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 += t;
+  v34 = v3; v34 += t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 -= t;
+  v34 = v3; v34 -= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 *= t;
+  v34 = v3; v34 *= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = v1; v12 /= t;
+  v34 = v3; v34 /= t;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = -v1;
+  v34 = -v3;
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1.cross(v2);
+  v34 = v3.cross(v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
+
+  v12 = min(v1, v2);
+  v34 = min(v3, v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  v12 = max(v1, v2);
+  v34 = max(v3, v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = abs(v2);
+  v34 = abs(v4);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  Vec3f64 delta1(1e-15, 1e-15, 1e-15);
+  Vec3f64Eigen delta2(1e-15, 1e-15, 1e-15);
+  BOOST_CHECK((v1 + delta1).equal(v1));
+  BOOST_CHECK((v3 + delta2).equal(v3));
+
+  BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
+  BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+ 
+  v12 = v1; v12.negate();
+  v34 = v3; v34.negate();
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+  v12 = v1; v12.normalize();
+  v34 = v3; v34.normalize();
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+  
+  v12 = normalize(v1);
+  v34 = normalize(v3);
+  BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+  BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);  
+}
+
+#endif
+
+
 #if FCL_HAVE_SSE
 
 BOOST_AUTO_TEST_CASE(vec_test_sse_vec32)