From 6b2a7ebc1358c54b8e5a9dc2210efd8ffcc823bb Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 13 Jun 2016 19:23:27 +0200
Subject: [PATCH] Fix Transform3f::transform

---
 include/hpp/fcl/math/transform.h |  140 ++--
 test/CMakeLists.txt              |    2 +
 test/test_fcl_math.cpp           | 1063 ++----------------------------
 3 files changed, 169 insertions(+), 1036 deletions(-)

diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index 3eba2726..6516d45e 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -42,6 +42,48 @@
 #include <hpp/fcl/math/matrix_3f.h>
 #include <boost/thread/mutex.hpp>
 
+namespace fcl {
+  class Quaternion3f;
+  template<typename RhsType> struct quaternion_transform_return_type;
+  template<typename Lhs, typename Rhs> struct translate_return_type;
+
+  template<typename RhsType>
+  struct quaternion_transform_return_type_traits {
+    typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
+    typedef typename Vec4f::     FixedSegmentReturnType<3>::Type XYZ_t;
+    typedef typename Vec4f::ConstFixedSegmentReturnType<3>::Type XYZConst_t;
+
+    typedef typename XYZConst_t::cross_product_return_type<RhsType>::type Cross_t;
+    typedef Eigen::CwiseBinaryOp <
+      Eigen::internal::scalar_sum_op <FCL_REAL>,
+      const typename RhsType::ScalarMultipleReturnType,
+      const typename fcl::Vec3f::ScalarMultipleReturnType >
+        rhs_type;
+    typedef Eigen::CwiseBinaryOp <
+      Eigen::internal::scalar_sum_op <FCL_REAL>,
+      const typename XYZConst_t::ScalarMultipleReturnType,
+      const rhs_type >
+        type;
+  };
+
+  template<typename Lhs, typename Rhs>
+  struct translate_return_type_traits {
+    typedef Eigen::CwiseBinaryOp <
+      Eigen::internal::scalar_sum_op <FCL_REAL>,
+      const quaternion_transform_return_type<Lhs>,
+      const Rhs > type;
+  };
+}
+
+namespace Eigen {
+  namespace internal {
+    template<typename Derived> struct traits<typename fcl::quaternion_transform_return_type<Derived> > :
+      traits< typename fcl::quaternion_transform_return_type_traits<Derived>::type > {};
+    template<typename Lhs, typename Rhs> struct traits<typename fcl::translate_return_type<Lhs, Rhs> > :
+      traits< typename fcl::translate_return_type_traits<Lhs, Rhs>::type > {};
+  }
+}
+
 namespace fcl
 {
 
@@ -54,20 +96,6 @@ private:
   typedef typename Vec4f::ConstFixedSegmentReturnType<3>::Type XYZConst_t;
 
 public:
-  template<typename Derived> struct transform_return_type {
-    typedef typename XYZConst_t::cross_product_return_type<Derived>::type Cross_t;
-    typedef Eigen::CwiseBinaryOp <
-      Eigen::internal::scalar_sum_op <FCL_REAL>,
-      const typename Derived::ScalarMultipleReturnType,
-      const typename Cross_t::ScalarMultipleReturnType >
-      RightSum_t;
-    typedef Eigen::CwiseBinaryOp <
-      Eigen::internal::scalar_sum_op <FCL_REAL>,
-      const typename XYZConst_t::ScalarMultipleReturnType,
-      const RightSum_t >
-      Type;
-  };
-
   enum {
     W = 0,
     X = 1,
@@ -171,7 +199,9 @@ public:
 
   /// @brief inverse
   inline Quaternion3f inverse() const {
-    return conj();
+    Quaternion3f inv = conj();
+    inv.normalize();
+    return inv;
   }
 
   inline void normalize () {
@@ -181,19 +211,8 @@ public:
 
   /// @brief rotate a vector
   template<typename Derived>
-    /*inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const*/
-  inline typename transform_return_type<Derived>::Type
-  transform(const Eigen::MatrixBase<Derived>& v) const
-  {
-    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
-    Vec4f::ConstFixedSegmentReturnType<3>::Type u (data.segment<3>(X));
-    const FCL_REAL& s = w();
-    /*return 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);*/
-    const FCL_REAL uDv = u.dot(v);
-    const FCL_REAL uDu = u.dot(u);
-    const typename transform_return_type<Derived>::RightSum_t rhs((s*s - uDu)*v, 2*s*u.cross(v.derived()));
-    return typename transform_return_type<Derived>::Type (2*uDv*u, rhs);
-  }
+  inline const quaternion_transform_return_type<Derived>
+  transform(const Eigen::MatrixBase<Derived>& v) const;
 
   bool operator == (const Quaternion3f& other) const
   {
@@ -242,26 +261,68 @@ private:
   inline Vec4f::ConstFixedSegmentReturnType<3>::Type vec() const { return data.segment<3>(X); }
 
   Vec4f data;
+
+  template<typename Derived>
+  friend struct quaternion_transform_return_type;
 };
 
+template<typename RhsType> struct quaternion_transform_return_type :
+  quaternion_transform_return_type_traits<RhsType>::type
+{
+  typedef quaternion_transform_return_type_traits<RhsType> quat_traits;
+
+  typedef typename quat_traits::type Base;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(quaternion_transform_return_type)
+
+  EIGEN_STRONG_INLINE quaternion_transform_return_type(const Quaternion3f& q, const Eigen::MatrixBase<RhsType>& v) :
+      Base (2*q.vec().dot(v) * q.vec(), ((q.w()*q.w() - q.vec().dot(q.vec()))*v + 2*q.w()*m_uCrossV)),
+      m_uCrossV (q.vec().cross(v))
+  {}
+
+  typename quat_traits::Cross_t m_uCrossV;
+};
+
+template<typename LhsType, typename RhsType> struct translate_return_type :
+  translate_return_type_traits<LhsType, RhsType>::type
+{
+  typedef translate_return_type_traits<LhsType, RhsType> trans_traits;
+
+  typedef typename trans_traits::type Base;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(translate_return_type)
+
+  EIGEN_STRONG_INLINE translate_return_type(const quaternion_transform_return_type<LhsType>& rv, const RhsType& T) :
+    Base (rv, T)
+  {}
+};
+
+template<typename Derived, typename OtherDerived>
+const translate_return_type<Derived, OtherDerived>
+operator+ (const quaternion_transform_return_type<Derived>& rv,
+           const Eigen::MatrixBase<OtherDerived>& T)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3);
+  return translate_return_type<Derived, OtherDerived>(rv, T.derived());
+}
+
+template<typename Derived>
+const quaternion_transform_return_type<Derived>
+Quaternion3f::transform (const Eigen::MatrixBase<Derived>& v) const
+{
+  return quaternion_transform_return_type<Derived> (*this, v.derived());
+}
+
 static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
 {
   o << "(" << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << ")";
   return o;
 }
 
-
 /// @brief Simple transform class used locally by InterpMotion
 class Transform3f
 {
-  template<typename Derived> struct transform_return_type {
-    typedef Eigen::CwiseBinaryOp <
-      Eigen::internal::scalar_sum_op <FCL_REAL>,
-      const typename Quaternion3f::transform_return_type<Derived>::Type,
-      const Vec3f >
-      Type;
-  };
-
   boost::mutex lock_;
 
   /// @brief Whether matrix cache is set
@@ -398,9 +459,8 @@ public:
 
   /// @brief transform a given vector by the transform
   template <typename Derived>
-  inline typename transform_return_type<Derived>::Type transform(const Eigen::MatrixBase<Derived>& v) const
+  inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
   {
-    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
     return q.transform(v) + T;
   }
 
@@ -408,7 +468,7 @@ public:
   inline Transform3f& inverse()
   {
     matrix_set = false;
-    q.conj();
+    q = q.conj();
     T = q.transform(-T).eval();
     return *this;
   }
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 1520f173..c1c2ea61 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -21,6 +21,8 @@ include_directories(${CMAKE_CURRENT_BINARY_DIR})
 include_directories(${Boost_INCLUDE_DIRS})
 
 
+add_fcl_template_test(test_fcl_math test_fcl_math.cpp)
+
 add_fcl_test(test_fcl_collision test_fcl_collision.cpp test_fcl_utility.cpp)
 add_fcl_test(test_fcl_distance test_fcl_distance.cpp test_fcl_utility.cpp)
 add_fcl_test(test_fcl_distance_lower_bound test_fcl_distance_lower_bound.cpp
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index 2262b579..56dbfc3c 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -37,1051 +37,122 @@
 #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
 #include <hpp/fcl/math/vec_3f.h>
 #include <hpp/fcl/math/matrix_3f.h>
+#include <hpp/fcl/math/transform.h>
 #include <hpp/fcl/broadphase/morton.h>
 #include <hpp/fcl/config.h>
 
 using namespace fcl;
 
-BOOST_AUTO_TEST_CASE(vec_test_basic_vec32)
-{
-  typedef Vec3fX<details::Vec3Data<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.squaredNorm() - 50.0) < 1e-5);
-  BOOST_CHECK(std::abs(v1.norm() - sqrt(50.0)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
-}
-
-BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
-{
-  typedef Vec3fX<details::Vec3Data<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.squaredNorm() - 50.0) < 1e-5);
-  BOOST_CHECK(std::abs(v1.norm() - sqrt(50.0)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
-
-
-  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);
-}
-
-#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.squaredNorm() - 50) < 1e-5);
-  BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
-}
-
 BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
 {
-  typedef Vec3fX<details::eigen_wrapper_v3<double> > Vec3f64;
-  Vec3f64 v1(1.0, 2.0, 3.0);
+  Vec3f 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);
+  Vec3f v2 = v1;
+  Vec3f v3(3.3, 4.3, 5.3);
   v1 += v3;
-  BOOST_CHECK(v1.equal(v2 + v3));
+  BOOST_CHECK(isEqual(v1, v2 + v3));
   v1 -= v3;
-  BOOST_CHECK(v1.equal(v2));
+  BOOST_CHECK(isEqual(v1, v2));
   v1 -= v3;
-  BOOST_CHECK(v1.equal(v2 - v3));
+  BOOST_CHECK(isEqual(v1, 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.array() *= v3.array();
+  BOOST_CHECK(isEqual(v1, v2.cwiseProduct(v3)));
+  v1.array() /= v3.array();
+  BOOST_CHECK(isEqual(v1, v2));
+  v1.array() /= v3.array();
+  BOOST_CHECK(isEqual(v1, v2.cwiseQuotient(v3)));
+  v1.array() *= v3.array();
 
   v1 *= 2.0;
-  BOOST_CHECK(v1.equal(v2 * 2.0));
+  BOOST_CHECK(isEqual(v1, v2 * 2.0));
   v1 /= 2.0;
-  BOOST_CHECK(v1.equal(v2));
+  BOOST_CHECK(isEqual(v1, v2));
   v1 /= 2.0;
-  BOOST_CHECK(v1.equal(v2 / 2.0));
+  BOOST_CHECK(isEqual(v1, 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;
+  v1.array() += 2.0;
+  BOOST_CHECK(isEqual(v1, (v2.array() + 2.0).matrix()));
+  v1.array() -= 2.0;
+  BOOST_CHECK(isEqual(v1, v2));
+  v1.array() -= 2.0;
+  BOOST_CHECK(isEqual(v1, (v2.array() - 2.0).matrix()));
+  v1.array() += 2.0;
 
-  BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
+  BOOST_CHECK(isEqual((-Vec3f(1.0, 2.0, 3.0)), Vec3f(-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)));
+  v1 = Vec3f(1.0, 2.0, 3.0);
+  v2 = Vec3f(3.0, 4.0, 5.0);
+  BOOST_CHECK(isEqual((v1.cross(v2)), Vec3f(-2.0, 4.0, -2.0)));
   BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
 
-  v1 = Vec3f64(3.0, 4.0, 5.0);
+  v1 = Vec3f(3.0, 4.0, 5.0);
   BOOST_CHECK(std::abs(v1.squaredNorm() - 50) < 1e-5);
   BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
+  BOOST_CHECK(isEqual(v1.normalized(), v1 / v1.norm()));
 
 
-  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)));
+  v1 = Vec3f(1.0, 2.0, 3.0);
+  v2 = Vec3f(3.0, 4.0, 5.0);
+  BOOST_CHECK(isEqual(v1.cross(v2), Vec3f(-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 = transpose(m1);
-  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 = 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 = 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));
-}
-
-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.norm() - v3.norm()) < 1e-5);
-  BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 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.norm() - v3.norm()) < 1e-5);
-  BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 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)
+BOOST_AUTO_TEST_CASE(morton)
 {
-  typedef Vec3fX<details::sse_meta_f4> 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;
+  AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000));
+  morton_functor<boost::dynamic_bitset<> > F1(bbox, 10);
+  morton_functor<boost::dynamic_bitset<> > F2(bbox, 20);
+  morton_functor<FCL_UINT64> F3(bbox);
+  morton_functor<FCL_UINT32> F4(bbox);
 
