Commit 699fb081 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Work in progress

parent d447c557
......@@ -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());
......
......@@ -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);
}
......
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()); \
}
This diff is collapsed.
......@@ -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>
......
......@@ -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;
......
......@@ -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;
......
......@@ -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>
......
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)
/*
* 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>