From 0d64ce1e1a676b304557de8d24438f6813162f50 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 11 Mar 2016 13:47:59 +0100
Subject: [PATCH] Add math details based on Eigen library.
---
CMakeLists.txt | 15 +-
include/hpp/fcl/config-fcl.hh.in | 1 +
include/hpp/fcl/eigen/math_details.h | 481 +++++++++++++++++++++++++++
include/hpp/fcl/math/matrix_3f.h | 4 +-
include/hpp/fcl/math/vec_3f.h | 8 +-
test/CMakeLists.txt | 1 +
test/test_fcl_math.cpp | 468 ++++++++++++++++++++++++++
7 files changed, 974 insertions(+), 4 deletions(-)
create mode 100644 include/hpp/fcl/eigen/math_details.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4ddca1f9..80adcfdf 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 14cc27dd..fa302b1b 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 00000000..cd90acd7
--- /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 f67934e9..fea3affa 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 555d12b6..a959c2b4 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 5ee7213a..e3e48010 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 e35c21d0..18ee66bb 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)
--
GitLab