-  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;
+  Vec3f p(254, 873, 674);
+  std::cout << std::hex << F1(p).to_ulong() << std::endl;
+  std::cout << std::hex << F2(p).to_ulong() << std::endl;
+  std::cout << std::hex << F3(p) << std::endl;
+  std::cout << std::hex << F4(p) << std::endl;
   
-  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.squaredNorm() - 50) < 1e-5);
-  BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
 }
 
-BOOST_AUTO_TEST_CASE(vec_test_sse_vec64)
-{
-  typedef Vec3fX<details::sse_meta_d4> 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.squaredNorm() - 50) < 1e-5);
-  BOOST_CHECK(std::abs(v1.norm() - sqrt(50)) < 1e-5);
-  BOOST_CHECK(normalize(v1).equal(v1 / v1.norm()));
-
-
-  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(sse_mat32_consistent)
-{
-  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
-  typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
-
-  typedef Matrix3fX<details::Matrix3Data<float> > Matrix3f32;
-  typedef Matrix3fX<details::sse_meta_f12> Matrix3f32SSE;
-
-  Vec3f32 v1(1, 2, 3);
-  Vec3f32SSE v2(1, 2, 3);
-
-  Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1);
-  Matrix3f32SSE 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));
-  Matrix3f32SSE 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 = transpose(m1);
-  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 = 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 = 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));
+Vec3f rotate (Vec3f input, FCL_REAL w, Vec3f vec) {
+  return 2*vec.dot(input)*vec + (w*w - vec.dot(vec))*input + 2*w*vec.cross(input);
 }
 
