Commit 7fdb22d9 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge pull request #10 from jmirabel/devel

Add Eigen wrapping
parents 77eb79e4 a32b7540
......@@ -2,7 +2,7 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2014 CNRS-LAAS
# Author: Florent Lamiraux
# Author: Florent Lamiraux, Joseph Mirabel
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
......@@ -35,6 +35,7 @@
cmake_minimum_required(VERSION 2.8)
set(CXX_DISABLE_WERROR TRUE)
include(cmake/base.cmake)
include(cmake/eigen.cmake)
include(cmake/boost.cmake)
set(PROJECT_NAME hpp-fcl)
......@@ -45,6 +46,16 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
setup_project()
set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
add_optional_dependency("eigen3 >= 3.0.0")
set(FCL_HAVE_EIGEN ${EIGEN3_FOUND} CACHE BOOL "Use eigen matrix type when possible")
if (EIGEN3_FOUND)
if (FCL_HAVE_EIGEN)
include_directories(${EIGEN3_INCLUDE_DIRS})
endif (FCL_HAVE_EIGEN)
endif (EIGEN3_FOUND)
# Required dependencies
add_required_dependency("ccd >= 1.4")
set(BOOST_COMPONENTS
......@@ -129,7 +140,9 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/continuous_collision.h
include/hpp/fcl/math/vec_nf.h
include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/matrix_3fx.h
include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/vec_3fx.h
include/hpp/fcl/math/sampling.h
include/hpp/fcl/math/math_details.h
include/hpp/fcl/math/transform.h
......@@ -156,6 +169,12 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/collision_object.h
include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh
include/hpp/fcl/eigen/math_details.h
include/hpp/fcl/eigen/vec_3fx.h
include/hpp/fcl/eigen/product.h
include/hpp/fcl/eigen/taylor_operator.h
include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
)
add_subdirectory(src)
......
Subproject commit 7cdd2146508a3431adb8e5a0d4233704080299cd
Subproject commit 5663a002a905b5c8d84a69d8e74044a8e34a48ed
......@@ -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]);
}
......
......@@ -71,7 +71,7 @@ public:
/// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb.
bool overlap(const OBB& other, OBB& overlap_part) const
bool overlap(const OBB& other, OBB& /*overlap_part*/) const
{
return overlap(other);
}
......
......@@ -72,7 +72,7 @@ public:
}
/// @brief Check collision between two OBBRSS and return the overlap part.
bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
{
return overlap(other);
}
......
......@@ -67,7 +67,7 @@ public:
bool overlap(const RSS& other) const;
/// Not implemented
bool overlap(const RSS& other, FCL_REAL& sqrDistLowerBound) const
bool overlap(const RSS& /*other*/, FCL_REAL& /*sqrDistLowerBound*/) const
{
throw std::runtime_error ("Not implemented.");
return false;
......@@ -75,7 +75,7 @@ public:
/// @brief Check collision between two RSS and return the overlap part.
/// For RSS, we return nothing, as the overlap part of two RSSs usually is not a RSS.
bool overlap(const RSS& other, RSS& overlap_part) const
bool overlap(const RSS& other, RSS& /*overlap_part*/) const
{
return overlap(other);
}
......
......@@ -97,7 +97,7 @@ public:
bool overlap(const KDOP<N>& other) const;
/// Not implemented
bool overlap(const KDOP<N>& other, FCL_REAL&) const
bool overlap(const KDOP<N>& /*other*/, FCL_REAL&) const
{
throw std::runtime_error ("Not implemented");
}
......
......@@ -69,7 +69,7 @@ class kIOS
}
else /** spheres partially overlapping or disjoint */
{
float dist = std::sqrt(dist2);
float dist = (float)std::sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
if(dist > 0)
......@@ -99,7 +99,7 @@ public:
/// @brief Check collision between two kIOS and return the overlap part.
/// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
/// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
bool overlap(const kIOS& other, kIOS& overlap_part) const
bool overlap(const kIOS& other, kIOS& /*overlap_part*/) const
{
return overlap(other);
}
......
......@@ -54,7 +54,7 @@ enum BVHBuildState
BVH_BUILD_STATE_PROCESSED, /// after tree has been build, ready for cd use
BVH_BUILD_STATE_UPDATE_BEGUN, /// after beginUpdateModel(), state for updating geometry primitives
BVH_BUILD_STATE_UPDATED, /// after tree has been build for updated geometry, ready for ccd use
BVH_BUILD_STATE_REPLACE_BEGUN, /// after beginReplaceModel(), state for replacing geometry primitives
BVH_BUILD_STATE_REPLACE_BEGUN /// after beginReplaceModel(), state for replacing geometry primitives
};
/// @brief Error code for BVH
......
......@@ -85,13 +85,13 @@ public:
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
virtual void update(CollisionObject* updated_obj)
virtual void update(CollisionObject* /*updated_obj*/)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<CollisionObject*>& updated_objs)
virtual void update(const std::vector<CollisionObject*>& /*updated_objs*/)
{
update();
}
......@@ -184,13 +184,13 @@ public:
virtual void update() = 0;
/// @brief update the manager by explicitly given the object updated
virtual void update(ContinuousCollisionObject* updated_obj)
virtual void update(ContinuousCollisionObject* /*updated_obj*/)
{
update();
}
/// @brief update the manager by explicitly given the set of objects update
virtual void update(const std::vector<ContinuousCollisionObject*>& updated_objs)
virtual void update(const std::vector<ContinuousCollisionObject*>& /*updated_objs*/)
{
update();
}
......
......@@ -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);
......
......@@ -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);
......
......@@ -88,7 +88,7 @@ public:
tf_ = tf;
}
void getTaylorModel(TMatrix3& tm, TVector3& tv) const
void getTaylorModel(TMatrix3& /*tm*/, TVector3& /*tv*/) const
{
}
......@@ -113,14 +113,14 @@ public:
const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3);
// @brief Construct motion from initial and goal transform
SplineMotion(const Matrix3f& R1, const Vec3f& T1,
const Matrix3f& R2, const Vec3f& T2) : MotionBase()
SplineMotion(const Matrix3f& /*R1*/, const Vec3f& /*T1*/,
const Matrix3f& /*R2*/, const Vec3f& /*T2*/) : MotionBase()
{
// TODO
}
SplineMotion(const Transform3f& tf1,
const Transform3f& tf2) : MotionBase()
SplineMotion(const Transform3f& /*tf1*/,
const Transform3f& /*tf2*/) : MotionBase()
{
// TODO
}
......
......@@ -72,11 +72,11 @@ class TBVMotionBoundVisitor : public BVMotionBoundVisitor
public:
TBVMotionBoundVisitor(const BV& bv_, const Vec3f& n_) : bv(bv_), n(n_) {}
virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& motion) const { return 0; }
virtual FCL_REAL visit(const ScrewMotion& motion) const { return 0; }
virtual FCL_REAL visit(const InterpMotion& motion) const { return 0; }
virtual FCL_REAL visit(const TranslationMotion& motion) const { return 0; }
virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const ScrewMotion& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const InterpMotion& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const TranslationMotion& /*motion*/) const { return 0; }
protected:
BV bv;
......@@ -102,7 +102,7 @@ public:
TriangleMotionBoundVisitor(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_, const Vec3f& n_) :
a(a_), b(b_), c(c_), n(n_) {}
virtual FCL_REAL visit(const MotionBase& motion) const { return 0; }
virtual FCL_REAL visit(const MotionBase& /*motion*/) const { return 0; }
virtual FCL_REAL visit(const SplineMotion& motion) const;
virtual FCL_REAL visit(const ScrewMotion& motion) const;
virtual FCL_REAL visit(const InterpMotion& motion) const;
......
......@@ -41,6 +41,10 @@
#include <hpp/fcl/ccd/interval_vector.h>
#include <hpp/fcl/ccd/taylor_model.h>
#if FCL_HAVE_EIGEN
#include <hpp/fcl/eigen/taylor_operator.h>
#endif
namespace fcl
{
......
......@@ -37,6 +37,8 @@
# include "config.h"
#cmakedefine01 FCL_HAVE_EIGEN
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#cmakedefine01 FCL_HAVE_FLANN
......
This diff is collapsed.
template <typename Derived>
IVector3& operator=(const FclType<Derived>& other)
{
const Vec3f& tmp = other.fcl();
setValue (tmp);
return *this;
}
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()); \
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment