From 25a89cca250923b32ce6beee009addb5b8712dc8 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 15 Mar 2016 19:05:42 +0100
Subject: [PATCH] Clean code and fix bugs. Test are passing.
---
CMakeLists.txt | 2 +-
include/hpp/fcl/BV/BV.h | 2 +-
include/hpp/fcl/ccd/interval_matrix.h | 4 +
include/hpp/fcl/ccd/interval_vector.h | 4 +
include/hpp/fcl/ccd/taylor_vector.h | 4 +
.../fcl/eigen/plugins/ccd/interval-matrix.h | 0
.../fcl/eigen/plugins/ccd/interval-vector.h | 7 +
include/hpp/fcl/eigen/taylor_operator.h | 25 ++
include/hpp/fcl/eigen/vec_3fx.h | 221 +++++++++++-------
include/hpp/fcl/math/matrix_3f.h | 4 +-
src/narrowphase/narrowphase.cpp | 22 +-
test/CMakeLists.txt | 1 -
12 files changed, 193 insertions(+), 103 deletions(-)
create mode 100644 include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
create mode 100644 include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
create mode 100644 include/hpp/fcl/eigen/taylor_operator.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9458a656..e236d612 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,7 +50,7 @@ set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
set(FCL_USE_NATIVE_EIGEN FALSE CACHE BOOL "Use native eigen matrix type when possible")
add_optional_dependency("eigen3 >= 3.0.0")
-set(FCL_HAVE_EIGEN EIGEN3_FOUND CACHE BOOL "Use eigen wrappers")
+set(FCL_HAVE_EIGEN ${EIGEN3_FOUND} CACHE BOOL "Use eigen wrappers")
if (EIGEN3_FOUND)
if (FCL_HAVE_EIGEN)
include_directories(${EIGEN3_INCLUDE_DIRS})
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index d8118640..957672fd 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/ccd/interval_matrix.h b/include/hpp/fcl/ccd/interval_matrix.h
index f828387a..6ad14b0d 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 f5b71b64..7c1216b9 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/taylor_vector.h b/include/hpp/fcl/ccd/taylor_vector.h
index a17c262a..e3ca9466 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_USE_NATIVE_EIGEN
+#include <hpp/fcl/eigen/taylor_operator.h>
+#endif
+
namespace fcl
{
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 00000000..e69de29b
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 00000000..18811c7c
--- /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.derived();
+ setValue (tmp);
+ return *this;
+}
diff --git a/include/hpp/fcl/eigen/taylor_operator.h b/include/hpp/fcl/eigen/taylor_operator.h
new file mode 100644
index 00000000..15351e0b
--- /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.derived();
+ 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
index 5445bb69..94a9829d 100644
--- a/include/hpp/fcl/eigen/vec_3fx.h
+++ b/include/hpp/fcl/eigen/vec_3fx.h
@@ -41,12 +41,16 @@
#include <hpp/fcl/config-fcl.hh>
#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/eigen/math_details.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) \
@@ -114,6 +118,26 @@
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,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& derived () { return static_cast<Derived&> (*this); }
+ const Derived& derived () const { return static_cast<const Derived&> (*this); }
+};
+}
+
namespace Eigen {
template <typename T> class FclOp;
template <typename T, int Cols, int Options> class FclMatrix;
@@ -123,24 +147,6 @@ namespace Eigen {
namespace internal {
template<typename T> struct traits< FclOp<T> > : traits <T> {};
- // template<typename T>
- // struct traits< FclOp<T> >
- // {
- // typedef T Scalar;
- // typedef FclOp<T> This;
- // typedef traits<typename This::Base> traits_base;
- // typedef typename traits_base::StorageKind StorageKind;
- // typedef typename traits_base::Index Index;
- // typedef typename traits_base::XprKind XprKind;
- // enum {
- // RowsAtCompileTime = traits_base::RowsAtCompileTime,
- // ColsAtCompileTime = traits_base::ColsAtCompileTime,
- // MaxRowsAtCompileTime = traits_base::MaxRowsAtCompileTime,
- // MaxColsAtCompileTime = traits_base::MaxColsAtCompileTime,
- // Flags = traits_base::Flags,
- // CoeffReadCost = traits_base::CoeffReadCost
- // };
- // };
template<typename T, int Cols, int _Options>
struct traits< FclMatrix<T, Cols, _Options> >
{
@@ -197,13 +203,14 @@ namespace Eigen {
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>
-class FclMatrix : public 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;
@@ -213,7 +220,11 @@ public:
FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstColXpr)
FCL_EIGEN_EXPOSE_PARENT_TYPE(ConstRowXpr)
- FclMatrix(void): Base() {}
+ // 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>
@@ -239,6 +250,16 @@ public:
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.derived();
+ this->row(1) = r1.derived();
+ this->row(2) = r2.derived();
+ }
+
/// @brief create vector (x, x, x)
FclMatrix(T x) : Base(Base::Constant (x)) {}
@@ -259,6 +280,8 @@ public:
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.
@@ -283,10 +306,10 @@ public:
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<FclMatrix>::Opposite operator - () const { return typename UnaryReturnType<FclMatrix>::Opposite(*this); }
+ inline const typename UnaryReturnType<FclMatrix>::Opposite operator-() const { return typename UnaryReturnType<FclMatrix>::Opposite(*this); }
// There is no class for cross
// 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); }
+ FCL_EIGEN_MAKE_CROSS()
inline FclMatrix& normalize()
{
T sqr_length = this->squaredNorm();
@@ -299,7 +322,7 @@ public:
T sqr_length = this->squaredNorm();
if(sqr_length > 0)
{
- this->operator/= ((T)sqrt(sqr_length));
+ this->operator/= ((T)std::sqrt(sqr_length));
*signal = true;
}
else
@@ -405,13 +428,58 @@ public:
const typename FclProduct<const FclMatrix,const OtherDerived>::ProductType left (*this, other.derived());
return typename FclProduct<const FclMatrix,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
+class FclOp : public EigenOp, public ::fcl::FclType<FclOp <EigenOp> >
{
public:
typedef typename internal::traits<EigenOp>::Scalar T;
@@ -430,6 +498,10 @@ public:
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) {}
@@ -437,14 +509,13 @@ public:
Base& base () { return *this; }
const Base& base () const { return *this; }
- // template<typename Lhs, typename Rhs, typename BinaryOp>
- // FclOp (const CwiseBinaryOp<BinaryOp, Lhs, Rhs>& o)
- // : Base (o.lhs(), o.rhs(), o.functor()) {}
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.
@@ -461,6 +532,9 @@ public:
}
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 FclOp>::Opposite operator-() const { return typename UnaryReturnType<const FclOp>::Opposite(*this); }
+
+ FCL_EIGEN_MAKE_CROSS()
inline const typename UnaryReturnType<EigenOp>::Normalize
normalize() const
@@ -492,19 +566,8 @@ public:
return typename UnaryReturnType<EigenOp>::Abs (this->derived());
}
- 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); }
- /* Useless
- inline void setValue(T x, T y, T z) {
- this->m_storage.data()[0] = x;
- this->m_storage.data()[1] = y;
- this->m_storage.data()[2] = z;
- }
- inline void setValue(T x) { this->setConstant (x); }
- inline void setZero () {data.setValue (0); }
- //*/
+ 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
@@ -536,11 +599,7 @@ public:
bool isZero() const
{
return this->isZero ();
- // (this->m_storage.data()[0] == 0)
- // && (this->m_storage.data()[1] == 0)
- // && (this->m_storage.data()[2] == 0);
}
- // */
const FclOp<Transpose<const FclOp> > transpose () const {
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(EigenOp, 3, 3);
@@ -549,6 +608,7 @@ public:
// const FclOp<internal::inverse_impl<FclOp> > inverse () const { return FclOp<Transpose<FclOp> >(*this); }
};
+
}
namespace fcl
@@ -564,54 +624,49 @@ static inline Eigen::FclMatrix<T, 1, _Options> normalize(const Eigen::FclMatrix<
return v;
}
-template<typename T, int _Options>
-static inline T triple(
- const Eigen::FclMatrix<T, 1, _Options>& x,
- const Eigen::FclMatrix<T, 1, _Options>& y,
- const Eigen::FclMatrix<T, 1, _Options>& z)
+template<typename Derived>
+static inline typename Derived::Scalar triple(
+ const FclType<Derived>& x,
+ const FclType<Derived>& y,
+ const FclType<Derived>& z)
{
- return x.dot(y.cross(z));
+ return x.derived().dot(y.derived().cross(z.derived()));
}
-// template<typename T, int _Options>
-// std::ostream& operator << (std::ostream& out, const Eigen::FclMatrix<T>& x)
-// {
- // out << x[0] << " " << x[1] << " " << x[2];
- // return out;
-// }
-template<typename T, int _Options>
-static inline const typename Eigen::BinaryReturnType<
- const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options>
- >::Min
- min(const Eigen::FclMatrix<T, 1, _Options>& x, const Eigen::FclMatrix<T, 1, _Options>& y)
+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 Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Min (x, y);
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min (x.derived(), y.derived());
}
-template<typename T, int _Options>
-static inline const typename Eigen::BinaryReturnType<
- const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options>
- >::Max
- max(const Eigen::FclMatrix<T, 1, _Options>& x, const Eigen::FclMatrix<T, 1, _Options>& y)
+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 Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Max (x, y);
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max (x.derived(), y.derived());
}
-template<typename T, int _Cols, int _Options>
-// static inline Vec3fX<T> abs(const Vec3fX<T>& x)
-static inline const typename Eigen::UnaryReturnType<const Eigen::FclMatrix<T, _Cols, _Options> >
-abs(const Eigen::FclMatrix<T, _Cols, _Options>& x)
+template<typename Derived>
+static inline const typename Eigen::UnaryReturnType<const Derived>::Abs
+abs(const FclType<Derived>& x)
{
- return typename Eigen::UnaryReturnType<const Eigen::FclMatrix<T, _Cols, _Options> >::Abs (x);
+ return typename Eigen::UnaryReturnType<const Derived>::Abs (x.derived());
}
-template<typename T, int _Options>
+template<typename Derived>
void generateCoordinateSystem(
- const Eigen::FclMatrix<T, 1, _Options>& w,
- const Eigen::FclMatrix<T, 1, _Options>& u,
- const Eigen::FclMatrix<T, 1, _Options>& v)
+ FclType<Derived>& _w,
+ FclType<Derived>& _u,
+ FclType<Derived>& _v)
{
+ typedef typename Derived::Scalar T;
+
+ Derived& w = _w.derived();
+ Derived& u = _u.derived();
+ Derived& v = _v.derived();
+
T inv_length;
if(std::abs(w[0]) >= std::abs(w[1]))
{
@@ -662,10 +717,10 @@ void relativeTransform(const Matrix& R1, const Vector& 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 Matrix& m, typename Matrix::Scalar dout[3], Vector vout[3])
+void eigen(const FclType<Matrix>& m, typename Matrix::Scalar dout[3], Vector* vout)
{
typedef typename Matrix::Scalar Scalar;
- Matrix R(m);
+ Matrix R(m.derived());
int n = 3;
int j, iq, ip, i;
Scalar tresh, theta, tau, t, sm, s, h, g, c;
@@ -750,12 +805,6 @@ void eigen(const Matrix& m, typename Matrix::Scalar dout[3], Vector vout[3])
return;
}
-// template<typename >
-// Matrix abs(const Matrix& R)
-// {
- // return R.abs());
-// }
-
template<typename T, int _Options>
Eigen::FclOp<Eigen::Transpose<const Eigen::FclMatrix<T,3,_Options> > > transpose(const Eigen::FclMatrix<T, 3, _Options>& R)
{
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index f90476b6..b43b5d6c 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -40,9 +40,7 @@
#include <hpp/fcl/math/vec_3f.h>
-#if FCL_USE_NATIVE_EIGEN
-# include <hpp/fcl/eigen/matrix_3fx.h>
-#else
+#if ! FCL_USE_NATIVE_EIGEN
# include <hpp/fcl/math/matrix_3fx.h>
#endif
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 4d2eb0a3..c5c1c4b1 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];
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 6bb87269..ed1da73f 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -46,4 +46,3 @@ if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
-add_fcl_template_test(test_fcl_eigen test_fcl_eigen.cpp)
--
GitLab