-BOOST_AUTO_TEST_CASE(vec_test_sse_vec32_consistent)
+BOOST_AUTO_TEST_CASE(quaternion)
 {
-  typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
-  typedef Vec3fX<details::sse_meta_f4> Vec3f32SSE;
-
-  Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f);
-  Vec3f32SSE v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f);
-  Vec3f32 v12 = v1 + v2;
-  Vec3f32SSE 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);
+  Quaternion3f q1, q2, q3;
+  q2.fromAxisAngle(Vec3f(0,0,1), M_PI/2);
+  q3 = q2.inverse();
 
-  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);
+  Vec3f v(1,-1,0);
 
-  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);
-
-  Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f);
-  Vec3f32SSE delta2(1e-9f, 1e-9f, 1e-9f);
-  BOOST_CHECK((v1 + delta1).equal(v1));
-  BOOST_CHECK((v3 + delta2).equal(v3));
-
-  BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
-  BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 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_CHECK(isEqual(v, q1.transform(v)));
+  BOOST_CHECK(isEqual(Vec3f(1,1,0), q2.transform(v)));
+  BOOST_CHECK(isEqual(rotate(v, q3.w(), Vec3f(q3.x(), q3.y(), q3.z())), q3.transform(v)));
 }
 
-BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent)
+BOOST_AUTO_TEST_CASE(transform)
 {
-  typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
-  typedef Vec3fX<details::sse_meta_d4> Vec3f64SSE;
+  Quaternion3f q;
+  q.fromAxisAngle(Vec3f(0,0,1), M_PI/2);
+  Vec3f T (0,1,2);
+  Transform3f tf (q, T);
 
-  Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4);
-  Vec3f64SSE v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4);
-  Vec3f64 v12 = v1 + v2;
-  Vec3f64SSE 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);
+  Vec3f v(1,-1,0);
 
