Add math details based on Eigen library.

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)

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

include/hpp/fcl/eigen/math_details.h

+/*
+ * 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

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;

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;

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)
 

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);