Skip to content
GitLab
Commit 405a4b83 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Clean code and remove unused files.

parent f1923507
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 405a4b83
......@@ -48,9 +48,8 @@ setup_project()
set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
# add_optional_dependency("eigen3 >= 3.0.0")
add_required_dependency("eigen3 >= 3.0.0")
set(FCL_HAVE_EIGEN TRUE CACHE BOOL "Use eigen matrix type when possible")
set(FCL_HAVE_EIGEN TRUE)
include_directories(${EIGEN3_INCLUDE_DIRS})
# Required dependencies
......
include/hpp/fcl/ccd/interval_matrix.h
View file @ 405a4b83
......@@ -99,10 +99,6 @@ struct IMatrix3
IMatrix3& rotationConstrain();
void print() const;
#ifdef FCL_CCD_INTERVALMATRIX_PLUGIN
# include FCL_CCD_INTERVALMATRIX_PLUGIN
#endif
};
IMatrix3 rotationConstrain(const IMatrix3& m);
......
include/hpp/fcl/ccd/interval_vector.h
View file @ 405a4b83
......@@ -157,9 +157,15 @@ 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
template <typename Derived>
IVector3& operator=(const Eigen::MatrixBase<Derived>& other)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); \
setValue (other.derived());
// const Vec3f& tmp (other);
// setValue (tmp);
return *this;
}
};
IVector3 bound(const IVector3& i, const Vec3f& v);
......
include/hpp/fcl/ccd/taylor_vector.h
View file @ 405a4b83
......@@ -40,10 +40,8 @@
#include <hpp/fcl/ccd/interval_vector.h>
#include <hpp/fcl/ccd/taylor_model.h>
#if FCL_HAVE_EIGEN
#include <hpp/fcl/eigen/taylor_operator.h>
#endif
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
namespace fcl
{
......@@ -106,8 +104,20 @@ public:
const boost::shared_ptr<TimeInterval>& getTimeInterval() const;
};
class TMatrix3;
void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
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 Eigen::MatrixBase<Derived>& v, const TaylorModel& a)
{
const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.derived();
return b * a;
}
TVector3 operator * (const Vec3f& v, const TaylorModel& a);
TVector3 operator + (const Vec3f& v1, const TVector3& v2);
......
include/hpp/fcl/eigen/math_details.h deleted 100644 → 0
View file @ f1923507
This diff is collapsed.
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h deleted 100644 → 0
View file @ f1923507
template <typename Derived>
IVector3& operator=(const Eigen::MatrixBase<Derived>& other)
{
const Vec3f& tmp (other);
setValue (tmp);
return *this;
}
include/hpp/fcl/eigen/product.h deleted 100644 → 0
View file @ f1923507
namespace internal {
template<typename Derived, typename OtherDerived, bool coefwise> struct deduce_fcl_type {};
template<typename Derived, typename OtherDerived>
struct deduce_fcl_type<Derived, OtherDerived, false> {
typedef typename ProductReturnType<Derived, OtherDerived>::Type Type;
};
template<typename Derived, typename OtherDerived>
struct deduce_fcl_type<Derived, OtherDerived, true> {
typedef CwiseBinaryOp<scalar_multiple_op<typename Derived::Scalar>, const Derived, const OtherDerived> Type;
};
}
template<typename Derived, typename OtherDerived>
struct FclProduct
{
enum {
COEFWISE = Derived::ColsAtCompileTime == 1 && OtherDerived::ColsAtCompileTime == 1
};
typedef typename internal::remove_fcl<Derived>::type EDerived;
typedef typename internal::remove_fcl<OtherDerived>::type EOtherDerived;
typedef FclOp<typename internal::deduce_fcl_type<EDerived, EOtherDerived, COEFWISE>::Type> ProductType;
typedef FclOp<typename ProductReturnType<Transpose<Derived>, EOtherDerived >::Type> TransposeTimesType;
typedef FclOp<typename ProductReturnType<EDerived, Transpose<EOtherDerived> >::Type> TimesTransposeType;
typedef FclOp<typename ProductReturnType<ProductType, Transpose<EDerived> >::Type> TensorTransformType;
static EIGEN_STRONG_INLINE ProductType run (const Derived& l, const OtherDerived& r) { return ProductType (l, r); }
};
#define FCL_EIGEN_MAKE_PRODUCT_OPERATOR() \
template <typename OtherDerived> \
EIGEN_STRONG_INLINE const typename FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::ProductType \
operator*(const MatrixBase<OtherDerived>& other) const \
{ \
return FclProduct<const typename FCL_EIGEN_CURRENT_CLASS::Base, const OtherDerived>::run (*this, other.derived()); \
}
include/hpp/fcl/eigen/taylor_operator.h deleted 100644 → 0
View file @ f1923507
#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 Eigen::MatrixBase<Derived>& v, const TaylorModel& a)
{
const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.derived();
return b * a;
}
}
#endif // FCL_CCD_TAYLOR_OPERATOR_H
include/hpp/fcl/eigen/vec_3fx.h deleted 100644 → 0
View file @ f1923507
This diff is collapsed.
include/hpp/fcl/math/math_details.h deleted 100644 → 0
View file @ f1923507
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Open Source Robotics Foundation nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FCL_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>
#include <cstring>
namespace fcl
{
namespace details
{
template <typename T>
struct Vec3Data
{
typedef T meta_type;
T vs[3];
Vec3Data() { setValue(0); }
Vec3Data(T x)
{
setValue(x);
}
Vec3Data(T* x)
{
memcpy(vs, x, sizeof(T) * 3);
}
Vec3Data(T x, T y, T z)
{
setValue(x, y, z);
}
inline void setValue(T x, T y, T z)
{
vs[0] = x; vs[1] = y; vs[2] = z;
}
inline void setValue(T x)
{
vs[0] = x; vs[1] = x; vs[2] = x;
}
inline Vec3Data<T>& ubound(const Vec3Data<T>& u)
{
vs[0] = std::min(vs[0], u.vs[0]);
vs[1] = std::min(vs[1], u.vs[1]);
vs[2] = std::min(vs[2], u.vs[2]);
return *this;
}
inline Vec3Data<T>& lbound(const Vec3Data<T>& l)
{
vs[0] = std::max(vs[0], l.vs[0]);
vs[1] = std::max(vs[1], l.vs[1]);
vs[2] = std::max(vs[2], l.vs[2]);
return *this;
}
T operator [] (size_t i) const { return vs[i]; }
T& operator [] (size_t i) { return vs[i]; }
inline Vec3Data<T> operator + (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] + other.vs[0], vs[1] + other.vs[1], vs[2] + other.vs[2]); }
inline Vec3Data<T> operator - (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] - other.vs[0], vs[1] - other.vs[1], vs[2] - other.vs[2]); }
inline Vec3Data<T> operator * (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] * other.vs[0], vs[1] * other.vs[1], vs[2] * other.vs[2]); }
inline Vec3Data<T> operator / (const Vec3Data<T>& other) const { return Vec3Data<T>(vs[0] / other.vs[0], vs[1] / other.vs[1], vs[2] / other.vs[2]); }
inline Vec3Data<T>& operator += (const Vec3Data<T>& other) { vs[0] += other.vs[0]; vs[1] += other.vs[1]; vs[2] += other.vs[2]; return *this; }
inline Vec3Data<T>& operator -= (const Vec3Data<T>& other) { vs[0] -= other.vs[0]; vs[1] -= other.vs[1]; vs[2] -= other.vs[2]; return *this; }
inline Vec3Data<T>& operator *= (const Vec3Data<T>& other) { vs[0] *= other.vs[0]; vs[1] *= other.vs[1]; vs[2] *= other.vs[2]; return *this; }
inline Vec3Data<T>& operator /= (const Vec3Data<T>& other) { vs[0] /= other.vs[0]; vs[1] /= other.vs[1]; vs[2] /= other.vs[2]; return *this; }
inline Vec3Data<T> operator + (T t) const { return Vec3Data<T>(vs[0] + t, vs[1] + t, vs[2] + t); }
inline Vec3Data<T> operator - (T t) const { return Vec3Data<T>(vs[0] - t, vs[1] - t, vs[2] - t); }
inline Vec3Data<T> operator * (T t) const { return Vec3Data<T>(vs[0] * t, vs[1] * t, vs[2] * t); }
inline Vec3Data<T> operator / (T t) const { T inv_t = 1.0 / t; return Vec3Data<T>(vs[0] * inv_t, vs[1] * inv_t, vs[2] * inv_t); }
inline Vec3Data<T>& operator += (T t) { vs[0] += t; vs[1] += t; vs[2] += t; return *this; }
inline Vec3Data<T>& operator -= (T t) { vs[0] -= t; vs[1] -= t; vs[2] -= t; return *this; }
inline Vec3Data<T>& operator *= (T t) { vs[0] *= t; vs[1] *= t; vs[2] *= t; return *this; }
inline Vec3Data<T>& operator /= (T t) { T inv_t = 1.0 / t; vs[0] *= inv_t; vs[1] *= inv_t; vs[2] *= inv_t; return *this; }
inline Vec3Data<T> operator - () const { return Vec3Data<T>(-vs[0], -vs[1], -vs[2]); }
};
template <typename T>
static inline Vec3Data<T> cross_prod(const Vec3Data<T>& l, const Vec3Data<T>& r)
{
return Vec3Data<T>(l.vs[1] * r.vs[2] - l.vs[2] * r.vs[1],
l.vs[2] * r.vs[0] - l.vs[0] * r.vs[2],
l.vs[0] * r.vs[1] - l.vs[1] * r.vs[0]);
}
template <typename T>
static inline T dot_prod3(const Vec3Data<T>& l, const Vec3Data<T>& r)
{
return l.vs[0] * r.vs[0] + l.vs[1] * r.vs[1] + l.vs[2] * r.vs[2];
}
template <typename T>
static inline Vec3Data<T> min(const Vec3Data<T>& x, const Vec3Data<T>& y)
{
return Vec3Data<T>(std::min(x.vs[0], y.vs[0]), std::min(x.vs[1], y.vs[1]), std::min(x.vs[2], y.vs[2]));
}
template <typename T>
static inline Vec3Data<T> max(const Vec3Data<T>& x, const Vec3Data<T>& y)
{
return Vec3Data<T>(std::max(x.vs[0], y.vs[0]), std::max(x.vs[1], y.vs[1]), std::max(x.vs[2], y.vs[2]));
}
template <typename T>
static inline Vec3Data<T> abs(const Vec3Data<T>& x)
{
return Vec3Data<T>(std::abs(x.vs[0]), std::abs(x.vs[1]), std::abs(x.vs[2]));
}
template <typename T>
static inline bool equal(const Vec3Data<T>& x, const Vec3Data<T>& y, T epsilon)
{
return ((x.vs[0] - y.vs[0] < epsilon) &&
(x.vs[0] - y.vs[0] > -epsilon) &&
(x.vs[1] - y.vs[1] < epsilon) &&
(x.vs[1] - y.vs[1] > -epsilon) &&
(x.vs[2] - y.vs[2] < epsilon) &&
(x.vs[2] - y.vs[2] > -epsilon));
}
template<typename T>
struct Matrix3Data
{
typedef T meta_type;
typedef Vec3Data<T> vector_type;
Vec3Data<T> rs[3];
Matrix3Data() {};
Matrix3Data(T xx, T xy, T xz,
T yx, T yy, T yz,
T zx, T zy, T zz)
{
setValue(xx, xy, xz,
yx, yy, yz,
zx, zy, zz);
}
Matrix3Data(const Vec3Data<T>& v1, const Vec3Data<T>& v2, const Vec3Data<T>& v3)
{
rs[0] = v1;
rs[1] = v2;
rs[2] = v3;
}
Matrix3Data(const Matrix3Data<T>& other)
{
rs[0] = other.rs[0];
rs[1] = other.rs[1];
rs[2] = other.rs[2];
}
inline Vec3Data<T> getColumn(size_t i) const
{
return Vec3Data<T>(rs[0][i], rs[1][i], rs[2][i]);
}
inline const Vec3Data<T>& getRow(size_t i) const
{
return rs[i];
}
inline T operator() (size_t i, size_t j) const
{
return rs[i][j];
}
inline T& operator() (size_t i, size_t j)
{
return rs[i][j];
}
inline Vec3Data<T> operator * (const Vec3Data<T>& v) const
{
return Vec3Data<T>(dot_prod3(rs[0], v), dot_prod3(rs[1], v), dot_prod3(rs[2], v));
}
inline Matrix3Data<T> operator * (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]),
other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]),
other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
}
inline Matrix3Data<T> operator + (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(rs[0] + other.rs[0], rs[1] + other.rs[1], rs[2] + other.rs[2]);
}
inline Matrix3Data<T> operator - (const Matrix3Data<T>& other) const
{
return Matrix3Data<T>(rs[0] - other.rs[0], rs[1] - other.rs[1], rs[2] - other.rs[2]);
}
inline Matrix3Data<T> operator + (T c) const
{
return Matrix3Data<T>(rs[0] + c, rs[1] + c, rs[2] + c);
}
inline Matrix3Data<T> operator - (T c) const
{
return Matrix3Data<T>(rs[0] - c, rs[1] - c, rs[2] - c);
}
inline Matrix3Data<T> operator * (T c) const
{
return Matrix3Data<T>(rs[0] * c, rs[1] * c, rs[2] * c);
}
inline Matrix3Data<T> operator / (T c) const
{
return Matrix3Data<T>(rs[0] / c, rs[1] / c, rs[2] / c);
}
inline Matrix3Data<T>& operator *= (const Matrix3Data<T>& other)
{
rs[0].setValue(other.transposeDotX(rs[0]), other.transposeDotY(rs[0]), other.transposeDotZ(rs[0]));
rs[1].setValue(other.transposeDotX(rs[1]), other.transposeDotY(rs[1]), other.transposeDotZ(rs[1]));
rs[2].setValue(other.transposeDotX(rs[2]), other.transposeDotY(rs[2]), other.transposeDotZ(rs[2]));
return *this;
}
inline Matrix3Data<T>& operator += (const Matrix3Data<T>& other)
{
rs[0] += other.rs[0];
rs[1] += other.rs[1];
rs[2] += other.rs[2];
return *this;
}
inline Matrix3Data<T>& operator -= (const Matrix3Data<T>& other)
{
rs[0] -= other.rs[0];
rs[1] -= other.rs[1];
rs[2] -= other.rs[2];
return *this;
}
inline Matrix3Data<T>& operator += (T c)
{
rs[0] += c;
rs[1] += c;
rs[2] += c;
return *this;
}
inline Matrix3Data<T>& operator - (T c)
{
rs[0] -= c;
rs[1] -= c;
rs[2] -= c;
return *this;
}
inline Matrix3Data<T>& operator * (T c)
{
rs[0] *= c;
rs[1] *= c;
rs[2] *= c;
return *this;
}
inline Matrix3Data<T>& operator / (T c)
{
rs[0] /= c;
rs[1] /= c;
rs[2] /= c;
return *this;
}
void setIdentity()
{
setValue((T)1, (T)0, (T)0,
(T)0, (T)1, (T)0,
(T)0, (T)0, (T)1);
}
void setZero()
{
setValue((T)0);
}
static const Matrix3Data<T>& getIdentity()
{
static const Matrix3Data<T> I((T)1, (T)0, (T)0,
(T)0, (T)1, (T)0,
(T)0, (T)0, (T)1);
return I;
}
T determinant() const
{
return dot_prod3(rs[0], cross_prod(rs[1], rs[2]));
}
Matrix3Data<T>& transpose()
{
register T tmp = rs[0][1];
rs[0][1] = rs[1][0];
rs[1][0] = tmp;
tmp = rs[0][2];
rs[0][2] = rs[2][0];
rs[2][0] = tmp;
tmp = rs[2][1];
rs[2][1] = rs[1][2];
rs[1][2] = tmp;
return *this;
}
Matrix3Data<T>& inverse()
{
T det = determinant();
register T inrsdet = 1 / det;
setValue((rs[1][1] * rs[2][2] - rs[1][2] * rs[2][1]) * inrsdet,
(rs[0][2] * rs[2][1] - rs[0][1] * rs[2][2]) * inrsdet,
(rs[0][1] * rs[1][2] - rs[0][2] * rs[1][1]) * inrsdet,
(rs[1][2] * rs[2][0] - rs[1][0] * rs[2][2]) * inrsdet,
(rs[0][0] * rs[2][2] - rs[0][2] * rs[2][0]) * inrsdet,
(rs[0][2] * rs[1][0] - rs[0][0] * rs[1][2]) * inrsdet,
(rs[1][0] * rs[2][1] - rs[1][1] * rs[2][0]) * inrsdet,
(rs[0][1] * rs[2][0] - rs[0][0] * rs[2][1]) * inrsdet,
(rs[0][0] * rs[1][1] - rs[0][1] * rs[1][0]) * inrsdet);
return *this;
}
Matrix3Data<T> transposeTimes(const Matrix3Data<T>& m) const
{
return Matrix3Data<T>(rs[0][0] * m.rs[0][0] + rs[1][0] * m.rs[1][0] + rs[2][0] * m.rs[2][0],
rs[0][0] * m.rs[0][1] + rs[1][0] * m.rs[1][1] + rs[2][0] * m.rs[2][1],
rs[0][0] * m.rs[0][2] + rs[1][0] * m.rs[1][2] + rs[2][0] * m.rs[2][2],
rs[0][1] * m.rs[0][0] + rs[1][1] * m.rs[1][0] + rs[2][1] * m.rs[2][0],
rs[0][1] * m.rs[0][1] + rs[1][1] * m.rs[1][1] + rs[2][1] * m.rs[2][1],
rs[0][1] * m.rs[0][2] + rs[1][1] * m.rs[1][2] + rs[2][1] * m.rs[2][2],
rs[0][2] * m.rs[0][0] + rs[1][2] * m.rs[1][0] + rs[2][2] * m.rs[2][0],
rs[0][2] * m.rs[0][1] + rs[1][2] * m.rs[1][1] + rs[2][2] * m.rs[2][1],
rs[0][2] * m.rs[0][2] + rs[1][2] * m.rs[1][2] + rs[2][2] * m.rs[2][2]);
}
Matrix3Data<T> timesTranspose(const Matrix3Data<T>& m) const
{
return Matrix3Data<T>(dot_prod3(rs[0], m[0]), dot_prod3(rs[0], m[1]), dot_prod3(rs[0], m[2]),
dot_prod3(rs[1], m[0]), dot_prod3(rs[1], m[1]), dot_prod3(rs[1], m[2]),
dot_prod3(rs[2], m[0]), dot_prod3(rs[2], m[1]), dot_prod3(rs[2], m[2]));
}
Vec3Data<T> transposeTimes(const Vec3Data<T>& v) const
{
return Vec3Data<T>(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
}
inline T transposeDotX(const Vec3Data<T>& v) const
{
return rs[0][0] * v[0] + rs[1][0] * v[1] + rs[2][0] * v[2];
}