diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4ddca1f9b1dfc8e9852199f244c1b767eb1b2f2b..0b0b3f923389a1248342e87c9d26c60ca6903f50 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,16 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
setup_project()
+set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
+
+add_optional_dependency("eigen3 >= 3.0.0")
+set(FCL_HAVE_EIGEN ${EIGEN3_FOUND} CACHE BOOL "Use eigen matrix type when possible")
+if (EIGEN3_FOUND)
+ 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
@@ -129,7 +140,9 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/continuous_collision.h
include/hpp/fcl/math/vec_nf.h
include/hpp/fcl/math/matrix_3f.h
+ include/hpp/fcl/math/matrix_3fx.h
include/hpp/fcl/math/vec_3f.h
+ include/hpp/fcl/math/vec_3fx.h
include/hpp/fcl/math/sampling.h
include/hpp/fcl/math/math_details.h
include/hpp/fcl/math/transform.h
@@ -156,6 +169,12 @@ 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
+ include/hpp/fcl/eigen/vec_3fx.h
+ include/hpp/fcl/eigen/product.h
+ include/hpp/fcl/eigen/taylor_operator.h
+ include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
+ include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
)
add_subdirectory(src)
diff --git a/cmake b/cmake
index 7cdd2146508a3431adb8e5a0d4233704080299cd..5663a002a905b5c8d84a69d8e74044a8e34a48ed 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd
+Subproject commit 5663a002a905b5c8d84a69d8e74044a8e34a48ed
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index d81186404c36570e36e4f22314a8197d103da444..957672fdbe91d84cd6f21dac312a586811826e38 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -283,7 +283,7 @@ public:
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
- bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
+ if (left_hand) bv2.axis[0] = -R.getColumn(id[0]); else bv2.axis[0] = R.getColumn(id[0]);
bv2.axis[1] = R.getColumn(id[1]);
bv2.axis[2] = R.getColumn(id[2]);
}
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 1dcd716065498d3933cd088c874ebc4a11cfd347..e720480525930e5675d8f9ab9f8a618ae25851f4 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -71,7 +71,7 @@ public:
/// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb.
- bool overlap(const OBB& other, OBB& overlap_part) const
+ bool overlap(const OBB& other, OBB& /*overlap_part*/) const
{
return overlap(other);
}
diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h
index 5b3b9532dce74a9b26a2de130ee589162ad8a2ea..376413fea410333803d551e89fe5a6fc1b92a7f6 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -72,7 +72,7 @@ public:
}
/// @brief Check collision between two OBBRSS and return the overlap part.
- bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
+ bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
{
return overlap(other);
}
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 4e42bab3c72de5986caecdef79699b11b8142189..7dc79c3c0fc0e0cf0e2f5bbe6bba55e678a8d6ad 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -67,7 +67,7 @@ public:
bool overlap(const RSS& other) const;
/// Not implemented
- bool overlap(const RSS& other, FCL_REAL& sqrDistLowerBound) const
+ bool overlap(const RSS& /*other*/, FCL_REAL& /*sqrDistLowerBound*/) const
{
throw std::runtime_error ("Not implemented.");
return false;
@@ -75,7 +75,7 @@ public:
/// @brief Check collision between two RSS and return the overlap part.
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
- bool overlap(const RSS& other, RSS& overlap_part) const
+ bool overlap(const RSS& other, RSS& /*overlap_part*/) const
{
return overlap(other);
}
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index eb102f96ae6dd60830d4e671d0b88a4261b439ad..f38ccce92fa644e2032c55576c48d7f3f5129fa3 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -97,7 +97,7 @@ public:
bool overlap(const KDOP<N>& other) const;
/// Not implemented
- bool overlap(const KDOP<N>& other, FCL_REAL&) const
+ bool overlap(const KDOP<N>& /*other*/, FCL_REAL&) const
{
throw std::runtime_error ("Not implemented");
}
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 8df8b9f1685ee1d96a4073714511bb9b921075d7..5bda122cab7f5ecbe82652f60d105dc2729a822d 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -69,7 +69,7 @@ class kIOS
}
else /** spheres partially overlapping or disjoint */
{
- float dist = std::sqrt(dist2);
+ float dist = (float)std::sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
if(dist > 0)
@@ -99,7 +99,7 @@ public:
/// @brief Check collision between two kIOS and return the overlap part.
/// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
/// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
- bool overlap(const kIOS& other, kIOS& overlap_part) const
+ bool overlap(const kIOS& other, kIOS& /*overlap_part*/) const
{
return overlap(other);
}
diff --git a/include/hpp/fcl/BVH/BVH_internal.h b/include/hpp/fcl/BVH/BVH_internal.h
index b7fd4147b9e37686e806393fc292d4f147369644..3bed65800cb415eed4d3d9389a45405ca5dd2596 100644
--- a/include/hpp/fcl/BVH/BVH_internal.h
+++ b/include/hpp/fcl/BVH/BVH_internal.h
@@ -54,7 +54,7 @@ enum BVHBuildState
BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use
BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives
BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use
- BVH_BUILD_STATE_REPLACE_BEGUN, /// after beginReplaceModel(), state for replacing geometry primitives
+ BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for replacing geometry primitives
};
/// @brief Error code for BVH
diff --git a/include/hpp/fcl/broadphase/broadphase.h b/include/hpp/fcl/broadphase/broadphase.h
index 04311be5d15f86fe380fdb29e8ccc7890812265b..de22336e20077fa056f5f29fbaf159602b7a3213 100644
--- a/include/hpp/fcl/broadphase/broadphase.h
+++ b/include/hpp/fcl/broadphase/broadphase.h
@@ -85,13 +85,13 @@ public:
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
- virtual void update(CollisionObject* updated_obj)
+ virtual void update(CollisionObject* /*updated_obj*/)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
- virtual void update(const std::vector<CollisionObject*>& updated_objs)
+ virtual void update(const std::vector<CollisionObject*>& /*updated_objs*/)
{
update();
}
@@ -184,13 +184,13 @@ public:
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
- virtual void update(ContinuousCollisionObject* updated_obj)
+ virtual void update(ContinuousCollisionObject* /*updated_obj*/)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
- virtual void update(const std::vector<ContinuousCollisionObject*>& updated_objs)
+ virtual void update(const std::vector<ContinuousCollisionObject*>& /*updated_objs*/)
{
update();
}
diff --git a/include/hpp/fcl/ccd/interval_matrix.h b/include/hpp/fcl/ccd/interval_matrix.h
index f828387a26804588a37d5497dd02a3cac2071ea8..6ad14b0daf4683257979074ffec4cfda5f8c9327 100644
--- a/include/hpp/fcl/ccd/interval_matrix.h
+++ b/include/hpp/fcl/ccd/interval_matrix.h
@@ -99,6 +99,10 @@ struct IMatrix3
IMatrix3& rotationConstrain();
void print() const;
+
+#ifdef FCL_CCD_INTERVALMATRIX_PLUGIN
+# include FCL_CCD_INTERVALMATRIX_PLUGIN
+#endif
};
IMatrix3 rotationConstrain(const IMatrix3& m);
diff --git a/include/hpp/fcl/ccd/interval_vector.h b/include/hpp/fcl/ccd/interval_vector.h
index f5b71b640294b07ebceeea425c172aac70e4cdd1..7c1216b901887c7f1853cdde55f874fb42d27bde 100644
--- a/include/hpp/fcl/ccd/interval_vector.h
+++ b/include/hpp/fcl/ccd/interval_vector.h
@@ -156,6 +156,10 @@ struct IVector3
bool overlap(const IVector3& v) const;
bool contain(const IVector3& v) const;
+
+#ifdef FCL_CCD_INTERVALVECTOR_PLUGIN
+# include FCL_CCD_INTERVALVECTOR_PLUGIN
+#endif
};
IVector3 bound(const IVector3& i, const Vec3f& v);
diff --git a/include/hpp/fcl/ccd/motion.h b/include/hpp/fcl/ccd/motion.h
index c54d3398fdc8b7153661caa2d97720166f5f4124..7f2af4718d7bb8145a58f1d24dd52c3363a5594a 100644
--- a/include/hpp/fcl/ccd/motion.h
+++ b/include/hpp/fcl/ccd/motion.h
@@ -88,7 +88,7 @@ public:
tf_ = tf;
}
- void getTaylorModel(TMatrix3& tm, TVector3& tv) const
+ void getTaylorModel(TMatrix3& /*tm*/, TVector3& /*tv*/) const
{
}
@@ -113,14 +113,14 @@ public:
const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
// @brief Construct motion from initial and goal transform
- SplineMotion(const Matrix3f& R1, const Vec3f& T1,
- const Matrix3f& R2, const Vec3f& T2) : MotionBase()
+ SplineMotion(const Matrix3f& /*R1*/, const Vec3f& /*T1*/,
+ const Matrix3f& /*R2*/, const Vec3f& /*T2*/) : MotionBase()
{
// TODO
}
- SplineMotion(const Transform3f& tf1,
- const Transform3f& tf2) : MotionBase()
+ SplineMotion(const Transform3f& /*tf1*/,
+ const Transform3f& /*tf2*/) : MotionBase()
{
// TODO
}
diff --git a/include/hpp/fcl/ccd/motion_base.h b/include/hpp/fcl/ccd/motion_base.h
index 500888e6af1eb422f6e9d05dc551e89b62fc64a7..8f2656e77f70e3aafc5f70daa8b5806f3776f9be 100644
--- a/include/hpp/fcl/ccd/motion_base.h
+++ b/include/hpp/fcl/ccd/motion_base.h
@@ -72,11 +72,11 @@ class TBVMotionBoundVisitor : public BVMotionBoundVisitor
public:
TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
- virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
- virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
- virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
- virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
- virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
+ virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
+ virtual FCL_REAL visit(const SplineMotion& /*motion*/) const { return 0; }
+ virtual FCL_REAL visit(const ScrewMotion& /*motion*/) const { return 0; }
+ virtual FCL_REAL visit(const InterpMotion& /*motion*/) const { return 0; }
+ virtual FCL_REAL visit(const TranslationMotion& /*motion*/) const { return 0; }
protected:
BV bv;
@@ -102,7 +102,7 @@ public:
TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
a(a_), b(b_), c(c_), n(n_) {}
- virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
+ virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& motion) const;
virtual FCL_REAL visit(const ScrewMotion& motion) const;
virtual FCL_REAL visit(const InterpMotion& motion) const;
diff --git a/include/hpp/fcl/ccd/taylor_vector.h b/include/hpp/fcl/ccd/taylor_vector.h
index a17c262ab06104746a6905e60a282ea142d41003..be570a38c55501fc975dd1565ac10516ac352333 100644
--- a/include/hpp/fcl/ccd/taylor_vector.h
+++ b/include/hpp/fcl/ccd/taylor_vector.h
@@ -41,6 +41,10 @@
#include <hpp/fcl/ccd/interval_vector.h>
#include <hpp/fcl/ccd/taylor_model.h>
+#if FCL_HAVE_EIGEN
+#include <hpp/fcl/eigen/taylor_operator.h>
+#endif
+
namespace fcl
{
diff --git a/include/hpp/fcl/config-fcl.hh.in b/include/hpp/fcl/config-fcl.hh.in
index 14cc27ddfd5960d453ca509ef6444dc2e60e292d..e8a2448f228a288f0ec446edacfa5bb81fe23a6c 100644
--- a/include/hpp/fcl/config-fcl.hh.in
+++ b/include/hpp/fcl/config-fcl.hh.in
@@ -37,6 +37,8 @@
# include "config.h"
+#cmakedefine01 FCL_HAVE_EIGEN
+
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#cmakedefine01 FCL_HAVE_FLANN
diff --git a/include/hpp/fcl/eigen/math_details.h b/include/hpp/fcl/eigen/math_details.h
new file mode 100644
index 0000000000000000000000000000000000000000..c1ed7fd54feb04ab08360dd7423b1a4d7ea12fb7
--- /dev/null
+++ b/include/hpp/fcl/eigen/math_details.h
@@ -0,0 +1,782 @@
+/*
+ * 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.
+ */
+
+/** \author Joseph Mirabel */
+
+#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_v3 :
+ Eigen::Matrix <T, 3, 1>
+{
+ typedef T meta_type;
+ typedef Eigen::Matrix <T, 3, 1> Base;
+
+ eigen_v3(void): Base () { this->setZero (); }
+
+ // This constructor allows you to construct MyVectorType from Eigen expressions
+ template<typename OtherDerived>
+ eigen_v3(const Eigen::MatrixBase<OtherDerived>& other)
+ : Base(other)
+ {}
+
+ // This method allows you to assign Eigen expressions to MyVectorType
+ template<typename OtherDerived>
+ eigen_v3& operator= (const Eigen::MatrixBase <OtherDerived>& other)
+ {
+ this->Base::operator=(other);
+ return *this;
+ }
+
+ eigen_v3(T x) :
+ Base (x, x, x)
+ {}
+
+ eigen_v3(T* x) :
+ Base (*x, *x, *x)
+ {}
+
+ eigen_v3(T x, T y, T z) :
+ Base (x, y, z)
+ {}
+
+ inline void setValue(T x, T y, T z)
+ {
+ this->operator[] (0) = x;
+ this->operator[] (1) = y;
+ this->operator[] (2) = z;
+ }
+
+ inline void setValue(T x)
+ {
+ this->setConstant (x);
+ }
+
+ inline void negate()
+ {
+ this->operator*= (-1);
+ }
+
+ template<typename OtherDerived>
+ inline eigen_v3<T>& ubound(const Eigen::MatrixBase<OtherDerived>& u)
+ {
+ *this = this->cwiseMin (u);
+ return *this;
+ }
+
+ template<typename OtherDerived>
+ inline eigen_v3<T>& lbound(const Eigen::MatrixBase<OtherDerived>& l)
+ {
+ *this = this->cwiseMax (l);
+ 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[0] - other.v[0], v[1] - other.v[1], v[2] - other.v[2]); }
+ // inline eigen_wrapper_v3<T> operator * (const eigen_wrapper_v3<T>& other) const { return eigen_wrapper_v3<T>(v[0] * other.v[0], v[1] * other.v[1], v[2] * other.v[2]); }
+ // inline eigen_v3<T> operator / (const eigen_v3<T>& other) const { return eigen_v3<T>(this->array().cwiseQuotient (other)); }
+ inline eigen_v3<T>& operator += (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->Base::operator+= (other)); }
+ inline eigen_v3<T>& operator -= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->Base::operator-= (other)); }
+ inline eigen_v3<T>& operator *= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->array().operator*=(other)); }
+ inline eigen_v3<T>& operator /= (const eigen_v3<T>& other) { return static_cast<eigen_v3<T>&>(this->array().operator/=(other)); }
+ // inline eigen_wrapper_v4<T>& operator /= (const eigen_wrapper_v4<T>& other) { = d().cwiseQuotient (other.d()); return *this; }
+ // inline eigen_wrapper_v3<T>& operator *= (const eigen_wrapper_v3<T>& other) { return this->Base::operator*= (other); }
+ // inline eigen_wrapper_v3<T>& operator /= (const eigen_wrapper_v3<T>& other) { return this->Base::operator/= (other); }
+ // 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) 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_v3<T>& operator += (T t) { this->array() += t; return *this; }
+ inline eigen_v3<T>& operator -= (T t) { this->array() -= t; return *this; }
+ inline eigen_v3<T>& operator *= (T t) { this->array() *= t; return *this; }
+ inline eigen_v3<T>& operator /= (T t) { this->array() /= t; return *this; }
+ // inline eigen_wrapper_v3<T> operator - () const { return eigen_wrapper_v3<T>(-v); }
+};
+
+
+template <typename T>
+static inline eigen_v3<T> cross_prod(const eigen_v3<T>& l, const eigen_v3<T>& r)
+{
+ return l.cross (r);
+}
+
+template <typename T>
+static inline T dot_prod3(const eigen_v3<T>& l, const eigen_v3<T>& r)
+{
+ return l.dot(r);
+}
+
+template <typename T>
+static inline eigen_v3<T> min(const eigen_v3<T>& x, const eigen_v3<T>& y)
+{
+ return x.cwiseMin (y);
+}
+
+template <typename T>
+static inline eigen_v3<T> max(const eigen_v3<T>& x, const eigen_v3<T>& y)
+{
+ return x.cwiseMax(y);
+}
+
+template <typename T>
+static inline eigen_v3<T> abs(const eigen_v3<T>& x)
+{
+ return x.cwiseAbs();
+}
+
+template <typename T>
+static inline bool equal(const eigen_v3<T>& x, const eigen_v3<T>& y, T epsilon)
+{
+ return ((x - y).cwiseAbs ().array () < epsilon).all();
+}
+
+template<typename T>
+struct eigen_wrapper_m3
+{
+ typedef T meta_type;
+ typedef eigen_wrapper_v3<T> vector_type;
+ typedef Eigen::Matrix <T, 3, 3, Eigen::RowMajor> matrix_type;
+ typedef Eigen::Matrix <T, 3, 1> inner_col_type;
+ 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 + (T c) const { return eigen_wrapper_m3<T>(m + c); }
+ inline eigen_wrapper_m3<T> operator - (T c) const { return eigen_wrapper_m3<T>(m - c); }
+ inline eigen_wrapper_m3<T> operator * (T c) const { return eigen_wrapper_m3<T>(m * c); }
+ inline eigen_wrapper_m3<T> operator / (T c) const { return eigen_wrapper_m3<T>(m / c); }
+
+ inline eigen_wrapper_m3<T>& operator *= (const eigen_wrapper_m3<T>& other) { m *= other.m; return *this; }
+ inline eigen_wrapper_m3<T>& operator += (const eigen_wrapper_m3<T>& other) { m += other.m; return *this; }
+ inline eigen_wrapper_m3<T>& operator -= (const eigen_wrapper_m3<T>& other) { m -= other.m; return *this; }
+
+ inline eigen_wrapper_m3<T>& operator += (T c) { m.array() += c; return *this; }
+ inline eigen_wrapper_m3<T>& operator -= (T c) { m.array() -= c; return *this; }
+ inline eigen_wrapper_m3<T>& operator *= (T c) { m.array() *= c; return *this; }
+ inline eigen_wrapper_m3<T>& operator /= (T c) { m.array() /= c; return *this; }
+
+
+ void setIdentity() { m.setIdentity (); }
+
+ 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 ());
+}
+
+template<typename T>
+struct eigen_m3 :
+ Eigen::Matrix <T, 3, 3>
+{
+ typedef T meta_type;
+ typedef eigen_v3<T> vector_type;
+ typedef Eigen::Matrix <T, 3, 3> Base;
+
+ typedef typename Base::ColXpr ColXpr;
+ typedef typename Base::ConstColXpr ConstColXpr;
+ typedef typename Base::RowXpr RowXpr;
+ typedef typename Base::ConstRowXpr ConstRowXpr;
+
+ eigen_m3(void): Base () { }
+
+ // This constructor allows you to construct MyVectorType from Eigen expressions
+ template<typename OtherDerived>
+ eigen_m3(const Eigen::MatrixBase<OtherDerived>& other)
+ : Base(other)
+ {}
+
+ // This method allows you to assign Eigen expressions to MyVectorType
+ template<typename OtherDerived>
+ eigen_m3& operator= (const Eigen::MatrixBase <OtherDerived>& other)
+ {
+ this->Base::operator=(other);
+ return *this;
+ }
+
+ eigen_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_m3(const vector_type& v1, const vector_type& v2, const vector_type& v3)
+ {
+ this->row(1) = v1;
+ this->row(2) = v2;
+ this->row(3) = v3;
+ }
+
+ inline ColXpr getColumn(size_t i) { return this->col (i); }
+ inline RowXpr getRow(size_t i) { return this->row (i); }
+ inline ConstColXpr getColumn(size_t i) const { return this->col (i); }
+ inline ConstRowXpr getRow(size_t i) const { return this->row (i); }
+
+ inline eigen_m3<T>& operator *= (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator*=(other)); }
+ inline eigen_m3<T>& operator += (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator+=(other)); }
+ inline eigen_m3<T>& operator -= (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator-=(other)); }
+
+ inline eigen_m3<T>& operator += (T c) { return static_cast<eigen_m3<T>&>(this->operator+=(c)); }
+ inline eigen_m3<T>& operator -= (T c) { return static_cast<eigen_m3<T>&>(this->operator-=(c)); }
+ inline eigen_m3<T>& operator *= (T c) { return static_cast<eigen_m3<T>&>(this->operator*=(c)); }
+ inline eigen_m3<T>& operator /= (T c) { return static_cast<eigen_m3<T>&>(this->operator/=(c)); }
+
+ static const eigen_m3<T>& getIdentity()
+ {
+ static const eigen_m3<T> I(Base::Identity ());
+ return I;
+ }
+
+ eigen_m3<T>& transpose() { this->transposeInPlace (); return *this; }
+
+ eigen_m3<T>& inverse()
+ {
+ this->Base::operator=(this->inverse ().eval());
+ // m = m.inverse().eval();
+ return *this;
+ }
+
+ template <typename OtherDerived>
+ const typename Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+ transposeTimes(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->Base::transpose() * other;
+ }
+
+ template <typename OtherDerived>
+ const typename Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+ timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->operator* (other.Base::transpose());
+ }
+
+ template <typename OtherDerived>
+ inline T transposeDotX(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->col(0).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T transposeDotY(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->col(1).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T transposeDotZ(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->col(2).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T transposeDot(size_t i, const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->col(i).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T dotX(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->row(0).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T dotY(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->row(1).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T dotZ(const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->row(2).dot(other);
+ }
+
+ template <typename OtherDerived>
+ inline T dot(size_t i, const Eigen::MatrixBase<OtherDerived>& other) const
+ {
+ return this->row(i).dot(other);
+ }
+
+ inline void setValue(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
+ {
+ *this << xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz;
+ }
+
+ inline void setValue(T x)
+ {
+ this->setConstant (x);
+ }
+};
+
+
+template<typename T>
+eigen_m3<T> abs(const eigen_m3<T>& m)
+{
+ return eigen_m3<T>(m.cwiseAbs ());
+}
+
+template<typename T>
+eigen_m3<T> transpose(const eigen_m3<T>& m)
+{
+ return eigen_m3<T>(m.eigen_m3<T>::Base::transpose ());
+}
+
+
+template<typename T>
+eigen_m3<T> inverse(const eigen_m3<T>& m)
+{
+ return eigen_m3<T> (m.eigen_m3<T>::Base::inverse().eval ());
+}
+
+}
+
+}
+
+#endif
diff --git a/include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h b/include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
new file mode 100644
index 0000000000000000000000000000000000000000..fda38f99323f5b9a05a7f7e4d8aa0a0a173a787f
--- /dev/null
+++ b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
@@ -0,0 +1,7 @@
+template <typename Derived>
+IVector3& operator=(const FclType<Derived>& other)
+{
+ const Vec3f& tmp = other.fcl();
+ setValue (tmp);
+ return *this;
+}
diff --git a/include/hpp/fcl/eigen/product.h b/include/hpp/fcl/eigen/product.h
new file mode 100644
index 0000000000000000000000000000000000000000..3998cf39b4a5fc6931508e7a6b50e7dc72830331
--- /dev/null
+++ b/include/hpp/fcl/eigen/product.h
@@ -0,0 +1,39 @@
+namespace internal {
+
+template<typename Derived, typename OtherDerived, bool coefwise> struct deduce_fcl_type {};
+
+template<typename Derived, typename OtherDerived>
+struct deduce_fcl_type<Derived, OtherDerived, false> {
+ typedef typename ProductReturnType<Derived, OtherDerived>::Type Type;
+};
+template<typename Derived, typename OtherDerived>
+struct deduce_fcl_type<Derived, OtherDerived, true> {
+ typedef CwiseBinaryOp<scalar_multiple_op<typename Derived::Scalar>, const Derived, const OtherDerived> Type;
+};
+
+}
+
+template<typename Derived, typename OtherDerived>
+struct FclProduct
+{
+ enum {
+ COEFWISE = Derived::ColsAtCompileTime == 1 && OtherDerived::ColsAtCompileTime == 1
+ };
+
+ typedef typename internal::remove_fcl<Derived>::type EDerived;
+ typedef typename internal::remove_fcl<OtherDerived>::type EOtherDerived;
+
+ typedef FclOp<typename internal::deduce_fcl_type<EDerived, EOtherDerived, COEFWISE>::Type> ProductType;
+ typedef FclOp<typename ProductReturnType<Transpose<Derived>, EOtherDerived >::Type> TransposeTimesType;
+ typedef FclOp<typename ProductReturnType<EDerived, Transpose<EOtherDerived> >::Type> TimesTransposeType;
+ typedef FclOp<typename ProductReturnType<ProductType, Transpose<EDerived> >::Type> TensorTransformType;
+ static EIGEN_STRONG_INLINE ProductType run (const Derived& l, const OtherDerived& r) { return ProductType (l, r); }
+};
+
+#define FCL_EIGEN_MAKE_PRODUCT_OPERATOR() \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE const typename FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::ProductType \
+ operator*(const MatrixBase<OtherDerived>& other) const \
+ { \
+ return FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::run (*this, other.derived()); \
+ }
diff --git a/include/hpp/fcl/eigen/taylor_operator.h b/include/hpp/fcl/eigen/taylor_operator.h
new file mode 100644
index 0000000000000000000000000000000000000000..fa02f2b7aea2b7454b255e0c0cb93e35cf673a2c
--- /dev/null
+++ b/include/hpp/fcl/eigen/taylor_operator.h
@@ -0,0 +1,25 @@
+#ifndef FCL_CCD_TAYLOR_OPERATOR_H
+#define FCL_CCD_TAYLOR_OPERATOR_H
+
+#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
+
+namespace fcl {
+
+class TVector3;
+class TMatrix3;
+
+template<int Col> struct TaylorReturnType {};
+template<> struct TaylorReturnType<1> { typedef TVector3 type; typedef Vec3f eigen_type; };
+template<> struct TaylorReturnType<3> { typedef TMatrix3 type; typedef Matrix3f eigen_type; };
+
+template<typename Derived>
+typename TaylorReturnType<Derived::ColsAtCompileTime>::type operator * (const FclType<Derived>& v, const TaylorModel& a)
+{
+ const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.fcl();
+ return b * a;
+}
+
+}
+
+#endif // FCL_CCD_TAYLOR_OPERATOR_H
diff --git a/include/hpp/fcl/eigen/vec_3fx.h b/include/hpp/fcl/eigen/vec_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..23a1e3fd1a2a1e85230577b734eb3a7f68773d3d
--- /dev/null
+++ b/include/hpp/fcl/eigen/vec_3fx.h
@@ -0,0 +1,813 @@
+/*
+ * 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.
+ */
+
+/** \author Joseph Mirabel */
+
+#ifndef FCL_EIGEN_VEC_3F_H
+#define FCL_EIGEN_VEC_3F_H
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/data_types.h>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+#define FCL_CCD_INTERVALVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-vector.h>
+#define FCL_CCD_MATRIXVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-matrix.h>
+
+#define FCL_EIGEN_EXPOSE_PARENT_TYPE(Type) typedef typename Base::Type Type;
+
+#define FCL_EIGEN_MAKE_CWISE_BINARY_OP(METHOD,FUNCTOR) \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE const FclOp<const CwiseBinaryOp<FUNCTOR<Scalar>, const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived> > \
+ (METHOD) (const MatrixBase<OtherDerived>& other) const \
+ { \
+ return FclOp <const CwiseBinaryOp<FUNCTOR<Scalar>, const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived> > (*this, other.derived()); \
+ }
+
+#define FCL_EIGEN_MAKE_CWISE_UNARY_OP(METHOD,FUNCTOR) \
+ EIGEN_STRONG_INLINE const FclOp<const CwiseUnaryOp<FUNCTOR<Scalar>, const typename FCL_EIGEN_CURRENT_CLASS::Base> > \
+ (METHOD) (const Scalar& scalar) const \
+ { \
+ return FclOp <const CwiseUnaryOp<FUNCTOR<Scalar>, const typename FCL_EIGEN_CURRENT_CLASS::Base> > (*this, FUNCTOR<Scalar>(scalar)); \
+ }
+
+#define FCL_EIGEN_RENAME_PARENT_METHOD(OLD,NEW,RETTYPE) \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE RETTYPE (METHOD) () \
+ { \
+ return (this->Base::METHOD) (); \
+ }
+
+#define FCL_EIGEN_MAKE_EXPOSE_PARENT1(METHOD) \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE FclMatrix& (METHOD) (const MatrixBase<OtherDerived>& other) \
+ { \
+ (this->Base::METHOD)(other); return *this; \
+ }
+
+#define FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY1(METHOD) \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE FclMatrix& (METHOD) (const MatrixBase<OtherDerived>& other) \
+ { \
+ (this->array().METHOD)(other.derived().array()); return *this; \
+ }
+
+#define FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(METHOD) \
+ EIGEN_STRONG_INLINE FCL_EIGEN_CURRENT_CLASS& (METHOD) (const Scalar& other) \
+ { \
+ (this->array().METHOD)(other); return *this; \
+ }
+
+#define FCL_EIGEN_MAKE_GET_COL_ROW() \
+ EIGEN_STRONG_INLINE FclOp<ColXpr> getColumn (size_t i) { return FclOp<ColXpr>(*this, i); } \
+ EIGEN_STRONG_INLINE FclOp<RowXpr> getRow (size_t i) { return FclOp<RowXpr>(*this, i); } \
+ EIGEN_STRONG_INLINE FclOp<ConstColXpr> getColumn (size_t i) const { return FclOp<ConstColXpr>(*this, i); } \
+ EIGEN_STRONG_INLINE FclOp<ConstRowXpr> getRow (size_t i) const { return FclOp<ConstRowXpr>(*this, i); }
+
+#define FCL_EIGEN_MATRIX_DOT_AXIS(NAME,axis,index) \
+ template<typename OtherDerived> \
+ EIGEN_STRONG_INLINE Scalar NAME##axis (const MatrixBase<OtherDerived>& other) const \
+ { return this->NAME (index, other); }
+
+#define FCL_EIGEN_MATRIX_DOT(NAME,FUNC) \
+ template<typename OtherDerived> \
+ EIGEN_STRONG_INLINE Scalar NAME(size_t i, const MatrixBase<OtherDerived>& other) const \
+ { \
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(FCL_EIGEN_CURRENT_CLASS, 3, 3); \
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(OtherDerived, 3); \
+ return this->FUNC(i).dot(other); \
+ } \
+ FCL_EIGEN_MATRIX_DOT_AXIS(NAME,X,0)\
+ FCL_EIGEN_MATRIX_DOT_AXIS(NAME,Y,1)\
+ FCL_EIGEN_MATRIX_DOT_AXIS(NAME,Z,2)
+
+#define FCL_EIGEN_MAKE_CROSS() \
+ template<typename OtherDerived> \
+ EIGEN_STRONG_INLINE typename BinaryReturnType<FCL_EIGEN_CURRENT_CLASS::Base,OtherDerived>::Cross \
+ cross (const MatrixBase<OtherDerived>& other) const { return this->Base::cross (other); }
+
+#define FCL_EIGEN_MAKE_DOT() \
+ template <typename OtherDerived> \
+ EIGEN_STRONG_INLINE Scalar dot (const MatrixBase<OtherDerived>& other) const \
+ { return this->Base::dot (other.derived()); }
+
+namespace fcl {
+template <typename Derived>
+class FclType
+{
+ public:
+ Derived& fcl () { return static_cast<Derived&> (*this); }
+ const Derived& fcl () const { return static_cast<const Derived&> (*this); }
+};
+}
+
+namespace Eigen {
+ template <typename T> class FclOp;
+ template <typename T, int Cols, int Options> class FclMatrix;
+
+ namespace internal {
+
+ template<typename T> struct traits< FclOp<T> > : traits <T> {};
+ template<typename T, int Cols, int _Options>
+ struct traits< FclMatrix<T, Cols, _Options> >
+ : traits<typename FclMatrix<T, Cols, _Options>::Base>
+ {};
+
+ template <typename Derived> struct remove_fcl { typedef Derived type; };
+ template <> template <typename Derived> struct remove_fcl <FclOp<Derived> > { typedef Derived type; };
+ template <> template <typename Derived> struct remove_fcl <const FclOp<Derived> > { typedef Derived type; };
+ template <> template <typename T, int Col, int Options> struct remove_fcl <FclMatrix<T,Col,Options> > { typedef typename FclMatrix<T,Col,Options>::Base type; };
+ template <> template <typename T, int Col, int Options> struct remove_fcl <const FclMatrix<T,Col,Options> > { typedef typename FclMatrix<T,Col,Options>::Base type; };
+ }
+
+#include <hpp/fcl/eigen/product.h>
+
+ template <typename Derived>
+ struct UnaryReturnType {
+ typedef typename Derived::Scalar Scalar;
+ typedef typename Derived::PlainObject Normalize;
+ typedef FclOp <
+ const CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const Derived>
+ > Opposite;
+ typedef FclOp <
+ const CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived>
+ > Abs;
+ };
+
+ template <typename Derived, typename OtherDerived>
+ struct BinaryReturnType {
+ typedef typename Derived::Scalar Scalar;
+ typedef FclOp <
+ const CwiseBinaryOp<internal::scalar_difference_op<Scalar>,
+ const Derived, const OtherDerived>
+ > Difference;
+ // static inline const Difference difference (const Derived& l, const OtherDerived& r)
+ // { return Difference (l, r, internal::scalar_difference_op<Scalar>()); }
+ typedef FclOp <
+ const CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
+ const Derived, const OtherDerived>
+ > Sum;
+ typedef FclOp <
+ const CwiseBinaryOp<internal::scalar_min_op<Scalar>,
+ const Derived, const OtherDerived>
+ > Min;
+ typedef FclOp <
+ const CwiseBinaryOp<internal::scalar_max_op<Scalar>,
+ const Derived, const OtherDerived>
+ > Max;
+ typedef FclMatrix <Scalar, 1, 0> Cross;
+ };
+
+#define FCL_EIGEN_CURRENT_CLASS FclMatrix
+
+/// @brief Vector3 class wrapper. The core data is in the template parameter class.
+template <typename T, int Cols, int _Options = internal::traits<Matrix <T, 3, Cols> >::Options >
+class FclMatrix : public Matrix <T, 3, Cols, _Options>, public ::fcl::FclType<FclMatrix <T, Cols, _Options> >
+{
+public:
+ typedef Matrix <T, 3, Cols, _Options> Base;
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(Scalar)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ColXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(RowXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstColXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstRowXpr)
+
+ // Compatibility with other Matrix3fX and Vec3f
+ typedef T U;
+ typedef T meta_type;
+
+ FclMatrix(void): Base(Base::Zero()) {}
+
+ // This constructor allows you to construct MyVectorType from Eigen expressions
+ template<typename OtherDerived>
+ FclMatrix(const MatrixBase<OtherDerived>& other)
+ : Base(other)
+ {}
+
+ // This method allows you to assign Eigen expressions to MyVectorType
+ template<typename OtherDerived>
+ FclMatrix& operator=(const MatrixBase <OtherDerived>& other)
+ {
+ this->Base::operator=(other);
+ return *this;
+ }
+
+ /// @brief create Vector (x, y, z)
+ FclMatrix(T x, T y, T z) : Base(x, y, z) {}
+
+ FclMatrix(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz) : Base()
+ {
+ setValue(xx, xy, xz, yx, yy, yz, zx, zy, zz);
+ }
+
+ template <typename Vector>
+ FclMatrix(const ::fcl::FclType<Vector>& r0,
+ const ::fcl::FclType<Vector>& r1,
+ const ::fcl::FclType<Vector>& r2) : Base()
+ {
+ this->row(0) = r0.fcl();
+ this->row(1) = r1.fcl();
+ this->row(2) = r2.fcl();
+ }
+
+ /// @brief create vector (x, x, x)
+ FclMatrix(T x) : Base(Base::Constant (x)) {}
+
+ /// @brief create vector using the internal data type
+ // Vec3fX(const T& data_) : data(data_) {}
+
+ // inline U operator [] (size_t i) const { return data[i]; }
+ // inline U& operator [] (size_t i) { return data[i]; }
+
+ // FclOp<typename Base::ColXpr> getColumn (size_t i) { return FclOp<typename Base::ColXpr>(*this, i); }
+ // FclOp<typename Base::RowXpr> getRow (size_t i) { return FclOp<typename Base::RowXpr>(*this, i); }
+ // FclOp<typename Base::ColXpr> getColumn (size_t i) { return FclOp<typename Base::ColXpr>(*this, i); }
+ // FclOp<typename Base::RowXpr> getRow (size_t i) { return FclOp<typename Base::RowXpr>(*this, i); }
+ FCL_EIGEN_MAKE_GET_COL_ROW()
+ FCL_EIGEN_MATRIX_DOT(dot,row)
+ FCL_EIGEN_MATRIX_DOT(transposeDot,col)
+
+ FCL_EIGEN_MAKE_DOT()
+
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator+,internal::scalar_sum_op)
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator-,internal::scalar_difference_op)
+ // Map this to scalar product or matrix product depending on the Cols.
+ // FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator*,internal::scalar_product_op)
+ FCL_EIGEN_MAKE_PRODUCT_OPERATOR()
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator/,internal::scalar_quotient_op)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT1(operator+=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT1(operator-=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY1(operator*=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY1(operator/=)
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator+,internal::scalar_add_op)
+ // This operator cannot be implement with the macro
+ // FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator-,internal::scalar_difference_op)
+ EIGEN_STRONG_INLINE const FclOp<const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const Base> >
+ operator- (const Scalar& scalar) const
+ {
+ return FclOp <const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const Base> > (*this, internal::scalar_add_op<Scalar>(-scalar));
+ }
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator*,internal::scalar_multiple_op)
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator/,internal::scalar_quotient1_op)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator+=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator-=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator*=)
+ FCL_EIGEN_MAKE_EXPOSE_PARENT_ARRAY_SCALAR1(operator/=)
+ inline const typename UnaryReturnType<Base>::Opposite operator-() const { return typename UnaryReturnType<Base>::Opposite(*this); }
+ // There is no class for cross
+ // inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
+ FCL_EIGEN_MAKE_CROSS()
+ inline FclMatrix& normalize()
+ {
+ T sqr_length = this->squaredNorm();
+ if(sqr_length > 0) this->operator/= ((T)std::sqrt(sqr_length));
+ return *this;
+ }
+
+ inline FclMatrix& normalize(bool* signal)
+ {
+ T sqr_length = this->squaredNorm();
+ if(sqr_length > 0)
+ {
+ this->operator/= ((T)std::sqrt(sqr_length));
+ *signal = true;
+ }
+ else
+ *signal = false;
+ return *this;
+ }
+
+ inline FclMatrix& abs()
+ {
+ *this = this->cwiseAbs ();
+ return *this;
+ }
+
+ inline T length() const { return this->norm(); }
+ // inline T norm() const { return sqrt(details::dot_prod3(data, data)); }
+ inline T sqrLength() const { return this->squaredNorm(); }
+ // inline T squaredNorm() const { return details::dot_prod3(data, data); }
+ inline void setValue(T x, T y, T z) {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(FclMatrix, 3);
+ this->m_storage.data()[0] = x;
+ this->m_storage.data()[1] = y;
+ this->m_storage.data()[2] = z;
+ }
+ inline void setValue(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
+ {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(FclMatrix, 3, 3);
+ this->operator()(0,0) = xx; this->operator()(0,1) = xy; this->operator()(0,2) = xz;
+ this->operator()(1,0) = yx; this->operator()(1,1) = yy; this->operator()(1,2) = yz;
+ this->operator()(2,0) = zx; this->operator()(2,1) = zy; this->operator()(2,2) = zz;
+ }
+ inline void setValue(T x) { this->setConstant (x); }
+ // inline void setZero () {data.setValue (0); }
+ inline bool equal(const FclMatrix& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
+ {
+ return ((*this - other).cwiseAbs().array () < epsilon).all();
+ }
+ inline FclMatrix& negate() { *this = -*this; return *this; }
+
+ bool operator == (const FclMatrix& other) const
+ {
+ return equal(other, 0);
+ }
+
+ bool operator != (const FclMatrix& other) const
+ {
+ return !(*this == other);
+ }
+
+ inline FclMatrix& ubound(const FclMatrix& u)
+ {
+ *this = this->cwiseMin (u);
+ return *this;
+ }
+
+ inline FclMatrix& lbound(const FclMatrix& l)
+ {
+ *this = this->cwiseMax (l);
+ return *this;
+ }
+
+ bool isZero() const
+ {
+ return (this->m_storage.data()[0] == 0)
+ && (this->m_storage.data()[1] == 0)
+ && (this->m_storage.data()[2] == 0);
+ }
+
+ FclMatrix& transpose () {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Base, 3, 3);
+ this->transposeInPlace();
+ return *this;
+ }
+
+ FclMatrix& inverse()
+ {
+ this->Base::operator=(this->Base::inverse ().eval());
+ return *this;
+ }
+
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename FclProduct<const Base,const OtherDerived>::TransposeTimesType
+ transposeTimes (const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const Base> t (*this);
+ return typename FclProduct<const Base,const OtherDerived>::TransposeTimesType (t, other.derived());
+ }
+
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename FclProduct<const Base,const OtherDerived>::TimesTransposeType
+ timesTranspose (const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const OtherDerived> t (other.derived());
+ return typename FclProduct<const Base,const OtherDerived>::TimesTransposeType (*this, t);
+ }
+
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename FclProduct<const Base,const OtherDerived>::TensorTransformType
+ tensorTransform(const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const Base> t (*this);
+ const typename FclProduct<const Base,const OtherDerived>::ProductType left (*this, other.derived());
+ return typename FclProduct<const Base,const OtherDerived>::TensorTransformType (left, t);
+ }
+
+ static const FclMatrix& getIdentity()
+ {
+ static const FclMatrix I((T)1, (T)0, (T)0,
+ (T)0, (T)1, (T)0,
+ (T)0, (T)0, (T)1);
+ return I;
+ }
+
+ /// @brief Set the matrix from euler angles YPR around ZYX axes
+ /// @param eulerX Roll about X axis
+ /// @param eulerY Pitch around Y axis
+ /// @param eulerZ Yaw aboud Z axis
+ ///
+ /// These angles are used to produce a rotation matrix. The euler
+ /// angles are applied in ZYX order. I.e a vector is first rotated
+ /// about X then Y and then Z
+ inline void setEulerZYX(Scalar eulerX, Scalar eulerY, Scalar eulerZ)
+ {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(FclMatrix, 3, 3);
+ Scalar ci(std::cos(eulerX));
+ Scalar cj(std::cos(eulerY));
+ Scalar ch(std::cos(eulerZ));
+ Scalar si(std::sin(eulerX));
+ Scalar sj(std::sin(eulerY));
+ Scalar sh(std::sin(eulerZ));
+ Scalar cc = ci * ch;
+ Scalar cs = ci * sh;
+ Scalar sc = si * ch;
+ Scalar ss = si * sh;
+
+ setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+ cj * sh, sj * ss + cc, sj * cs - sc,
+ -sj, cj * si, cj * ci);
+
+ }
+
+ /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+ /// @param yaw Yaw about Y axis
+ /// @param pitch Pitch about X axis
+ /// @param roll Roll about Z axis
+ void setEulerYPR(Scalar yaw, Scalar pitch, Scalar roll)
+ {
+ setEulerZYX(roll, pitch, yaw);
+ }
+};
+
+#undef FCL_EIGEN_CURRENT_CLASS
+#define FCL_EIGEN_CURRENT_CLASS FclOp
+
+template <typename EigenOp>
+class FclOp : public EigenOp, public ::fcl::FclType<FclOp <EigenOp> >
+{
+public:
+ typedef typename internal::traits<EigenOp>::Scalar T;
+ typedef EigenOp Base;
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(Scalar)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ColXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(RowXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstColXpr)
+ FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstRowXpr)
+
+ // template<typename Lhs, typename Rhs, typename BinaryOp>
+ // FclOp (const Lhs& lhs, const Rhs& rhs, const BinaryOp& bop)
+ // : Base (lhs, rhs, bop) {}
+
+ template<typename Lhs, typename Rhs>
+ FclOp (Lhs& lhs, const Rhs& rhs)
+ : Base (lhs, rhs) {}
+
+ template<typename OtherEigenOp>
+ FclOp (const FclOp<OtherEigenOp>& other)
+ : Base (other) {}
+
+ template<typename XprType>
+ FclOp (XprType& xpr)
+ : Base (xpr) {}
+
+ FCL_EIGEN_MAKE_GET_COL_ROW()
+ FCL_EIGEN_MATRIX_DOT(dot,row)
+ FCL_EIGEN_MATRIX_DOT(transposeDot,col)
+
+ FCL_EIGEN_MAKE_DOT()
+
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator+,internal::scalar_sum_op)
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator-,internal::scalar_difference_op)
+ // Map this to scalar product or matrix product depending on the Cols.
+ // FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator*,internal::scalar_product_op)
+ FCL_EIGEN_MAKE_PRODUCT_OPERATOR()
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(operator/,internal::scalar_quotient_op)
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator+,internal::scalar_add_op)
+ // This operator cannot be implement with the macro
+ // FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator-,internal::scalar_difference_op)
+ EIGEN_STRONG_INLINE const FclOp<const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const Base> >
+ operator- (const Scalar& scalar) const
+ {
+ return FclOp <const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const Base> > (*this, internal::scalar_add_op<Scalar>(-scalar));
+ }
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator*,internal::scalar_multiple_op)
+ FCL_EIGEN_MAKE_CWISE_UNARY_OP(operator/,internal::scalar_quotient1_op)
+ inline const typename UnaryReturnType<const Base>::Opposite operator-() const { return typename UnaryReturnType<const Base>::Opposite(*this); }
+
+ FCL_EIGEN_MAKE_CROSS()
+
+ inline const typename UnaryReturnType<EigenOp>::Normalize
+ normalize() const
+ {
+ return this->normalized ();
+ // T sqr_length = this->squaredNorm();
+ // if(sqr_length > 0) CwiseExpose (*this /= ((T)sqrt(sqr_length)));
+ // return *this;
+ }
+
+ /*
+ inline Vec3fX<T> normalize(bool* signal)
+ {
+ T sqr_length = this->squaredNorm();
+ if(sqr_length > 0)
+ {
+ *this /= ((T)sqrt(sqr_length));
+ *signal = true;
+ }
+ else
+ *signal = false;
+ return *this;
+ }
+ // */
+
+ inline const typename UnaryReturnType<EigenOp>::Abs
+ abs() const
+ {
+ return typename UnaryReturnType<EigenOp>::Abs (*this);
+ }
+
+ inline Scalar length() const { return this->norm(); }
+ inline Scalar sqrLength() const { return this->squaredNorm(); }
+
+ template <typename Derived>
+ inline bool equal(const Eigen::MatrixBase<Derived>& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
+ {
+ return ((*this - other).cwiseAbs().array () < epsilon).all();
+ }
+
+ /*
+ inline const typename Eigen::CwiseUnaryOp<Eigen::internal::scalar_opposite_op<T>, const EigenOp>
+ negate() { return this->operator-(); }
+ // */
+
+ template <typename Derived>
+ bool operator == (const Eigen::MatrixBase<Derived>& other) const
+ {
+ return equal(other, 0);
+ }
+
+ template <typename Derived>
+ bool operator != (const Eigen::MatrixBase<Derived>& other) const
+ {
+ return !(*this == other);
+ }
+
+ // Not in place.
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(ubound,internal::scalar_min_op)
+ FCL_EIGEN_MAKE_CWISE_BINARY_OP(lbound,internal::scalar_max_op)
+
+ bool isZero() const
+ {
+ return this->Base::isZero ();
+ }
+
+ const FclOp<Transpose<const Base> > transpose () const {
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(EigenOp, 3, 3);
+ return FclOp<Transpose<const Base> >(*this);
+ }
+
+ // const FclOp<internal::inverse_impl<FclOp> > inverse () const { return FclOp<Transpose<FclOp> >(*this); }
+};
+
+}
+
+namespace fcl
+{
+// #if 0
+template<typename T, int _Options>
+static inline Eigen::FclMatrix<T, 1, _Options> normalize(const Eigen::FclMatrix<T, 1, _Options>& v)
+{
+ T sqr_length = v.squaredNorm ();
+ if(sqr_length > 0)
+ return v / (T)sqrt(sqr_length);
+ else
+ return v;
+}
+
+template<typename Derived>
+static inline typename Derived::Scalar triple(
+ const FclType<Derived>& x,
+ const FclType<Derived>& y,
+ const FclType<Derived>& z)
+{
+ return x.fcl().dot(y.fcl().cross(z.fcl()));
+}
+
+
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min
+ min(const FclType<Derived>& x, const FclType<OtherDerived>& y)
+{
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min (x.fcl(), y.fcl());
+}
+
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max
+ max(const FclType<Derived>& x, const FclType<OtherDerived>& y)
+{
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max (x.fcl(), y.fcl());
+}
+
+template<typename Derived>
+static inline const typename Eigen::UnaryReturnType<const Derived>::Abs
+abs(const FclType<Derived>& x)
+{
+ return typename Eigen::UnaryReturnType<const Derived>::Abs (x.fcl());
+}
+
+template<typename Derived>
+void generateCoordinateSystem(
+ FclType<Derived>& _w,
+ FclType<Derived>& _u,
+ FclType<Derived>& _v)
+{
+ typedef typename Derived::Scalar T;
+
+ Derived& w = _w.fcl();
+ Derived& u = _u.fcl();
+ Derived& v = _v.fcl();
+
+ T inv_length;
+ if(std::abs(w[0]) >= std::abs(w[1]))
+ {
+ inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (T)0;
+ u[2] = w[0] * inv_length;
+ v[0] = w[1] * u[2];
+ v[1] = w[2] * u[0] - w[0] * u[2];
+ v[2] = -w[1] * u[0];
+ }
+ else
+ {
+ inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (T)0;
+ u[1] = w[2] * inv_length;
+ u[2] = -w[1] * inv_length;
+ v[0] = w[1] * u[2] - w[2] * u[1];
+ v[1] = -w[0] * u[2];
+ v[2] = w[0] * u[1];
+ }
+}
+
+ // template <typename T>
+ // inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+ // const Vec3fX <T>& v)
+ // {
+ // return Vec3fX <T> (v.data * t);
+ // }
+
+// #endif
+
+/* ----- Start Matrices ------ */
+template<typename Matrix, typename Vector>
+void hat(Matrix& mat, const Vector& vec)
+{
+ mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
+}
+
+template<typename Matrix, typename Vector>
+void relativeTransform(const Matrix& R1, const Vector& t1,
+ const Matrix& R2, const Vector& t2,
+ Matrix& R, Vector& t)
+{
+ R = R1.transposeTimes(R2);
+ t = R1.transposeTimes(t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
+template<typename Matrix, typename Vector>
+void eigen(const FclType<Matrix>& m, typename Matrix::Scalar dout[3], Vector* vout)
+{
+ typedef typename Matrix::Scalar Scalar;
+ Matrix R(m.fcl());
+ int n = 3;
+ int j, iq, ip, i;
+ Scalar tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ Scalar b[3];
+ Scalar z[3];
+ Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ Scalar d[3];
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] = d[ip] = R(ip, ip);
+ z[ip] = 0;
+ }
+
+ nrot = 0;
+
+ for(i = 0; i < 50; ++i)
+ {
+ sm = 0;
+ for(ip = 0; ip < n; ++ip)
+ for(iq = ip + 1; iq < n; ++iq)
+ sm += std::abs(R(ip, iq));
+ if(sm == 0.0)
+ {
+ vout[0].setValue(v[0][0], v[0][1], v[0][2]);
+ vout[1].setValue(v[1][0], v[1][1], v[1][2]);
+ vout[2].setValue(v[2][0], v[2][1], v[2][2]);
+ dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
+ return;
+ }
+
+ if(i < 3) tresh = 0.2 * sm / (n * n);
+ else tresh = 0.0;
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ for(iq= ip + 1; iq < n; ++iq)
+ {
+ g = 100.0 * std::abs(R(ip, iq));
+ if(i > 3 &&
+ std::abs(d[ip]) + g == std::abs(d[ip]) &&
+ std::abs(d[iq]) + g == std::abs(d[iq]))
+ R(ip, iq) = 0.0;
+ else if(std::abs(R(ip, iq)) > tresh)
+ {
+ h = d[iq] - d[ip];
+ if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
+ else
+ {
+ theta = 0.5 * h / (R(ip, iq));
+ t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
+ if(theta < 0.0) t = -t;
+ }
+ c = 1.0 / std::sqrt(1 + t * t);
+ s = t * c;
+ tau = s / (1.0 + c);
+ h = t * R(ip, iq);
+ z[ip] -= h;
+ z[iq] += h;
+ d[ip] -= h;
+ d[iq] += h;
+ R(ip, iq) = 0.0;
+ for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
+ for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
+ nrot++;
+ }
+ }
+ }
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] += z[ip];
+ d[ip] = b[ip];
+ z[ip] = 0.0;
+ }
+ }
+
+ std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+ return;
+}
+
+template<typename Derived>
+Eigen::FclOp<Eigen::Transpose<const typename Eigen::internal::remove_fcl<Derived>::type> > transpose(const FclType<Derived>& R)
+{
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+ return Eigen::FclOp<Eigen::Transpose<const typename Eigen::internal::remove_fcl<Derived>::type > > (R.fcl());
+}
+
+template<typename T, int _Options>
+Eigen::FclMatrix<T,3,_Options> inverse(const Eigen::FclMatrix<T, 3, _Options>& R)
+{
+ return R.inverse();
+}
+
+template<typename Matrix, typename Vector>
+typename Matrix::Scalar quadraticForm(const Matrix& R, const Vector& v)
+{
+ return v.dot(R * v);
+}
+
+
+}
+
+
+#endif
diff --git a/include/hpp/fcl/math/math_details.h b/include/hpp/fcl/math/math_details.h
index 91db7f524d8e29406aaae329407f490e9edc870d..391efd1f789a8d7d86cb5c7fc33fc6dab0b5b50e 100644
--- a/include/hpp/fcl/math/math_details.h
+++ b/include/hpp/fcl/math/math_details.h
@@ -36,6 +36,9 @@
#ifndef FCL_MATH_DETAILS_H
#define FCL_MATH_DETAILS_H
+#if FCL_HAVE_EIGEN
+# error "This file should not be included when compiling with Eigen library"
+#endif
#include <cmath>
#include <algorithm>
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index f67934e9c22e962897c3b48649bd9d70efae5b0d..b04d816b96a9360eeac190d9b2b2f295d181b6c9 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -40,436 +40,19 @@
#include <hpp/fcl/math/vec_3f.h>
-namespace fcl
-{
-
-/// @brief Matrix2 class wrapper. the core data is in the template parameter class
-template<typename T>
-class Matrix3fX
-{
-public:
- typedef typename T::meta_type U;
- typedef typename T::vector_type S;
- T data;
-
- Matrix3fX() {}
- Matrix3fX(U xx, U xy, U xz,
- U yx, U yy, U yz,
- U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
- {}
-
- Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
- : data(v1.data, v2.data, v3.data) {}
-
- Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
-
- Matrix3fX(const T& data_) : data(data_) {}
-
- inline Vec3fX<S> getColumn(size_t i) const
- {
- return Vec3fX<S>(data.getColumn(i));
- }
-
- inline Vec3fX<S> getRow(size_t i) const
- {
- return Vec3fX<S>(data.getRow(i));
- }
-
- inline U operator () (size_t i, size_t j) const
- {
- return data(i, j);
- }
-
- inline U& operator () (size_t i, size_t j)
- {
- return data(i, j);
- }
-
- inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
- {
- return Vec3fX<S>(data * v.data);
- }
-
- inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
- {
- return Matrix3fX<T>(data * m.data);
- }
-
- inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data + other.data);
- }
-
- inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data - other.data);
- }
-
- inline Matrix3fX<T> operator + (U c) const
- {
- return Matrix3fX<T>(data + c);
- }
-
- inline Matrix3fX<T> operator - (U c) const
- {
- return Matrix3fX<T>(data - c);
- }
-
- inline Matrix3fX<T> operator * (U c) const
- {
- return Matrix3fX<T>(data * c);
- }
-
- inline Matrix3fX<T> operator / (U c) const
- {
- return Matrix3fX<T>(data / c);
- }
-
- inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
- {
- data *= other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
- {
- data += other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
- {
- data -= other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator += (U c)
- {
- data += c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator -= (U c)
- {
- data -= c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator *= (U c)
- {
- data *= c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator /= (U c)
- {
- data /= c;
- return *this;
- }
-
- inline void setIdentity()
- {
- data.setIdentity();
- }
-
- inline bool isIdentity () const
- {
- return
- data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
- data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
- data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
- }
-
- inline void setZero()
- {
- data.setZero();
- }
-
- /// @brief Set the matrix from euler angles YPR around ZYX axes
- /// @param eulerX Roll about X axis
- /// @param eulerY Pitch around Y axis
- /// @param eulerZ Yaw aboud Z axis
- ///
- /// These angles are used to produce a rotation matrix. The euler
- /// angles are applied in ZYX order. I.e a vector is first rotated
- /// about X then Y and then Z
- inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
- {
- FCL_REAL ci(cos(eulerX));
- FCL_REAL cj(cos(eulerY));
- FCL_REAL ch(cos(eulerZ));
- FCL_REAL si(sin(eulerX));
- FCL_REAL sj(sin(eulerY));
- FCL_REAL sh(sin(eulerZ));
- FCL_REAL cc = ci * ch;
- FCL_REAL cs = ci * sh;
- FCL_REAL sc = si * ch;
- FCL_REAL ss = si * sh;
-
- setValue(cj * ch, sj * sc - cs, sj * cc + ss,
- cj * sh, sj * ss + cc, sj * cs - sc,
- -sj, cj * si, cj * ci);
-
- }
-
- /// @brief Set the matrix from euler angles using YPR around YXZ respectively
- /// @param yaw Yaw about Y axis
- /// @param pitch Pitch about X axis
- /// @param roll Roll about Z axis
- void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
- {
- setEulerZYX(roll, pitch, yaw);
- }
-
- inline U determinant() const
- {
- return data.determinant();
- }
-
- Matrix3fX<T>& transpose()
- {
- data.transpose();
- return *this;
- }
-
- Matrix3fX<T>& inverse()
- {
- data.inverse();
- return *this;
- }
-
- Matrix3fX<T>& abs()
- {
- data.abs();
- return *this;
- }
-
- static const Matrix3fX<T>& getIdentity()
- {
- static const Matrix3fX<T> I((U)1, (U)0, (U)0,
- (U)0, (U)1, (U)0,
- (U)0, (U)0, (U)1);
- return I;
- }
-
- Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data.transposeTimes(other.data));
- }
-
- Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data.timesTranspose(other.data));
- }
-
- Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
- {
- return Vec3fX<S>(data.transposeTimes(v.data));
- }
-
- Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
- {
- Matrix3fX<T> res(*this);
- res *= m;
- return res.timesTranspose(*this);
- }
-
- // (1 0 0)^T (*this)^T v
- inline U transposeDotX(const Vec3fX<S>& v) const
- {
- return data.transposeDot(0, v.data);
- }
-
- // (0 1 0)^T (*this)^T v
- inline U transposeDotY(const Vec3fX<S>& v) const
- {
- return data.transposeDot(1, v.data);
- }
-
- // (0 0 1)^T (*this)^T v
- inline U transposeDotZ(const Vec3fX<S>& v) const
- {
- return data.transposeDot(2, v.data);
- }
-
- // (\delta_{i3})^T (*this)^T v
- inline U transposeDot(size_t i, const Vec3fX<S>& v) const
- {
- return data.transposeDot(i, v.data);
- }
-
- // (1 0 0)^T (*this) v
- inline U dotX(const Vec3fX<S>& v) const
- {
- return data.dot(0, v.data);
- }
-
- // (0 1 0)^T (*this) v
- inline U dotY(const Vec3fX<S>& v) const
- {
- return data.dot(1, v.data);
- }
-
- // (0 0 1)^T (*this) v
- inline U dotZ(const Vec3fX<S>& v) const
- {
- return data.dot(2, v.data);
- }
-
- // (\delta_{i3})^T (*this) v
- inline U dot(size_t i, const Vec3fX<S>& v) const
- {
- return data.dot(i, v.data);
- }
-
- inline void setValue(U xx, U xy, U xz,
- U yx, U yy, U yz,
- U zx, U zy, U zz)
- {
- data.setValue(xx, xy, xz,
- yx, yy, yz,
- zx, zy, zz);
- }
-
- inline void setValue(U x)
- {
- data.setValue(x);
- }
-};
-
-template<typename T>
-void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
-{
- mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
-}
-
-template<typename T>
-void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
- const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
- Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
-{
- R = R1.transposeTimes(R2);
- t = R1.transposeTimes(t2 - t1);
-}
-
-/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
-template<typename T>
-void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
-{
- Matrix3fX<T> R(m);
- int n = 3;
- int j, iq, ip, i;
- typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- typename T::meta_type b[3];
- typename T::meta_type z[3];
- typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- typename T::meta_type d[3];
-
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] = d[ip] = R(ip, ip);
- z[ip] = 0;
- }
-
- nrot = 0;
-
- for(i = 0; i < 50; ++i)
- {
- sm = 0;
- for(ip = 0; ip < n; ++ip)
- for(iq = ip + 1; iq < n; ++iq)
- sm += std::abs(R(ip, iq));
- if(sm == 0.0)
- {
- vout[0].setValue(v[0][0], v[0][1], v[0][2]);
- vout[1].setValue(v[1][0], v[1][1], v[1][2]);
- vout[2].setValue(v[2][0], v[2][1], v[2][2]);
- dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
- return;
- }
-
- if(i < 3) tresh = 0.2 * sm / (n * n);
- else tresh = 0.0;
-
- for(ip = 0; ip < n; ++ip)
- {
- for(iq= ip + 1; iq < n; ++iq)
- {
- g = 100.0 * std::abs(R(ip, iq));
- if(i > 3 &&
- std::abs(d[ip]) + g == std::abs(d[ip]) &&
- std::abs(d[iq]) + g == std::abs(d[iq]))
- R(ip, iq) = 0.0;
- else if(std::abs(R(ip, iq)) > tresh)
- {
- h = d[iq] - d[ip];
- if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
- else
- {
- theta = 0.5 * h / (R(ip, iq));
- t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
- if(theta < 0.0) t = -t;
- }
- c = 1.0 / std::sqrt(1 + t * t);
- s = t * c;
- tau = s / (1.0 + c);
- h = t * R(ip, iq);
- z[ip] -= h;
- z[iq] += h;
- d[ip] -= h;
- d[iq] += h;
- R(ip, iq) = 0.0;
- for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
- for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
- for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
- for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
- nrot++;
- }
- }
- }
- for(ip = 0; ip < n; ++ip)
- {
- b[ip] += z[ip];
- d[ip] = b[ip];
- z[ip] = 0.0;
- }
- }
-
- std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
-
- return;
-}
-
-template<typename T>
-Matrix3fX<T> abs(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(abs(R.data));
-}
-
-template<typename T>
-Matrix3fX<T> transpose(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(transpose(R.data));
-}
-
-template<typename T>
-Matrix3fX<T> inverse(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(inverse(R.data));
-}
+#if ! FCL_HAVE_EIGEN
+# include <hpp/fcl/math/matrix_3fx.h>
+#endif
-template<typename T>
-typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+namespace fcl
{
- return v.dot(R * v);
-}
-
-#if FCL_HAVE_SSE
-typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
+#if FCL_HAVE_EIGEN
+ typedef Eigen::FclMatrix<FCL_REAL, 3> Matrix3f;
+#elif FCL_HAVE_SSE
+ typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
#else
-typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
+ typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
#endif
static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
diff --git a/include/hpp/fcl/math/matrix_3fx.h b/include/hpp/fcl/math/matrix_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..18584b0652ee56f850567b76672eff362301f54e
--- /dev/null
+++ b/include/hpp/fcl/math/matrix_3fx.h
@@ -0,0 +1,470 @@
+/*
+ * 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_MATRIX_3FX_H
+#define FCL_MATRIX_3FX_H
+
+#include <hpp/fcl/math/vec_3f.h>
+
+namespace fcl
+{
+
+/// @brief Matrix2 class wrapper. the core data is in the template parameter class
+template<typename T>
+class Matrix3fX
+{
+public:
+ typedef typename T::meta_type U;
+ typedef typename T::vector_type S;
+ T data;
+
+ Matrix3fX() {}
+ Matrix3fX(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
+ {}
+
+ Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
+ : data(v1.data, v2.data, v3.data) {}
+
+ Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
+
+ Matrix3fX(const T& data_) : data(data_) {}
+
+ inline Vec3fX<S> getColumn(size_t i) const
+ {
+ return Vec3fX<S>(data.getColumn(i));
+ }
+
+ inline Vec3fX<S> getRow(size_t i) const
+ {
+ return Vec3fX<S>(data.getRow(i));
+ }
+
+ inline U operator () (size_t i, size_t j) const
+ {
+ return data(i, j);
+ }
+
+ inline U& operator () (size_t i, size_t j)
+ {
+ return data(i, j);
+ }
+
+ inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data * v.data);
+ }
+
+ inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
+ {
+ return Matrix3fX<T>(data * m.data);
+ }
+
+ inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data + other.data);
+ }
+
+ inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data - other.data);
+ }
+
+ inline Matrix3fX<T> operator + (U c) const
+ {
+ return Matrix3fX<T>(data + c);
+ }
+
+ inline Matrix3fX<T> operator - (U c) const
+ {
+ return Matrix3fX<T>(data - c);
+ }
+
+ inline Matrix3fX<T> operator * (U c) const
+ {
+ return Matrix3fX<T>(data * c);
+ }
+
+ inline Matrix3fX<T> operator / (U c) const
+ {
+ return Matrix3fX<T>(data / c);
+ }
+
+ inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
+ {
+ data *= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
+ {
+ data += other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
+ {
+ data -= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (U c)
+ {
+ data += c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (U c)
+ {
+ data -= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator *= (U c)
+ {
+ data *= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator /= (U c)
+ {
+ data /= c;
+ return *this;
+ }
+
+ inline void setIdentity()
+ {
+ data.setIdentity();
+ }
+
+ inline bool isIdentity () const
+ {
+ return
+ data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
+ data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
+ data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
+ }
+
+ inline void setZero()
+ {
+ data.setZero();
+ }
+
+ /// @brief Set the matrix from euler angles YPR around ZYX axes
+ /// @param eulerX Roll about X axis
+ /// @param eulerY Pitch around Y axis
+ /// @param eulerZ Yaw aboud Z axis
+ ///
+ /// These angles are used to produce a rotation matrix. The euler
+ /// angles are applied in ZYX order. I.e a vector is first rotated
+ /// about X then Y and then Z
+ inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
+ {
+ FCL_REAL ci(cos(eulerX));
+ FCL_REAL cj(cos(eulerY));
+ FCL_REAL ch(cos(eulerZ));
+ FCL_REAL si(sin(eulerX));
+ FCL_REAL sj(sin(eulerY));
+ FCL_REAL sh(sin(eulerZ));
+ FCL_REAL cc = ci * ch;
+ FCL_REAL cs = ci * sh;
+ FCL_REAL sc = si * ch;
+ FCL_REAL ss = si * sh;
+
+ setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+ cj * sh, sj * ss + cc, sj * cs - sc,
+ -sj, cj * si, cj * ci);
+
+ }
+
+ /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+ /// @param yaw Yaw about Y axis
+ /// @param pitch Pitch about X axis
+ /// @param roll Roll about Z axis
+ void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
+ {
+ setEulerZYX(roll, pitch, yaw);
+ }
+
+ inline U determinant() const
+ {
+ return data.determinant();
+ }
+
+ Matrix3fX<T>& transpose()
+ {
+ data.transpose();
+ return *this;
+ }
+
+ Matrix3fX<T>& inverse()
+ {
+ data.inverse();
+ return *this;
+ }
+
+ Matrix3fX<T>& abs()
+ {
+ data.abs();
+ return *this;
+ }
+
+ static const Matrix3fX<T>& getIdentity()
+ {
+ static const Matrix3fX<T> I((U)1, (U)0, (U)0,
+ (U)0, (U)1, (U)0,
+ (U)0, (U)0, (U)1);
+ return I;
+ }
+
+ Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.transposeTimes(other.data));
+ }
+
+ Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.timesTranspose(other.data));
+ }
+
+ Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data.transposeTimes(v.data));
+ }
+
+ Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+ {
+ Matrix3fX<T> res(*this);
+ res *= m;
+ return res.timesTranspose(*this);
+ }
+
+ // (1 0 0)^T (*this)^T v
+ inline U transposeDotX(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this)^T v
+ inline U transposeDotY(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this)^T v
+ inline U transposeDotZ(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this)^T v
+ inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(i, v.data);
+ }
+
+ // (1 0 0)^T (*this) v
+ inline U dotX(const Vec3fX<S>& v) const
+ {
+ return data.dot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this) v
+ inline U dotY(const Vec3fX<S>& v) const
+ {
+ return data.dot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this) v
+ inline U dotZ(const Vec3fX<S>& v) const
+ {
+ return data.dot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this) v
+ inline U dot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.dot(i, v.data);
+ }
+
+ inline void setValue(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz)
+ {
+ data.setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+
+ inline void setValue(U x)
+ {
+ data.setValue(x);
+ }
+};
+
+template<typename T>
+void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
+{
+ mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
+}
+
+template<typename T>
+void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
+ const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
+ Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
+{
+ R = R1.transposeTimes(R2);
+ t = R1.transposeTimes(t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
+template<typename T>
+void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
+{
+ Matrix3fX<T> R(m);
+ int n = 3;
+ int j, iq, ip, i;
+ typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ typename T::meta_type b[3];
+ typename T::meta_type z[3];
+ typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ typename T::meta_type d[3];
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] = d[ip] = R(ip, ip);
+ z[ip] = 0;
+ }
+
+ nrot = 0;
+
+ for(i = 0; i < 50; ++i)
+ {
+ sm = 0;
+ for(ip = 0; ip < n; ++ip)
+ for(iq = ip + 1; iq < n; ++iq)
+ sm += std::abs(R(ip, iq));
+ if(sm == 0.0)
+ {
+ vout[0].setValue(v[0][0], v[0][1], v[0][2]);
+ vout[1].setValue(v[1][0], v[1][1], v[1][2]);
+ vout[2].setValue(v[2][0], v[2][1], v[2][2]);
+ dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
+ return;
+ }
+
+ if(i < 3) tresh = 0.2 * sm / (n * n);
+ else tresh = 0.0;
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ for(iq= ip + 1; iq < n; ++iq)
+ {
+ g = 100.0 * std::abs(R(ip, iq));
+ if(i > 3 &&
+ std::abs(d[ip]) + g == std::abs(d[ip]) &&
+ std::abs(d[iq]) + g == std::abs(d[iq]))
+ R(ip, iq) = 0.0;
+ else if(std::abs(R(ip, iq)) > tresh)
+ {
+ h = d[iq] - d[ip];
+ if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
+ else
+ {
+ theta = 0.5 * h / (R(ip, iq));
+ t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
+ if(theta < 0.0) t = -t;
+ }
+ c = 1.0 / std::sqrt(1 + t * t);
+ s = t * c;
+ tau = s / (1.0 + c);
+ h = t * R(ip, iq);
+ z[ip] -= h;
+ z[iq] += h;
+ d[ip] -= h;
+ d[iq] += h;
+ R(ip, iq) = 0.0;
+ for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
+ for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
+ nrot++;
+ }
+ }
+ }
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] += z[ip];
+ d[ip] = b[ip];
+ z[ip] = 0.0;
+ }
+ }
+
+ std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+ return;
+}
+
+template<typename T>
+Matrix3fX<T> abs(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(abs(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(transpose(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> inverse(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(inverse(R.data));
+}
+
+template<typename T>
+typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+{
+ return v.dot(R * v);
+}
+
+}
+
+#endif
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index 555d12b6249f3d447ef86ff7ca22d36a8add2277..f81e6da128d6ed07db88736b87c7e5d817e71d60 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -40,10 +40,19 @@
#include <hpp/fcl/config-fcl.hh>
#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/math/math_details.h>
-#if FCL_HAVE_SSE
-#include <hpp/fcl/simd/math_simd_details.h>
+#if FCL_HAVE_EIGEN
+# include <hpp/fcl/eigen/math_details.h>
+#elif FCL_HAVE_SSE
+# include <hpp/fcl/simd/math_simd_details.h>
+#else
+# include <hpp/fcl/math/math_details.h>
+#endif
+
+#if FCL_HAVE_EIGEN
+# include <hpp/fcl/eigen/vec_3fx.h>
+#else
+# include <hpp/fcl/math/vec_3fx.h>
#endif
#include <cmath>
@@ -54,186 +63,9 @@
namespace fcl
{
-/// @brief Vector3 class wrapper. The core data is in the template parameter class.
-template <typename T>
-class Vec3fX
-{
-public:
- typedef typename T::meta_type U;
-
- /// @brief interval vector3 data
- T data;
-
- Vec3fX() {}
- Vec3fX(const Vec3fX& other) : data(other.data) {}
-
- /// @brief create Vector (x, y, z)
- Vec3fX(U x, U y, U z) : data(x, y, z) {}
-
- /// @brief create vector (x, x, x)
- Vec3fX(U x) : data(x) {}
-
- /// @brief create vector using the internal data type
- Vec3fX(const T& data_) : data(data_) {}
-
- inline U operator [] (size_t i) const { return data[i]; }
- inline U& operator [] (size_t i) { return data[i]; }
-
- inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
- inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
- inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
- inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
- inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
- inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
- inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
- inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
- inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
- inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
- inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
- inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
- inline Vec3fX& operator += (U t) { data += t; return *this; }
- inline Vec3fX& operator -= (U t) { data -= t; return *this; }
- inline Vec3fX& operator *= (U t) { data *= t; return *this; }
- inline Vec3fX& operator /= (U t) { data /= t; return *this; }
- inline Vec3fX operator - () const { return Vec3fX(-data); }
- inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
- inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
- inline Vec3fX& normalize()
- {
- U sqr_length = details::dot_prod3(data, data);
- if(sqr_length > 0)
- *this /= (U)sqrt(sqr_length);
- return *this;
- }
-
- inline Vec3fX& normalize(bool* signal)
- {
- U sqr_length = details::dot_prod3(data, data);
- if(sqr_length > 0)
- {
- *this /= (U)sqrt(sqr_length);
- *signal = true;
- }
- else
- *signal = false;
- return *this;
- }
-
- inline Vec3fX& abs()
- {
- data = abs(data);
- return *this;
- }
-
- inline U length() const { return sqrt(details::dot_prod3(data, data)); }
- inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
- inline U sqrLength() const { return details::dot_prod3(data, data); }
- inline U squaredNorm() const { return details::dot_prod3(data, data); }
- inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
- inline void setValue(U x) { data.setValue(x); }
- inline void setZero () {data.setValue (0); }
- inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
- inline Vec3fX<T>& negate() { data.negate(); return *this; }
-
- bool operator == (const Vec3fX& other) const
- {
- return equal(other, 0);
- }
-
- bool operator != (const Vec3fX& other) const
- {
- return !(*this == other);
- }
-
-
- inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
- {
- data.ubound(u.data);
- return *this;
- }
-
- inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
- {
- data.lbound(l.data);
- return *this;
- }
-
- bool isZero() const
- {
- return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
- }
-
-};
-
-template<typename T>
-static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
-{
- typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
- if(sqr_length > 0)
- return v / (typename T::meta_type)sqrt(sqr_length);
- else
- return v;
-}
-
-template <typename T>
-static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
-{
- return x.dot(y.cross(z));
-}
-
-template <typename T>
-std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
-{
- out << x[0] << " " << x[1] << " " << x[2];
- return out;
-}
-
-template <typename T>
-static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
-{
- return Vec3fX<T>(details::min(x.data, y.data));
-}
-
-template <typename T>
-static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
-{
- return Vec3fX<T>(details::max(x.data, y.data));
-}
-
-template <typename T>
-static inline Vec3fX<T> abs(const Vec3fX<T>& x)
-{
- return Vec3fX<T>(details::abs(x.data));
-}
-
-template <typename T>
-void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
-{
- typedef typename T::meta_type U;
- U inv_length;
- if(std::abs(w[0]) >= std::abs(w[1]))
- {
- inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = (U)0;
- u[2] = w[0] * inv_length;
- v[0] = w[1] * u[2];
- v[1] = w[2] * u[0] - w[0] * u[2];
- v[2] = -w[1] * u[0];
- }
- else
- {
- inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = (U)0;
- u[1] = w[2] * inv_length;
- u[2] = -w[1] * inv_length;
- v[0] = w[1] * u[2] - w[2] * u[1];
- v[1] = -w[0] * u[2];
- v[2] = w[0] * u[1];
- }
-}
-
-#if FCL_HAVE_SSE
+#if FCL_HAVE_EIGEN
+ typedef Eigen::FclMatrix<FCL_REAL, 1> Vec3f;
+#elif FCL_HAVE_SSE
typedef Vec3fX<details::sse_meta_f4> Vec3f;
#else
typedef Vec3fX<details::Vec3Data<FCL_REAL> > Vec3f;
@@ -245,14 +77,6 @@ static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
return o;
}
- template <typename T>
- inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
- const Vec3fX <T>& v)
- {
- return Vec3fX <T> (v.data * t);
- }
-
-
}
diff --git a/include/hpp/fcl/math/vec_3fx.h b/include/hpp/fcl/math/vec_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..4cc8d86907cb73442dc1650d3f38771616034917
--- /dev/null
+++ b/include/hpp/fcl/math/vec_3fx.h
@@ -0,0 +1,241 @@
+/*
+ * 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.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_VEC_3FX_H
+#define FCL_VEC_3FX_H
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/data_types.h>
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+
+namespace fcl
+{
+
+/// @brief Vector3 class wrapper. The core data is in the template parameter class.
+template <typename T>
+class Vec3fX
+{
+public:
+ typedef typename T::meta_type U;
+
+ /// @brief interval vector3 data
+ T data;
+
+ Vec3fX() {}
+ Vec3fX(const Vec3fX& other) : data(other.data) {}
+
+ /// @brief create Vector (x, y, z)
+ Vec3fX(U x, U y, U z) : data(x, y, z) {}
+
+ /// @brief create vector (x, x, x)
+ Vec3fX(U x) : data(x) {}
+
+ /// @brief create vector using the internal data type
+ Vec3fX(const T& data_) : data(data_) {}
+
+ inline U operator [] (size_t i) const { return data[i]; }
+ inline U& operator [] (size_t i) { return data[i]; }
+
+ inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
+ inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
+ inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
+ inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
+ inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
+ inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
+ inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
+ inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
+ inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
+ inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
+ inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
+ inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
+ inline Vec3fX& operator += (U t) { data += t; return *this; }
+ inline Vec3fX& operator -= (U t) { data -= t; return *this; }
+ inline Vec3fX& operator *= (U t) { data *= t; return *this; }
+ inline Vec3fX& operator /= (U t) { data /= t; return *this; }
+ inline Vec3fX operator - () const { return Vec3fX(-data); }
+ inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
+ inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
+ inline Vec3fX& normalize()
+ {
+ U sqr_length = details::dot_prod3(data, data);
+ if(sqr_length > 0)
+ *this /= (U)sqrt(sqr_length);
+ return *this;
+ }
+
+ inline Vec3fX& normalize(bool* signal)
+ {
+ U sqr_length = details::dot_prod3(data, data);
+ if(sqr_length > 0)
+ {
+ *this /= (U)sqrt(sqr_length);
+ *signal = true;
+ }
+ else
+ *signal = false;
+ return *this;
+ }
+
+ inline Vec3fX& abs()
+ {
+ data = abs(data);
+ return *this;
+ }
+
+ inline U length() const { return sqrt(details::dot_prod3(data, data)); }
+ inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
+ inline U sqrLength() const { return details::dot_prod3(data, data); }
+ inline U squaredNorm() const { return details::dot_prod3(data, data); }
+ inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
+ inline void setValue(U x) { data.setValue(x); }
+ inline void setZero () {data.setValue (0); }
+ inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
+ inline Vec3fX<T>& negate() { data.negate(); return *this; }
+
+ bool operator == (const Vec3fX& other) const
+ {
+ return equal(other, 0);
+ }
+
+ bool operator != (const Vec3fX& other) const
+ {
+ return !(*this == other);
+ }
+
+
+ inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
+ {
+ data.ubound(u.data);
+ return *this;
+ }
+
+ inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
+ {
+ data.lbound(l.data);
+ return *this;
+ }
+
+ bool isZero() const
+ {
+ return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
+ }
+
+};
+
+template<typename T>
+static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
+{
+ typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
+ if(sqr_length > 0)
+ return v / (typename T::meta_type)sqrt(sqr_length);
+ else
+ return v;
+}
+
+template <typename T>
+static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
+{
+ return x.dot(y.cross(z));
+}
+
+template <typename T>
+std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
+{
+ out << x[0] << " " << x[1] << " " << x[2];
+ return out;
+}
+
+template <typename T>
+static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return Vec3fX<T>(details::min(x.data, y.data));
+}
+
+template <typename T>
+static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return Vec3fX<T>(details::max(x.data, y.data));
+}
+
+template <typename T>
+static inline Vec3fX<T> abs(const Vec3fX<T>& x)
+{
+ return Vec3fX<T>(details::abs(x.data));
+}
+
+template <typename T>
+void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
+{
+ typedef typename T::meta_type U;
+ U inv_length;
+ if(std::abs(w[0]) >= std::abs(w[1]))
+ {
+ inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (U)0;
+ u[2] = w[0] * inv_length;
+ v[0] = w[1] * u[2];
+ v[1] = w[2] * u[0] - w[0] * u[2];
+ v[2] = -w[1] * u[0];
+ }
+ else
+ {
+ inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (U)0;
+ u[1] = w[2] * inv_length;
+ u[2] = -w[1] * inv_length;
+ v[0] = w[1] * u[2] - w[2] * u[1];
+ v[1] = -w[0] * u[2];
+ v[2] = w[0] * u[1];
+ }
+}
+
+ template <typename T>
+ inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+ const Vec3fX <T>& v)
+ {
+ return Vec3fX <T> (v.data * t);
+ }
+
+}
+
+
+#endif
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 0db42cdf967319f37a5be93f7a2d3dc0dcce02b7..eb308a1ce9f81d77d4f36d99ec2cc2f6f110ce85 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -209,12 +209,12 @@ struct GJKSolver_libccd
}
- void enableCachedGuess(bool if_enable) const
+ void enableCachedGuess(bool /*if_enable*/) const
{
// TODO: need change libccd to exploit spatial coherence
}
- void setCachedGuess(const Vec3f& guess) const
+ void setCachedGuess(const Vec3f& /*guess*/) const
{
// TODO: need change libccd to exploit spatial coherence
}
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 7d9322a0d5f7dc20b8362eb17270ba24886bdf42..6c8717ad522ca2edd5c6b93e10694205ca1cdf00 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -271,7 +271,7 @@ public:
FCL_REAL* plane_dis_,
int num_planes_,
Vec3f* points_,
- int num_points_,
+ int /*num_points_*/,
int* polygons_) : ShapeBase()
{
plane_normals = plane_normals_;
diff --git a/include/hpp/fcl/traversal/traversal_node_base.h b/include/hpp/fcl/traversal/traversal_node_base.h
index 2208b0ab7fc077df553e12b9e4e3a12420864105..edaf35e340b4253fd3017c0ae6a94a009b963f97 100644
--- a/include/hpp/fcl/traversal/traversal_node_base.h
+++ b/include/hpp/fcl/traversal/traversal_node_base.h
@@ -106,7 +106,7 @@ public:
virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const = 0;
/// @brief Leaf test between node b1 and b2, if they are both leafs
- virtual void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
+ virtual void leafTesting(int /*b1*/, int /*b2*/, FCL_REAL& /*sqrDistLowerBound*/) const
{
throw std::runtime_error ("Not implemented");
}
diff --git a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
index fe61f5f84540aa2bfc339434a67031765a4c0da5..119a549b40b3f11d3d4b4042d8c978c5b02bb493 100644
--- a/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/hpp/fcl/traversal/traversal_node_bvh_shape.h
@@ -723,7 +723,7 @@ public:
}
/// @brief BV culling test in one BVTT node
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int /*b2*/) const
{
return model1->getBV(b1).bv.distance(model2_bv);
}
@@ -803,7 +803,7 @@ public:
}
/// @brief Distance testing between leaves (one triangle and one shape)
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_leaf_tests++;
@@ -987,7 +987,7 @@ public:
}
- FCL_REAL BVTesting(int b1, int b2) const
+ FCL_REAL BVTesting(int b1, int /*b2*/) const
{
if(this->enable_statistics) this->num_bv_tests++;
return distance(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 8fb9e8dedf1229844d485197ff1e12bddcf5b4db..c89883caca70f6dc92aa669a666ba820f479b95a 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -610,7 +610,7 @@ OBB OBB::operator + (const OBB& other) const
}
-FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
{
std::cerr << "OBB distance not implemented!" << std::endl;
return 0.0;
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index 5c6f2ddd2fc0037949431e84703fd13d4d15fa75..dbdbc5fcc42d4e52b27883009e7292da578df565 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -220,7 +220,7 @@ KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
template<size_t N>
-FCL_REAL KDOP<N>::distance(const KDOP<N>& other, Vec3f* P, Vec3f* Q) const
+FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
{
std::cerr << "KDOP distance not implemented!" << std::endl;
return 0.0;
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index 111aecb40e8d4d82e546b69155e1ffbbe56e06fc..9a1ae33e743cbb443e9598f1d287c4a7446da2b1 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -63,7 +63,7 @@ bool kIOS::overlap(const kIOS& other) const
return true;
}
- bool kIOS::overlap(const kIOS& other, FCL_REAL& sqrDistLowerBound) const
+ bool kIOS::overlap(const kIOS& /*other*/, FCL_REAL& /*sqrDistLowerBound*/) const
{
throw std::runtime_error ("Not implemented yet.");
}
diff --git a/src/ccd/interpolation/interpolation_linear.cpp b/src/ccd/interpolation/interpolation_linear.cpp
index 3821531d2ed7388176db8e8606d61cf81294ff12..a9635dad0f15bcf00592cdac939981a6f0d18715 100644
--- a/src/ccd/interpolation/interpolation_linear.cpp
+++ b/src/ccd/interpolation/interpolation_linear.cpp
@@ -84,7 +84,7 @@ FCL_REAL InterpolationLinear::getMovementLengthBound(FCL_REAL time) const
return getValueUpperBound() - getValue(time);
}
-FCL_REAL InterpolationLinear::getVelocityBound(FCL_REAL time) const
+FCL_REAL InterpolationLinear::getVelocityBound(FCL_REAL /*time*/) const
{
return (value_1_ - value_0_);
}
diff --git a/src/collision.cpp b/src/collision.cpp
index d71c1231919afef403732b3f2046460d487a30a0..6934af5da62ec2839754b5d61f633bf997a5ca58 100644
--- a/src/collision.cpp
+++ b/src/collision.cpp
@@ -50,7 +50,7 @@ CollisionFunctionMatrix<GJKSolver>& getCollisionFunctionLookTable()
{
static CollisionFunctionMatrix<GJKSolver> table;
return table;
-};
+}
template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
diff --git a/src/distance_func_matrix.cpp b/src/distance_func_matrix.cpp
index 84a182a1d549b511beeb51a1e88a02d452f1dd54..07d4feb10e39fc15ff6cde96815c6cc221baace7 100644
--- a/src/distance_func_matrix.cpp
+++ b/src/distance_func_matrix.cpp
@@ -280,7 +280,7 @@ FCL_REAL BVHDistance<OBBRSS>(const CollisionGeometry* o1, const Transform3f& tf1
template<typename T_BVH, typename NarrowPhaseSolver>
FCL_REAL BVHDistance(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
+ const NarrowPhaseSolver* /*nsolver*/,
const DistanceRequest& request, DistanceResult& result)
{
return BVHDistance<T_BVH>(o1, tf1, o2, tf2, request, result);
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 4d2eb0a3ffd04a6540e28aad1bd3de3c313c5cc4..dfa5da3c10ff82a777a81dc5615a7b1d71d03c33 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -1882,7 +1882,7 @@ bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
FCL_REAL depth = - std::abs(signed_dist) + s1.radius;
if(depth >= 0)
{
- if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
if(penetration_depth) *penetration_depth = depth;
if(contact_points) *contact_points = center - new_s2.n * signed_dist;
@@ -1958,7 +1958,7 @@ bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
// compute the contact point by project the deepest point onto the plane
if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = (signed_dist > 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
if(contact_points) *contact_points = p - new_s2.n * new_s2.signedDistance(p);
return true;
@@ -2025,13 +2025,13 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
{
if(penetration_depth) *penetration_depth = abs_d1 + s1.radius;
if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
- if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
}
else
{
if(penetration_depth) *penetration_depth = abs_d2 + s1.radius;
if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
- if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
}
return true;
}
@@ -2062,7 +2062,7 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
}
}
- if(normal) *normal = (d1 < 0) ? new_s2.n : -new_s2.n;
+ if(normal) { if (d1 < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
return true;
}
}
@@ -2117,7 +2117,7 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
else
{
if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n;
+ if(normal) { if (d < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
if(contact_points) *contact_points = T - new_s2.n * d;
return true;
}
@@ -2161,13 +2161,13 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
{
if(penetration_depth) *penetration_depth = abs_d2;
if(contact_points) *contact_points = c2 - new_s2.n * d2;
- if(normal) *normal = (d2 < 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
}
else
{
if(penetration_depth) *penetration_depth = abs_d1;
if(contact_points) *contact_points = c1 - new_s2.n * d1;
- if(normal) *normal = (d1 < 0) ? -new_s2.n : new_s2.n;
+ if(normal) { if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
}
return true;
}
@@ -2197,7 +2197,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
else
{
if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = (d < 0) ? new_s2.n : -new_s2.n;
+ if(normal) { if (d < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
if(contact_points) *contact_points = T - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d;
return true;
}
@@ -2248,7 +2248,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
}
if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
- if(normal) *normal = (d_positive > d_negative) ? -new_s2.n : new_s2.n;
+ if(normal) { if (d_positive > d_negative) *normal = -new_s2.n; else *normal = new_s2.n; }
if(contact_points)
{
Vec3f p[2];
@@ -2367,7 +2367,7 @@ bool planeTriangleIntersect(const Plane& s1, const Transform3f& tf1,
}
if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
- if(normal) *normal = (d_positive > d_negative) ? new_s1.n : -new_s1.n;
+ if(normal) { if (d_positive > d_negative) *normal = new_s1.n; else *normal = -new_s1.n; }
if(contact_points)
{
Vec3f p[2];
@@ -2416,7 +2416,7 @@ bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1,
bool planeIntersect(const Plane& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/)
{
Plane new_s1 = transform(s1, tf1);
Plane new_s2 = transform(s2, tf2);
@@ -2583,7 +2583,7 @@ bool GJKSolver_libccd::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, cons
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Halfspace s;
Vec3f p, d;
@@ -2595,7 +2595,7 @@ bool GJKSolver_libccd::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1,
template<>
bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Plane pl;
Vec3f p, d;
@@ -2607,7 +2607,7 @@ bool GJKSolver_libccd::shapeIntersect<Plane, Halfspace>(const Plane& s1, const T
template<>
bool GJKSolver_libccd::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Plane pl;
Vec3f p, d;
@@ -2792,9 +2792,9 @@ bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tra
}
template<>
-bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_libccd::shapeDistance<Capsule, Capsule>(const Capsule& /*s1*/, const Transform3f& /*tf1*/,
+ const Capsule& /*s2*/, const Transform3f& /*tf2*/,
+ FCL_REAL* /*dist*/, Vec3f* /*p1*/, Vec3f* /*p2*/) const
{
abort ();
}
@@ -2967,7 +2967,7 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Halfspace s;
Vec3f p, d;
@@ -2979,7 +2979,7 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Halfspace>(const Halfspace& s1,
template<>
bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Plane pl;
Vec3f p, d;
@@ -2991,7 +2991,7 @@ bool GJKSolver_indep::shapeIntersect<Plane, Halfspace>(const Plane& s1, const Tr
template<>
bool GJKSolver_indep::shapeIntersect<Halfspace, Plane>(const Halfspace& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/) const
{
Plane pl;
Vec3f p, d;
@@ -3176,9 +3176,9 @@ bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const Tran
}
template<>
-bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2) const
+bool GJKSolver_indep::shapeDistance<Capsule, Capsule>(const Capsule& /*s1*/, const Transform3f& /*tf1*/,
+ const Capsule& /*s2*/, const Transform3f& /*tf2*/,
+ FCL_REAL* /*dist*/, Vec3f* /*p1*/, Vec3f* /*p2*/) const
{
abort ();
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 5ee7213a634a5967e13b8e0046af3b9ee99856aa..ed1da73fe4671a66ef6a3c6b4261772d218b7f3f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,5 +1,13 @@
config_files(fcl_resources/config.h)
+macro(add_fcl_template_test test_name)
+ add_executable(${ARGV})
+ target_link_libraries(${test_name}
+ ${Boost_LIBRARIES}
+ )
+ add_test(${test_name} ${EXECUTABLE_OUTPUT_PATH}/${test_name})
+endmacro(add_fcl_template_test)
+
macro(add_fcl_test test_name)
add_executable(${ARGV})
target_link_libraries(${test_name}
diff --git a/test/test_fcl_eigen.cpp b/test/test_fcl_eigen.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7db1958a888e2e2db5cc4ba5266d0e3739a8d641
--- /dev/null
+++ b/test/test_fcl_eigen.cpp
@@ -0,0 +1,120 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#define BOOST_TEST_MODULE "FCL_EIGEN"
+#include <boost/test/included/unit_test.hpp>
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/eigen/vec_3fx.h>
+
+using namespace fcl;
+
+#define PRINT_VECTOR(a) std::cout << #a": " << a.base().transpose() << std::endl;
+#define PRINT_MATRIX(a) std::cout << #a": " << a << std::endl;
+
+BOOST_AUTO_TEST_CASE(fcl_eigen_vec3fx)
+{
+ typedef Eigen::FclMatrix <double, 1, 0> Vec3f;
+ Vec3f a (0, 1, 2);
+ Vec3f b (1, 2, 3);
+
+ std::cout << (Vec3f::Base&) a - (Vec3f::Base&) b << std::endl;
+ std::cout << a - b << std::endl;
+ Vec3f c = a - b;
+ std::cout << c << std::endl;
+ c.normalize ();
+
+ Vec3f l = (a - b).normalize ();
+
+ std::cout << l << std::endl;
+ std::cout << c << std::endl;
+
+ Vec3f d = -b;
+ std::cout << d << std::endl;
+
+ d += b;
+ std::cout << d << std::endl;
+
+ d += 1;
+ std::cout << d << std::endl;
+
+ d *= b;
+ std::cout << d << std::endl;
+
+ // std::cout << d * b << std::endl;
+ // std::cout << d / b << std::endl;
+ // std::cout << d + 3.4 << std::endl;
+ // std::cout << d - 3.4 << std::endl;
+ // std::cout << d * 2 << std::endl;
+ // std::cout << d / 3 << std::endl;
+ // std::cout << (d - 3.4).abs().sqrLength() << std::endl;
+
+ // std::cout << (((d - 3.4).abs() + 1) + 3).sqrLength() << std::endl;
+ PRINT_VECTOR(a)
+ PRINT_VECTOR(b)
+ PRINT_VECTOR(min(a,b))
+ PRINT_VECTOR(max(a,b))
+ a.lbound(b);
+ PRINT_VECTOR(a)
+ std::cout << (a+1).lbound(b) << std::endl;
+ std::cout << (a+1).ubound(b) << std::endl;
+
+ std::cout << a.getRow(1) << std::endl;
+}
+
+BOOST_AUTO_TEST_CASE(fcl_eigen_matrix3fx)
+{
+ typedef Eigen::FclMatrix <double, 3, 0> Matrix3fX;
+ Matrix3fX a (0, 1, 2, 3, 4, 5, 6, 7, 8);
+
+ PRINT_MATRIX(a);
+ a.transpose ();
+ PRINT_MATRIX(a);
+ a += a;
+ PRINT_MATRIX(a);
+ a *= a;
+ PRINT_MATRIX(a);
+
+ Matrix3fX b = a; b.inverse ();
+ a += a + a * b;
+ PRINT_MATRIX(a);
+
+ b = a; b.inverse ();
+ a.transpose ();
+ PRINT_MATRIX(a);
+ PRINT_MATRIX(a.transposeTimes (b));
+ PRINT_MATRIX(a.timesTranspose (b));
+ PRINT_MATRIX(a.tensorTransform (b));
+}
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index e35c21d09755461ecafcb9c3f09df7d67dc8b5a9..18ee66bb85251e3c17665ae62172d21294053f79 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -37,6 +37,9 @@
#define BOOST_TEST_MODULE "FCL_MATH"
#include <boost/test/included/unit_test.hpp>
+#if FCL_HAVE_EIGEN
+#include <hpp/fcl/eigen/math_details.h>
+#endif
#if FCL_HAVE_SSE
#include <hpp/fcl/simd/math_simd_details.h>
#endif
@@ -163,6 +166,471 @@ BOOST_AUTO_TEST_CASE(vec_test_basic_vec64)
BOOST_CHECK(v1.dot(v2) == 26);
}
+#if FCL_HAVE_EIGEN
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32)
+{
+ typedef Vec3fX<details::eigen_wrapper_v3 <float> > Vec3f32;
+ Vec3f32 v1(1.0f, 2.0f, 3.0f);
+ BOOST_CHECK(v1[0] == 1.0f);
+ BOOST_CHECK(v1[1] == 2.0f);
+ BOOST_CHECK(v1[2] == 3.0f);
+
+ Vec3f32 v2 = v1;
+ Vec3f32 v3(3.3f, 4.3f, 5.3f);
+ v1 += v3;
+ BOOST_CHECK(v1.equal(v2 + v3));
+ v1 -= v3;
+ BOOST_CHECK(v1.equal(v2));
+ v1 -= v3;
+ BOOST_CHECK(v1.equal(v2 - v3));
+ v1 += v3;
+
+ v1 *= v3;
+ BOOST_CHECK(v1.equal(v2 * v3));
+ v1 /= v3;
+ BOOST_CHECK(v1.equal(v2));
+ v1 /= v3;
+ BOOST_CHECK(v1.equal(v2 / v3));
+ v1 *= v3;
+
+ v1 *= 2.0f;
+ BOOST_CHECK(v1.equal(v2 * 2.0f));
+ v1 /= 2.0f;
+ BOOST_CHECK(v1.equal(v2));
+ v1 /= 2.0f;
+ BOOST_CHECK(v1.equal(v2 / 2.0f));
+ v1 *= 2.0f;
+
+ v1 += 2.0f;
+ BOOST_CHECK(v1.equal(v2 + 2.0f));
+ v1 -= 2.0f;
+ BOOST_CHECK(v1.equal(v2));
+ v1 -= 2.0f;
+ BOOST_CHECK(v1.equal(v2 - 2.0f));
+ v1 += 2.0f;
+
+ BOOST_CHECK((-Vec3f32(1.0f, 2.0f, 3.0f)).equal(Vec3f32(-1.0f, -2.0f, -3.0f)));
+
+ v1 = Vec3f32(1.0f, 2.0f, 3.0f);
+ v2 = Vec3f32(3.0f, 4.0f, 5.0f);
+ BOOST_CHECK((v1.cross(v2)).equal(Vec3f32(-2.0f, 4.0f, -2.0f)));
+ BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
+
+ v1 = Vec3f32(3.0f, 4.0f, 5.0f);
+ BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64)
+{
+ typedef Vec3fX<details::eigen_wrapper_v3<double> > Vec3f64;
+ Vec3f64 v1(1.0, 2.0, 3.0);
+ BOOST_CHECK(v1[0] == 1.0);
+ BOOST_CHECK(v1[1] == 2.0);
+ BOOST_CHECK(v1[2] == 3.0);
+
+ Vec3f64 v2 = v1;
+ Vec3f64 v3(3.3, 4.3, 5.3);
+ v1 += v3;
+ BOOST_CHECK(v1.equal(v2 + v3));
+ v1 -= v3;
+ BOOST_CHECK(v1.equal(v2));
+ v1 -= v3;
+ BOOST_CHECK(v1.equal(v2 - v3));
+ v1 += v3;
+
+ v1 *= v3;
+ BOOST_CHECK(v1.equal(v2 * v3));
+ v1 /= v3;
+ BOOST_CHECK(v1.equal(v2));
+ v1 /= v3;
+ BOOST_CHECK(v1.equal(v2 / v3));
+ v1 *= v3;
+
+ v1 *= 2.0;
+ BOOST_CHECK(v1.equal(v2 * 2.0));
+ v1 /= 2.0;
+ BOOST_CHECK(v1.equal(v2));
+ v1 /= 2.0;
+ BOOST_CHECK(v1.equal(v2 / 2.0));
+ v1 *= 2.0;
+
+ v1 += 2.0;
+ BOOST_CHECK(v1.equal(v2 + 2.0));
+ v1 -= 2.0;
+ BOOST_CHECK(v1.equal(v2));
+ v1 -= 2.0;
+ BOOST_CHECK(v1.equal(v2 - 2.0));
+ v1 += 2.0;
+
+ BOOST_CHECK((-Vec3f64(1.0, 2.0, 3.0)).equal(Vec3f64(-1.0, -2.0, -3.0)));
+
+ v1 = Vec3f64(1.0, 2.0, 3.0);
+ v2 = Vec3f64(3.0, 4.0, 5.0);
+ BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+ BOOST_CHECK(std::abs(v1.dot(v2) - 26) < 1e-5);
+
+ v1 = Vec3f64(3.0, 4.0, 5.0);
+ BOOST_CHECK(std::abs(v1.sqrLength() - 50) < 1e-5);
+ BOOST_CHECK(std::abs(v1.length() - sqrt(50)) < 1e-5);
+ BOOST_CHECK(normalize(v1).equal(v1 / v1.length()));
+
+
+ v1 = Vec3f64(1.0, 2.0, 3.0);
+ v2 = Vec3f64(3.0, 4.0, 5.0);
+ BOOST_CHECK((v1.cross(v2)).equal(Vec3f64(-2.0, 4.0, -2.0)));
+ BOOST_CHECK(v1.dot(v2) == 26);
+}
+
+BOOST_AUTO_TEST_CASE(eigen_mat32_consistent)
+{
+ typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+ typedef Vec3fX<details::eigen_wrapper_v3 <float> > Vec3f32Eigen;
+
+ typedef Matrix3fX<details::Matrix3Data<details::Vec3Data<float> > > Matrix3f32;
+ typedef Matrix3fX<details::eigen_wrapper_m3<details::eigen_wrapper_v3<float> > > Matrix3f32Eigen;
+
+ Vec3f32 v1(1, 2, 3);
+ Vec3f32Eigen v2(1, 2, 3);
+
+ Matrix3f32 m1(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+ Matrix3f32Eigen m2(-1, 3, -3, 0, -6, 6, -5, -3, 1);
+
+ for(size_t i = 0; i < 3; ++i)
+ for(size_t j = 0; j < 3; ++j)
+ BOOST_CHECK((m1(i, j) - m2(i, j) < 1e-1));
+
+ Matrix3f32 m3(transpose(m1));
+ Matrix3f32Eigen m4(transpose(m2));
+
+ for(size_t i = 0; i < 3; ++i)
+ for(size_t j = 0; j < 3; ++j)
+ BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+ m3 = m1; m3.transpose();
+ m4 = m2; m4.transpose();
+
+ for(size_t i = 0; i < 3; ++i)
+ for(size_t j = 0; j < 3; ++j)
+ BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+ m3 = inverse(m1);
+ m4 = inverse(m2);
+
+ for(size_t i = 0; i < 3; ++i)
+ for(size_t j = 0; j < 3; ++j)
+ BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+
+ m3 = m1; m3.inverse();
+ m4 = m2; m4.inverse();
+
+ for(size_t i = 0; i < 3; ++i)
+ for(size_t j = 0; j < 3; ++j)
+ BOOST_CHECK((m3(i, j) - m4(i, j) < 1e-1));
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32_consistent)
+{
+ typedef Vec3fX<details::Vec3Data<float> > Vec3f32;
+ typedef Vec3fX<details::eigen_wrapper_v3<float> > Vec3f32Eigen;
+
+ Vec3f32 v1(3.4f, 4.2f, 10.5f), v2(3.1f, 0.1f, -50.4f);
+ Vec3f32Eigen v3(3.4f, 4.2f, 10.5f), v4(3.1f, 0.1f, -50.4f);
+ Vec3f32 v12 = v1 + v2;
+ Vec3f32Eigen v34 = v3 + v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 - v2;
+ v34 = v3 - v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 * v2;
+ v34 = v3 * v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 / v2;
+ v34 = v3 / v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ float t = 1234.433f;
+ v12 = v1 + t;
+ v34 = v3 + t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 - t;
+ v34 = v3 - t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 * t;
+ v34 = v3 * t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 / t;
+ v34 = v3 / t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1; v12 += v2;
+ v34 = v3; v34 += v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 -= v2;
+ v34 = v3; v34 -= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 *= v2;
+ v34 = v3; v34 *= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 /= v2;
+ v34 = v3; v34 /= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 += t;
+ v34 = v3; v34 += t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 -= t;
+ v34 = v3; v34 -= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 *= t;
+ v34 = v3; v34 *= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 /= t;
+ v34 = v3; v34 /= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = -v1;
+ v34 = -v3;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1.cross(v2);
+ v34 = v3.cross(v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ // Difference if about 6e-5
+ BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-4);
+
+ v12 = min(v1, v2);
+ v34 = min(v3, v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = max(v1, v2);
+ v34 = max(v3, v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = abs(v2);
+ v34 = abs(v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ Vec3f32 delta1(1e-9f, 1e-9f, 1e-9f);
+ Vec3f32Eigen delta2(1e-9f, 1e-9f, 1e-9f);
+ BOOST_CHECK((v1 + delta1).equal(v1));
+ BOOST_CHECK((v3 + delta2).equal(v3));
+
+ BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+
+ v12 = v1; v12.negate();
+ v34 = v3; v34.negate();
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1; v12.normalize();
+ v34 = v3; v34.normalize();
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = normalize(v1);
+ v34 = normalize(v3);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+}
+
+BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64_consistent)
+{
+ typedef Vec3fX<details::Vec3Data<double> > Vec3f64;
+ typedef Vec3fX<details::eigen_wrapper_v3<double> > Vec3f64Eigen;
+
+ Vec3f64 v1(3.4, 4.2, 10.5), v2(3.1, 0.1, -50.4);
+ Vec3f64Eigen v3(3.4, 4.2, 10.5), v4(3.1, 0.1, -50.4);
+ Vec3f64 v12 = v1 + v2;
+ Vec3f64Eigen v34 = v3 + v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 - v2;
+ v34 = v3 - v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 * v2;
+ v34 = v3 * v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 / v2;
+ v34 = v3 / v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ double t = 1234.433;
+ v12 = v1 + t;
+ v34 = v3 + t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 - t;
+ v34 = v3 - t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 * t;
+ v34 = v3 * t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1 / t;
+ v34 = v3 / t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1; v12 += v2;
+ v34 = v3; v34 += v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 -= v2;
+ v34 = v3; v34 -= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 *= v2;
+ v34 = v3; v34 *= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 /= v2;
+ v34 = v3; v34 /= v4;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 += t;
+ v34 = v3; v34 += t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 -= t;
+ v34 = v3; v34 -= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 *= t;
+ v34 = v3; v34 *= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = v1; v12 /= t;
+ v34 = v3; v34 /= t;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = -v1;
+ v34 = -v3;
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1.cross(v2);
+ v34 = v3.cross(v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ BOOST_CHECK(std::abs(v1.dot(v2) - v3.dot(v4)) < 1e-5);
+
+ v12 = min(v1, v2);
+ v34 = min(v3, v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+ v12 = max(v1, v2);
+ v34 = max(v3, v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = abs(v2);
+ v34 = abs(v4);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ Vec3f64 delta1(1e-15, 1e-15, 1e-15);
+ Vec3f64Eigen delta2(1e-15, 1e-15, 1e-15);
+ BOOST_CHECK((v1 + delta1).equal(v1));
+ BOOST_CHECK((v3 + delta2).equal(v3));
+
+ BOOST_CHECK(std::abs(v1.length() - v3.length()) < 1e-5);
+ BOOST_CHECK(std::abs(v1.sqrLength() - v3.sqrLength()) < 1e-5);
+
+ v12 = v1; v12.negate();
+ v34 = v3; v34.negate();
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = v1; v12.normalize();
+ v34 = v3; v34.normalize();
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+
+ v12 = normalize(v1);
+ v34 = normalize(v3);
+ BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
+ BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
+}
+
+#endif
+
+
#if FCL_HAVE_SSE
BOOST_AUTO_TEST_CASE(vec_test_sse_vec32)