-  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);
+  BOOST_CHECK(isEqual(q.transform(v).eval() + T, q.transform(v) + T));
 
-  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);
-  Vec3f64SSE delta2(1e-15, 1e-15, 1e-15);
-  BOOST_CHECK((v1 + delta1).equal(v1));
-  BOOST_CHECK((v3 + delta2).equal(v3));
-
-  BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
-  BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 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
-
-BOOST_AUTO_TEST_CASE(morton)
-{
-  AABB bbox(Vec3f(0, 0, 0), Vec3f(1000, 1000, 1000));
-  morton_functor<boost::dynamic_bitset<> > F1(bbox, 10);
-  morton_functor<boost::dynamic_bitset<> > F2(bbox, 20);
-  morton_functor<FCL_UINT64> F3(bbox);
-  morton_functor<FCL_UINT32> F4(bbox);
-
-  Vec3f p(254, 873, 674);
-  std::cout << std::hex << F1(p).to_ulong() << std::endl;
-  std::cout << std::hex << F2(p).to_ulong() << std::endl;
-  std::cout << std::hex << F3(p) << std::endl;
-  std::cout << std::hex << F4(p) << std::endl;
-  
+  Vec3f rv (q.transform(v));
+  // typename Transform3f::transform_return_type<Vec3f>::type output =
+    // tf.transform(v);
+  // std::cout << rv << std::endl;
+  // std::cout << output.lhs() << std::endl;
+  BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
 }
-- 
GitLab