diff --git a/include/hpp/fcl/eigen/math_details.h b/include/hpp/fcl/eigen/math_details.h
index 207e797d4e5045e814405fd9ae6755bb04ed029f..c1ed7fd54feb04ab08360dd7423b1a4d7ea12fb7 100644
--- a/include/hpp/fcl/eigen/math_details.h
+++ b/include/hpp/fcl/eigen/math_details.h
@@ -436,6 +436,7 @@ static inline bool equal(const eigen_v3<T>& x, const eigen_v3<T>& y, T epsilon)
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;
@@ -477,19 +478,19 @@ struct eigen_wrapper_m3
inline eigen_wrapper_m3<T> operator + (const eigen_wrapper_m3<T>& other) const { return eigen_wrapper_m3<T>(m + other.m); }
inline eigen_wrapper_m3<T> operator - (const eigen_wrapper_m3<T>& other) const { return eigen_wrapper_m3<T>(m - other.m); }
- inline eigen_wrapper_m3<T> operator + (meta_type c) const { return eigen_wrapper_m3<T>(m + c); }
- inline eigen_wrapper_m3<T> operator - (meta_type c) const { return eigen_wrapper_m3<T>(m - c); }
- inline eigen_wrapper_m3<T> operator * (meta_type c) const { return eigen_wrapper_m3<T>(m * c); }
- inline eigen_wrapper_m3<T> operator / (meta_type c) const { return eigen_wrapper_m3<T>(m / c); }
+ inline eigen_wrapper_m3<T> operator + (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 += (meta_type c) { m.array() += c; return *this; }
- inline eigen_wrapper_m3<T>& operator -= (meta_type c) { m.array() -= c; return *this; }
- inline eigen_wrapper_m3<T>& operator *= (meta_type c) { m.array() *= c; return *this; }
- inline eigen_wrapper_m3<T>& operator /= (meta_type c) { m.array() /= c; return *this; }
+ 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 (); }
@@ -678,14 +679,14 @@ struct eigen_m3 :
}
template <typename OtherDerived>
- const Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+ 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 Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
+ const typename Eigen::ProductReturnType<eigen_m3<T>, OtherDerived>::Type
timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
{
return this->operator* (other.Base::transpose());
diff --git a/include/hpp/fcl/eigen/matrix_3fx.h b/include/hpp/fcl/eigen/matrix_3fx.h
index 8e9356ce94043b8f9ed709ff4ebbbc8998add19e..794924cf4d05be7a9cbb596fbc5062862d0352b0 100644
--- a/include/hpp/fcl/eigen/matrix_3fx.h
+++ b/include/hpp/fcl/eigen/matrix_3fx.h
@@ -43,35 +43,52 @@ namespace fcl
/// @brief Matrix2 class wrapper. the core data is in the template parameter class
template<typename T>
-class Matrix3fX
- : Eigen::Matrix <T, 3, 1>
+class Matrix3fX :
+ public Eigen::Matrix <T, 3, 3, Eigen::RowMajor>
{
public:
- // typedef typename T::vector_type S;
-
- 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)
- {}
+ typedef Eigen::Matrix <T, 3, 3, Eigen::RowMajor> Base;
+ typedef typename Base::ColXpr ColXpr;
+ typedef typename Base::ConstColXpr ConstColXpr;
+ typedef typename Base::RowXpr RowXpr;
+ typedef typename Base::ConstRowXpr ConstRowXpr;
- 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(void): Base() {}
- Matrix3fX(const T& data_) : data(data_) {}
-
- inline Vec3fX<S> getColumn(size_t i) const
- {
- return Vec3fX<S>(data.getColumn(i));
- }
+ // This constructor allows you to construct MyVectorType from Eigen expressions
+ template<typename OtherDerived>
+ Matrix3fX(const Eigen::MatrixBase<OtherDerived>& other)
+ : Base(other)
+ {}
- inline Vec3fX<S> getRow(size_t i) const
- {
- return Vec3fX<S>(data.getRow(i));
- }
+ // This method allows you to assign Eigen expressions to MyVectorType
+ template<typename OtherDerived>
+ Matrix3fX& operator=(const Eigen::MatrixBase <OtherDerived>& other)
+ {
+ this->Base::operator=(other);
+ return *this;
+ }
+ Matrix3fX(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
+ : Base((Base () << xx, xy, xz, yx, yy, yz, zx, zy, zz).finished ())
+ {}
+
+ template <typename OtherDerived>
+ Matrix3fX(const Eigen::MatrixBase<OtherDerived>& v1,
+ const Eigen::MatrixBase<OtherDerived>& v2,
+ const Eigen::MatrixBase<OtherDerived>& v3)
+ : Base((Base () << v1, v2, v3).finished ())
+ {}
+
+ 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 U operator () (size_t i, size_t j) const
{
return data(i, j);
@@ -168,6 +185,7 @@ public:
{
data.setIdentity();
}
+ // */
inline bool isIdentity () const
{
@@ -177,10 +195,12 @@ public:
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
@@ -218,150 +238,175 @@ public:
setEulerZYX(roll, pitch, yaw);
}
+ /*
inline U determinant() const
{
return data.determinant();
}
+ // */
Matrix3fX<T>& transpose()
{
- data.transpose();
+ this->transposeInPlace();
return *this;
}
Matrix3fX<T>& inverse()
{
- data.inverse();
+ *this = this->inverse().eval ();
return *this;
}
Matrix3fX<T>& abs()
{
- data.abs();
+ *this = this->cwiseAbs ();
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);
+ static const Matrix3fX<T> I((T)1, (T)0, (T)0,
+ (T)0, (T)1, (T)0,
+ (T)0, (T)0, (T)1);
return I;
}
- Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+ template<typename OtherDerived>
+ const typename Eigen::ProductReturnType< Eigen::Transpose <Matrix3fX<T> >,
+ OtherDerived>::Type
+ transposeTimes(const Eigen::MatrixBase<OtherDerived>& other) const
{
- return Matrix3fX<T>(data.transposeTimes(other.data));
+ return this->Base::transpose() * other;
}
- Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+ template<typename OtherDerived>
+ const typename Eigen::ProductReturnType< Matrix3fX<T>,
+ Eigen::Transpose <OtherDerived> >
+ ::Type timesTranspose(const Eigen::MatrixBase<OtherDerived>& other) const
{
- return Matrix3fX<T>(data.timesTranspose(other.data));
+ return *this * other.transpose ();
}
+ /*
Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
{
return Vec3fX<S>(data.transposeTimes(v.data));
}
+ // */
- Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+ template<typename OtherDerived>
+ const Eigen::ProductReturnType<
+ Eigen::ProductReturnType< Matrix3fX<T>,
+ OtherDerived>::Type,
+ Eigen::Transpose < Matrix3fX<T> >
+ > ::Type
+ tensorTransform(const Eigen::MatrixBase<OtherDerived>& m) const
{
- Matrix3fX<T> res(*this);
- res *= m;
- return res.timesTranspose(*this);
+ return (*this * m) * this->Base::transpose ();
+ // 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
+ template<typename OtherDerived>
+ inline T transposeDotX(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.transposeDot(0, v.data);
+ return transposeDot(0, v);
}
// (0 1 0)^T (*this)^T v
- inline U transposeDotY(const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T transposeDotY(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.transposeDot(1, v.data);
+ return transposeDot(1, v);
}
// (0 0 1)^T (*this)^T v
- inline U transposeDotZ(const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T transposeDotZ(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.transposeDot(2, v.data);
+ return transposeDot(2, v);
}
// (\delta_{i3})^T (*this)^T v
- inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T transposeDot(size_t i, const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.transposeDot(i, v.data);
+ return this->col(i).dot(v);
}
// (1 0 0)^T (*this) v
- inline U dotX(const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T dotX(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.dot(0, v.data);
+ return dot(0, v);
}
// (0 1 0)^T (*this) v
- inline U dotY(const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T dotY(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.dot(1, v.data);
+ return dot(1, v);
}
// (0 0 1)^T (*this) v
- inline U dotZ(const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T dotZ(const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.dot(2, v.data);
+ return dot(2, v);
}
// (\delta_{i3})^T (*this) v
- inline U dot(size_t i, const Vec3fX<S>& v) const
+ template<typename OtherDerived>
+ inline T dot(size_t i, const Eigen::MatrixBase<OtherDerived>& v) const
{
- return data.dot(i, v.data);
+ return this->row(i).dot(v);
}
- inline void setValue(U xx, U xy, U xz,
- U yx, U yy, U yz,
- U zx, U zy, U zz)
+ inline void setValue(T xx, T xy, T xz,
+ T yx, T yy, T yz,
+ T zx, T zy, T zz)
{
- data.setValue(xx, xy, xz,
- yx, yy, yz,
- zx, zy, zz);
+ (*this)(0,0) = xx; (*this)(0,1) = xy; (*this)(0,2) = xz;
+ (*this)(1,0) = yx; (*this)(1,1) = yy; (*this)(1,2) = yz;
+ (*this)(2,0) = zx; (*this)(2,1) = zy; (*this)(2,2) = zz;
}
- inline void setValue(U x)
+ inline void setValue(T x)
{
- data.setValue(x);
+ this->setConstant (x);
}
};
-template<typename T>
-void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
+template<typename T, typename OtherDerived>
+void hat(Matrix3fX<T>& mat, const Eigen::MatrixBase<OtherDerived>& 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)
+template<typename T, typename OtherDerived>
+void relativeTransform(const Matrix3fX<T>& R1, const Eigen::MatrixBase<OtherDerived>& t1,
+ const Matrix3fX<T>& R2, const Eigen::MatrixBase<OtherDerived>& t2,
+ Matrix3fX<T>& R, Eigen::MatrixBase<OtherDerived>& 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])
+template<typename T, typename Derived>
+void eigen(const Matrix3fX<T>& m, T dout[3], const Eigen::MatrixBase<Derived> 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;
+ T 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];
+ T b[3];
+ T z[3];
+ T v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ T d[3];
for(ip = 0; ip < n; ++ip)
{
@@ -439,25 +484,26 @@ void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename
}
template<typename T>
-Matrix3fX<T> abs(const Matrix3fX<T>& R)
+const typename CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Matrix3fX<T> >
+abs(const Matrix3fX<T>& R)
{
- return Matrix3fX<T>(abs(R.data));
+ return R.cwiseAbs ();
}
template<typename T>
-Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+typename Eigen::Transpose <Matrix3fX<T> > transpose(const Matrix3fX<T>& R)
{
- return Matrix3fX<T>(transpose(R.data));
+ return R.Matrix3fX<T>::Base::transpose ();
}
template<typename T>
Matrix3fX<T> inverse(const Matrix3fX<T>& R)
{
- return Matrix3fX<T>(inverse(R.data));
+ return R.Matrix3fX<T>::Base::inverse ().eval ();
}
-template<typename T>
-typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+template<typename T, typename Derived>
+T quadraticForm(const Matrix3fX<T>& R, const Eigen::MatrixBase<Derived>& v)
{
return v.dot(R * v);
}
diff --git a/include/hpp/fcl/eigen/product.h b/include/hpp/fcl/eigen/product.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c73f47c24f7f4f554386bed981eea000fa09a45
--- /dev/null
+++ b/include/hpp/fcl/eigen/product.h
@@ -0,0 +1,36 @@
+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 FclOp<typename internal::deduce_fcl_type<Derived, OtherDerived, COEFWISE>::Type> ProductType;
+ typedef FclOp<typename ProductReturnType<Transpose<Derived>, OtherDerived >::Type> TransposeTimesType;
+ typedef FclOp<typename ProductReturnType<Derived, Transpose<OtherDerived> >::Type> TimesTransposeType;
+ typedef FclOp<typename ProductReturnType<ProductType, Transpose<Derived> >::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 FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::ProductType \
+ operator*(const MatrixBase<OtherDerived>& other) const \
+ { \
+ return FclProduct<const FCL_EIGEN_CURRENT_CLASS, const OtherDerived>::run (*this, other.derived()); \
+ }
diff --git a/include/hpp/fcl/eigen/vec_3fx.h b/include/hpp/fcl/eigen/vec_3fx.h
index da68282678a4cc779b9eacdeee353f2a3aeedfda..5445bb69b5c1287656f70ef9ffdccdc58c641889 100644
--- a/include/hpp/fcl/eigen/vec_3fx.h
+++ b/include/hpp/fcl/eigen/vec_3fx.h
@@ -47,39 +47,203 @@
#include <iostream>
#include <limits>
+#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 FCL_EIGEN_CURRENT_CLASS, const OtherDerived> > \
+ (METHOD) (const MatrixBase<OtherDerived>& other) const \
+ { \
+ return FclOp <const CwiseBinaryOp<FUNCTOR<Scalar>, const FCL_EIGEN_CURRENT_CLASS, const OtherDerived> > (*this, other.derived()); \
+ }
+
+#define FCL_EIGEN_MAKE_CWISE_UNARY_OP(METHOD,FUNCTOR) \
+ EIGEN_STRONG_INLINE const FclOp<const CwiseUnaryOp<FUNCTOR<Scalar>, const FCL_EIGEN_CURRENT_CLASS> > \
+ (METHOD) (const Scalar& scalar) const \
+ { \
+ return FclOp <const CwiseUnaryOp<FUNCTOR<Scalar>, const FCL_EIGEN_CURRENT_CLASS> > (*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)
+
+namespace Eigen {
+ template <typename T> class FclOp;
+ template <typename T, int Cols, int Options> class FclMatrix;
+
+#include <hpp/fcl/eigen/product.h>
+
+ 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> >
+ {
+ typedef T Scalar;
+ typedef FclMatrix<T, Cols, _Options> 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,
+ Options = traits_base::Options,
+ InnerStrideAtCompileTime = 1,
+ OuterStrideAtCompileTime = traits_base::OuterStrideAtCompileTime
+ };
+ };
+ }
-namespace fcl
-{
+ 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;
+ };
+
+#define FCL_EIGEN_CURRENT_CLASS FclMatrix
/// @brief Vector3 class wrapper. The core data is in the template parameter class.
-template <typename T>
-class Vec3fX :
- Eigen::Matrix <T, 3, 1>
+template <typename T, int Cols, int _Options>
+class FclMatrix : public Matrix <T, 3, Cols, _Options>
{
public:
- typedef Eigen::Matrix <T, 3, 1> Base;
+ 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)
- Vec3fX(void): Base() {}
+ FclMatrix(void): Base() {}
// This constructor allows you to construct MyVectorType from Eigen expressions
template<typename OtherDerived>
- Vec3fX(const Eigen::MatrixBase<OtherDerived>& other)
+ FclMatrix(const MatrixBase<OtherDerived>& other)
: Base(other)
{}
// This method allows you to assign Eigen expressions to MyVectorType
template<typename OtherDerived>
- Vec3fX& operator=(const Eigen::MatrixBase <OtherDerived>& other)
+ FclMatrix& operator=(const MatrixBase <OtherDerived>& other)
{
this->Base::operator=(other);
return *this;
}
/// @brief create Vector (x, y, z)
- Vec3fX(T x, T y, T z) : Base(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);
+ }
/// @brief create vector (x, x, x)
- Vec3fX(U x) : Base(Base::Constant (x)) {}
+ FclMatrix(T x) : Base(Base::Constant (x)) {}
+
+ Base& base () { return *this; }
+ const Base& base () const { return *this; }
/// @brief create vector using the internal data type
// Vec3fX(const T& data_) : data(data_) {}
@@ -87,33 +251,50 @@ public:
// 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); }
+ // 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_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 FclMatrix> >
+ operator- (const Scalar& scalar) const
+ {
+ return FclOp <const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const FclMatrix> > (*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<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); }
- inline Vec3fX& normalize()
+ inline FclMatrix& normalize()
{
T sqr_length = this->squaredNorm();
- if(sqr_length > 0) this->operator/= ((T)sqrt(sqr_length));
+ if(sqr_length > 0) this->operator/= ((T)std::sqrt(sqr_length));
return *this;
}
- inline Vec3fX& normalize(bool* signal)
+ inline FclMatrix& normalize(bool* signal)
{
T sqr_length = this->squaredNorm();
if(sqr_length > 0)
@@ -126,7 +307,7 @@ public:
return *this;
}
- inline Vec3fX& abs()
+ inline FclMatrix& abs()
{
*this = this->cwiseAbs ();
return *this;
@@ -136,33 +317,46 @@ public:
// 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) { m_storage.data() = { x, y, z } }
+ 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 Vec3fX& other, T epsilon = std::numeric_limits<U>::epsilon() * 100) const
+ inline bool equal(const FclMatrix& other, T epsilon = std::numeric_limits<T>::epsilon() * 100) const
{
return ((*this - other).cwiseAbs().array () < epsilon).all();
}
- inline Vec3fX<T>& negate() { *this = -*this; return *this; }
+ inline FclMatrix& negate() { *this = -*this; return *this; }
- bool operator == (const Vec3fX& other) const
+ bool operator == (const FclMatrix& other) const
{
return equal(other, 0);
}
- bool operator != (const Vec3fX& other) const
+ bool operator != (const FclMatrix& other) const
{
return !(*this == other);
}
-
- inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
+ inline FclMatrix& ubound(const FclMatrix& u)
{
*this = this->cwiseMin (u);
return *this;
}
- inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
+ inline FclMatrix& lbound(const FclMatrix& l)
{
*this = this->cwiseMax (l);
return *this;
@@ -170,73 +364,260 @@ public:
bool isZero() const
{
- return (m_storage.data()[0] == 0)
- && (m_storage.data()[1] == 0)
- && (m_storage.data()[2] == 0);
+ 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 FclMatrix,const OtherDerived>::TransposeTimesType
+ transposeTimes (const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const FclMatrix> t (*this);
+ return typename FclProduct<const FclMatrix,const OtherDerived>::TransposeTimesType (t, other.derived());
}
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename FclProduct<const FclMatrix,const OtherDerived>::TimesTransposeType
+ timesTranspose (const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const OtherDerived> t (other.derived());
+ return typename FclProduct<const FclMatrix,const OtherDerived>::TimesTransposeType (*this, t);
+ }
+
+ template<typename OtherDerived>
+ EIGEN_STRONG_INLINE const typename FclProduct<const FclMatrix,const OtherDerived>::TensorTransformType
+ tensorTransform(const MatrixBase<OtherDerived>& other) const
+ {
+ const Transpose<const FclMatrix> t (*this);
+ const typename FclProduct<const FclMatrix,const OtherDerived>::ProductType left (*this, other.derived());
+ return typename FclProduct<const FclMatrix,const OtherDerived>::TensorTransformType (left, t);
+ }
};
-template<typename T>
-static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
+#undef FCL_EIGEN_CURRENT_CLASS
+#define FCL_EIGEN_CURRENT_CLASS FclOp
+
+template <typename EigenOp>
+class FclOp : public EigenOp
{
- typename T::meta_type sqr_length = v.squaredNorm ();
+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 XprType>
+ FclOp (XprType& xpr)
+ : Base (xpr) {}
+
+ 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_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 FclOp> >
+ operator- (const Scalar& scalar) const
+ {
+ return FclOp <const CwiseUnaryOp<internal::scalar_add_op<Scalar>, const FclOp> > (*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<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->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); }
+ //*/
+
+ 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->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);
+ return FclOp<Transpose<const FclOp> >(*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 / (typename T::meta_type)sqrt(sqr_length);
+ return v / (T)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)
+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)
{
return x.dot(y.cross(z));
}
-template <typename T>
-std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
+// 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)
{
- out << x[0] << " " << x[1] << " " << x[2];
- return out;
+ return typename Eigen::BinaryReturnType<const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Min (x, y);
}
-template <typename T>
-// static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
-static inline
-const Eigen::CwiseBinaryOp <Eigen::internal::scalar_min_op<T>, const Vec3fX<T>, const Vec3fX<T> >
- min(const Vec3fX<T>& x, const Vec3fX<T>& y)
+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)
{
- return x.cwiseMin (y);
+ return typename Eigen::BinaryReturnType<const Eigen::FclMatrix<T, 1, _Options>, const Eigen::FclMatrix<T, 1, _Options> >::Max (x, y);
}
-template <typename T>
-// static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
-static inline
-const Eigen::CwiseBinaryOp<Eigen::internal::scalar_max_op<T>, const Vec3fX<T>, const Vec3fX<T> >
-max(const Vec3fX<T>& x, const Vec3fX<T>& y)
-{
- return x.cwiseMax (y);
-}
-
-template <typename T>
+template<typename T, int _Cols, int _Options>
// static inline Vec3fX<T> abs(const Vec3fX<T>& x)
-static inline
-const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<T>, const 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)
{
- return x.cwiseAbs ();
+ return typename Eigen::UnaryReturnType<const Eigen::FclMatrix<T, _Cols, _Options> >::Abs (x);
}
-template <typename T>
-void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
+template<typename T, int _Options>
+void generateCoordinateSystem(
+ const Eigen::FclMatrix<T, 1, _Options>& w,
+ const Eigen::FclMatrix<T, 1, _Options>& u,
+ const Eigen::FclMatrix<T, 1, _Options>& v)
{
- typedef typename T::meta_type U;
- U inv_length;
+ T 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]);
+ inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
u[0] = -w[2] * inv_length;
- u[1] = (U)0;
+ 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];
@@ -244,8 +625,8 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
}
else
{
- inv_length = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = (U)0;
+ 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];
@@ -261,6 +642,138 @@ void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, 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 Matrix& m, typename Matrix::Scalar dout[3], Vector vout[3])
+{
+ typedef typename Matrix::Scalar Scalar;
+ Matrix R(m);
+ 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 >
+// 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)
+{
+ return Eigen::FclOp<Eigen::Transpose<const Eigen::FclMatrix<T,3,_Options> > > (R);
+}
+
+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);
+}
+
}
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 03f562cab45652e3c56226e7c9d27a573650b930..f90476b67918e08f6213af9eb317510534ca4938 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -51,9 +51,9 @@ namespace fcl
#if FCL_HAVE_EIGEN
# if FCL_USE_NATIVE_EIGEN
- typedef Matrix3fX<FCL_REAL> Matrix3f;
+ typedef Eigen::FclMatrix<FCL_REAL, 3, 0> Matrix3f;
# else
- typedef Matrix3fX<details::eigen_m3<FCL_REAL> > Matrix3f;
+ typedef Matrix3fX<details::eigen_wrapper_m3<FCL_REAL> > Matrix3f;
# endif
#elif FCL_HAVE_SSE
typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index f7db1ba3ff9c7aea873fab9e1cf554f0f4ddc5e5..5b1be28bb0bc12d748b4bfe8f7eee130bb22f6fd 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -41,12 +41,6 @@
#include <hpp/fcl/config-fcl.hh>
#include <hpp/fcl/data_types.h>
-#if FCL_USE_NATIVE_EIGEN
-# include <hpp/fcl/eigen/vec_3fx.h>
-#else
-# include <hpp/fcl/math/vec_3fx.h>
-#endif
-
#if FCL_HAVE_EIGEN
# if ! FCL_USE_NATIVE_EIGEN
# include <hpp/fcl/eigen/math_details.h>
@@ -57,6 +51,12 @@
# include <hpp/fcl/math/math_details.h>
#endif
+#if FCL_USE_NATIVE_EIGEN
+# include <hpp/fcl/eigen/vec_3fx.h>
+#else
+# include <hpp/fcl/math/vec_3fx.h>
+#endif
+
#include <cmath>
#include <iostream>
#include <limits>
@@ -67,9 +67,9 @@ namespace fcl
#if FCL_HAVE_EIGEN
# if FCL_USE_NATIVE_EIGEN
- typedef Vec3fX<FCL_REAL> Vec3f;
+ typedef Eigen::FclMatrix<FCL_REAL, 1, 0> Vec3f;
# else
- typedef Vec3fX<details::eigen_v3<FCL_REAL> > Vec3f;
+ typedef Vec3fX<details::eigen_wrapper_v3<FCL_REAL> > Vec3f;
# endif
#elif FCL_HAVE_SSE
typedef Vec3fX<details::sse_meta_f4> Vec3f;
diff --git a/include/hpp/fcl/math/vec_3fx.h b/include/hpp/fcl/math/vec_3fx.h
index bfa228b0eef85a9358ff60600a145b105c48445c..4cc8d86907cb73442dc1650d3f38771616034917 100644
--- a/include/hpp/fcl/math/vec_3fx.h
+++ b/include/hpp/fcl/math/vec_3fx.h
@@ -41,8 +41,6 @@
#include <hpp/fcl/config-fcl.hh>
#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/math/math_details.h>
-
#include <cmath>
#include <iostream>
#include <limits>
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index e3e480108a512ccea88e526a9428e331b56fbfbf..6bb872696047bd507d91d24eb39e76278d90f7ac 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}
@@ -37,5 +45,5 @@ add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
if (FCL_HAVE_OCTOMAP)
add_fcl_test(test_fcl_octomap test_fcl_octomap.cpp test_fcl_utility.cpp)
endif()
-add_fcl_test(test_fcl_math test_fcl_math.cpp)
+add_fcl_template_test(test_fcl_eigen test_fcl_eigen.cpp)
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));
+}