Commit b5646a33 authored by jpan's avatar jpan
Browse files

add octomap extension, still need test;

fix bug in sse matrix


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@128 253336fb-580f-4252-a368-f3cef5a2a82b
parent e28d6e7b
......@@ -12,7 +12,7 @@ set(CMAKE_BUILD_TYPE Release)
set(FCL_VERSION "0.1.1")
set(PKG_DESC "Fast Collision Library")
set(PKG_EXTERNAL_DEPS "ccd flann")
set(PKG_EXTERNAL_DEPS "ccd flann octomap")
set(CMAKE_SKIP_BUILD_RPATH FALSE)
set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
......@@ -35,11 +35,16 @@ pkg_check_modules(CCD REQUIRED ccd)
include_directories(${CCD_INCLUDE_DIRS})
link_directories(${CCD_LIBRARY_DIRS})
pkg_check_modules(OCTOMAP REQUIRED octomap)
include_directories(${OCTOMAP_INCLUDE_DIRS})
link_directories(${OCTOMAP_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp)
add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/ccd/interval.cpp src/ccd/interval_vector.cpp src/ccd/interval_matrix.cpp src/ccd/taylor_model.cpp src/ccd/taylor_vector.cpp src/ccd/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/narrowphase/gjk.cpp src/narrowphase/gjk_libccd.cpp src/narrowphase/narrowphase.cpp src/hierarchy_tree.cpp src/octomap_extension.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
set(pkg_conf_file "${CMAKE_CURRENT_SOURCE_DIR}/fcl.pc")
configure_file("${pkg_conf_file}.in" "${pkg_conf_file}" @ONLY)
......
......@@ -130,9 +130,9 @@ struct BVNode<OBB> : public BVNodeBase
}
void setOrientation(const Matrix3f& m)
{
bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
}
};
......@@ -165,9 +165,9 @@ struct BVNode<RSS> : public BVNodeBase
void setOrientation(const Matrix3f& m)
{
bv.axis[0].setValue(m[0][0], m[1][0], m[2][0]);
bv.axis[1].setValue(m[0][1], m[1][1], m[2][1]);
bv.axis[2].setValue(m[0][2], m[1][2], m[2][2]);
bv.axis[0].setValue(m(0, 0), m(1, 0), m(2, 0));
bv.axis[1].setValue(m(0, 1), m(1, 1), m(2, 1));
bv.axis[2].setValue(m(0, 2), m(1, 2), m(2, 2));
}
};
......
......@@ -1390,6 +1390,8 @@ public:
return dtree.size();
}
const HierarchyTree<AABB>& getTree() const { return dtree; }
private:
HierarchyTree<AABB> dtree;
......@@ -1533,6 +1535,8 @@ public:
}
const alternative::HierarchyTree<AABB>& getTree() const { return dtree; }
private:
alternative::HierarchyTree<AABB> dtree;
boost::unordered_map<CollisionObject*, size_t> table;
......
......@@ -48,8 +48,8 @@ struct Interval
{
FCL_REAL i_[2];
Interval() {}
Interval(FCL_REAL v)
Interval() { i_[0] = i_[1] = 0; }
explicit Interval(FCL_REAL v)
{
i_[0] = i_[1] = v;
}
......@@ -112,6 +112,8 @@ struct Interval
Interval operator * (const Interval& other) const;
Interval& operator *= (const Interval& other);
inline Interval operator * (FCL_REAL d) const
{
if(d >= 0) return Interval(i_[0] * d, i_[1] * d);
......@@ -138,6 +140,8 @@ struct Interval
/** \brief other must not contain 0 */
Interval operator / (const Interval& other) const;
Interval& operator /= (const Interval& other);
/** \brief determine whether the intersection between intervals is empty */
inline bool overlap(const Interval& other) const
{
......@@ -146,13 +150,13 @@ struct Interval
return true;
}
inline void intersect(const Interval& other)
inline bool intersect(const Interval& other)
{
if(i_[1] < other.i_[0]) return;
if(i_[0] > other.i_[1]) return;
if(i_[1] < other.i_[0]) return false;
if(i_[0] > other.i_[1]) return false;
if(i_[1] > other.i_[1]) i_[1] = other.i_[1];
if(i_[0] < other.i_[0]) i_[0] = other.i_[0];
return;
return true;
}
inline Interval operator - () const
......@@ -184,39 +188,31 @@ struct Interval
}
/** \brief Compute the minimum interval contains v and original interval */
inline void bound(FCL_REAL v)
inline Interval& bound(FCL_REAL v)
{
if(v < i_[0]) i_[0] = v;
if(v > i_[1]) i_[1] = v;
return *this;
}
inline Interval bounded(FCL_REAL v) const
{
Interval res = *this;
if(v < res.i_[0]) res.i_[0] = v;
if(v > res.i_[1]) res.i_[1] = v;
return res;
}
/** \brief Compute the minimum interval contains other and original interval */
inline void bound(const Interval& other)
inline Interval& bound(const Interval& other)
{
if(other.i_[0] < i_[0]) i_[0] = other.i_[0];
if(other.i_[1] > i_[1]) i_[1] = other.i_[1];
return *this;
}
inline Interval bounded(const Interval& other) const
{
Interval res = *this;
if(other.i_[0] < res.i_[0]) res.i_[0] = other.i_[0];
if(other.i_[1] > res.i_[1]) res.i_[1] = other.i_[1];
return res;
}
void print() const;
inline FCL_REAL center() const { return 0.5 * (i_[0] + i_[1]); }
inline FCL_REAL diameter() const { return i_[1] -i_[0]; }
};
Interval bound(const Interval& i, FCL_REAL v);
Interval bound(const Interval& i, const Interval& other);
}
#endif
......@@ -47,7 +47,7 @@ namespace fcl
struct IMatrix3
{
Interval i_[3][3];
IVector3 v_[3];
IMatrix3();
IMatrix3(FCL_REAL v);
......@@ -55,31 +55,48 @@ struct IMatrix3
IMatrix3(FCL_REAL m[3][3][2]);
IMatrix3(FCL_REAL m[3][3]);
IMatrix3(Interval m[3][3]);
IMatrix3(const IVector3& v1, const IVector3& v2, const IVector3& v3);
void setIdentity();
IVector3 getColumn(size_t i) const;
IVector3 getRow(size_t i) const;
const IVector3& getRow(size_t i) const;
Vec3f getRealColumn(size_t i) const;
Vec3f getRealRow(size_t i) const;
Vec3f getColumnLow(size_t i) const;
Vec3f getRowLow(size_t i) const;
const Interval& operator () (size_t i, size_t j) const;
Interval& operator () (size_t i, size_t j);
Vec3f getColumnHigh(size_t i) const;
Vec3f getRowHigh(size_t i) const;
Matrix3f getLow() const;
Matrix3f getHigh() const;
inline const IVector3& operator [] (size_t i) const
{
return v_[i];
}
inline IVector3& operator [] (size_t i)
{
return v_[i];
}
IMatrix3 operator + (const IMatrix3& m) const;
IMatrix3& operator += (const IMatrix3& m);
IMatrix3 operator - (const IMatrix3& m) const;
IMatrix3& operator -= (const IMatrix3& m);
IVector3 operator * (const Vec3f& v) const;
IVector3 operator * (const IVector3& v) const;
IMatrix3 operator * (const IMatrix3& m) const;
IMatrix3 operator * (const Matrix3f& m) const;
IMatrix3 operator + (const IMatrix3& m) const;
IMatrix3& operator += (const IMatrix3& m);
void print() const;
IMatrix3& operator *= (const IMatrix3& m);
IMatrix3& operator *= (const Matrix3f& m);
IMatrix3 nonIntervalAddMatrix(const IMatrix3& m) const;
IVector3 nonIntervalTimesVector(const IVector3& v) const;
IVector3 nonIntervalTimesVector(const Vec3f& v) const;
void print() const;
};
}
......
......@@ -57,14 +57,74 @@ struct IVector3
IVector3(const Interval& v1, const Interval& v2, const Interval& v3);
IVector3(const Vec3f& v);
inline void setValue(FCL_REAL v)
{
i_[0].setValue(v);
i_[1].setValue(v);
i_[2].setValue(v);
}
inline void setValue(FCL_REAL x, FCL_REAL y, FCL_REAL z)
{
i_[0].setValue(x);
i_[1].setValue(y);
i_[2].setValue(z);
}
inline void setValue(FCL_REAL xl, FCL_REAL xu, FCL_REAL yl, FCL_REAL yu, FCL_REAL zl, FCL_REAL zu)
{
i_[0].setValue(xl, xu);
i_[1].setValue(yl, yu);
i_[2].setValue(zl, zu);
}
inline void setValue(FCL_REAL v[3][2])
{
i_[0].setValue(v[0][0], v[0][1]);
i_[1].setValue(v[1][0], v[1][1]);
i_[2].setValue(v[2][0], v[2][1]);
}
inline void setValue(Interval v[3])
{
i_[0] = v[0];
i_[1] = v[1];
i_[2] = v[2];
}
inline void setValue(const Interval& v1, const Interval& v2, const Interval& v3)
{
i_[0] = v1;
i_[1] = v2;
i_[2] = v3;
}
inline void setValue(const Vec3f& v)
{
i_[0].setValue(v[0]);
i_[1].setValue(v[1]);
i_[2].setValue(v[2]);
}
inline void setValue(FCL_REAL v[3])
{
i_[0].setValue(v[0]);
i_[1].setValue(v[1]);
i_[2].setValue(v[2]);
}
IVector3 operator + (const IVector3& other) const;
IVector3& operator += (const IVector3& other);
IVector3 operator - (const IVector3& other) const;
IVector3& operator -= (const IVector3& other);
IVector3& operator = (const Vec3f& other);
Interval dot(const IVector3& other) const;
IVector3 cross(const IVector3& other) const;
Interval dot(const Vec3f& other) const;
IVector3 cross(const Vec3f& other) const;
inline const Interval& operator [] (size_t i) const
{
return i_[i];
......@@ -75,6 +135,16 @@ struct IVector3
return i_[i];
}
inline Vec3f getLow() const
{
return Vec3f(i_[0][0], i_[1][0], i_[2][0]);
}
inline Vec3f getHigh() const
{
return Vec3f(i_[0][1], i_[1][1], i_[2][1]);
}
void print() const;
Vec3f center() const;
FCL_REAL volumn() const;
......@@ -83,14 +153,14 @@ struct IVector3
void bound(const Vec3f& v);
void bound(const IVector3& v);
IVector3 bounded(const Vec3f& v) const;
IVector3 bounded(const IVector3& v) const;
bool overlap(const IVector3& v) const;
bool contain(const IVector3& v) const;
void normalize();
};
IVector3 bound(const IVector3& i, const Vec3f& v);
IVector3 bound(const IVector3& i, const IVector3& v);
}
#endif
......@@ -47,31 +47,45 @@ namespace fcl
struct TMatrix3
{
TaylorModel i_[3][3];
TVector3 v_[3];
TMatrix3();
TMatrix3(const boost::shared_ptr<TimeInterval>& time_interval);
TMatrix3(TaylorModel m[3][3]);
TMatrix3(const Matrix3f& m);
TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3);
TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval);
TVector3 getColumn(size_t i) const;
TVector3 getRow(size_t i) const;
const TVector3& getRow(size_t i) const;
const TaylorModel& operator () (size_t i, size_t j) const;
TaylorModel& operator () (size_t i, size_t j);
const TVector3& operator [] (size_t i) const;
TVector3& operator [] (size_t i);
TVector3 operator * (const Vec3f& v) const;
TVector3 operator * (const TVector3& v) const;
TMatrix3 operator * (const Matrix3f& m) const;
TMatrix3 operator * (const TMatrix3& m) const;
TMatrix3 operator * (const TaylorModel& d) const;
TMatrix3 operator * (FCL_REAL d) const;
TMatrix3& operator *= (const Matrix3f& m);
TMatrix3& operator *= (const TMatrix3& m);
TMatrix3& operator *= (const TaylorModel& d);
TMatrix3& operator *= (FCL_REAL d);
TMatrix3 operator + (const TMatrix3& m) const;
TMatrix3& operator += (const TMatrix3& m);
TMatrix3 operator - (const TMatrix3& m) const;
TMatrix3& operator -= (const TMatrix3& m);
IMatrix3 getBound() const;
void print() const;
void setIdentity();
void setZero();
FCL_REAL diameter() const;
void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
};
}
......
......@@ -38,16 +38,31 @@
#define FCL_TAYLOR_MODEL_H
#include "fcl/ccd/interval.h"
#include <boost/shared_ptr.hpp>
namespace fcl
{
struct TimeInterval
{
/** \brief time interval and different powers */
Interval t_; // [t1, t2]
Interval t2_; // [t1, t2]^2
Interval t3_; // [t1, t2]^3
Interval t4_; // [t1, t2]^4
Interval t5_; // [t1, t2]^5
Interval t6_; // [t1, t2]^6
};
/** \brief TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function
* over a time interval, with an interval remainder.
* All the operations on two Taylor models assume their time intervals are the same.
*/
struct TaylorModel
{
/** \brief time interval */
boost::shared_ptr<TimeInterval> time_interval_;
/** \brief Coefficients of the cubic polynomial approximation */
FCL_REAL coeffs_[4];
......@@ -55,18 +70,31 @@ struct TaylorModel
Interval r_;
void setTimeInterval(FCL_REAL l, FCL_REAL r);
void setTimeInterval(const boost::shared_ptr<TimeInterval> time_interval)
{
time_interval_ = time_interval;
}
TaylorModel();
TaylorModel(FCL_REAL coeff);
TaylorModel(FCL_REAL coeffs[3], const Interval& r);
TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r);
TaylorModel(const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL coeff, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL coeffs[3], const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel(FCL_REAL c0, FCL_REAL c1, FCL_REAL c2, FCL_REAL c3, const Interval& r, const boost::shared_ptr<TimeInterval>& time_interval);
TaylorModel operator + (const TaylorModel& other) const;
TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator += (const TaylorModel& other);
TaylorModel operator - (const TaylorModel& other) const;
TaylorModel& operator -= (const TaylorModel& other);
TaylorModel operator + (FCL_REAL d) const;
TaylorModel& operator += (FCL_REAL d);
TaylorModel operator * (const TaylorModel& other) const;
TaylorModel operator * (FCL_REAL d) const;
TaylorModel& operator *= (const TaylorModel& other);
TaylorModel& operator *= (FCL_REAL d);
TaylorModel operator - () const;
void print() const;
......@@ -80,16 +108,6 @@ struct TaylorModel
Interval getBound(FCL_REAL t) const;
void setZero();
/** \brief time interval and different powers */
Interval t_; // [t1, t2]
Interval t2_; // [t1, t2]^2
Interval t3_; // [t1, t2]^3
Interval t4_; // [t1, t2]^4
Interval t5_; // [t1, t2]^5
Interval t6_; // [t1, t2]^6
static const FCL_REAL PI_;
};
void generateTaylorModelForCosFunc(TaylorModel& tm, FCL_REAL w, FCL_REAL q0);
......
......@@ -48,22 +48,32 @@ struct TVector3
TaylorModel i_[3];
TVector3();
TVector3(const boost::shared_ptr<TimeInterval>& time_interval);
TVector3(TaylorModel v[3]);
TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2);
TVector3(const Vec3f& v);
TVector3(const Vec3f& v, const boost::shared_ptr<TimeInterval>& time_interval);
TVector3 operator + (const TVector3& other) const;
TVector3 operator + (FCL_REAL d) const;
TVector3& operator += (const TVector3& other);
TVector3 operator + (FCL_REAL d) const;
TVector3& operator += (FCL_REAL d);
TVector3 operator - (const TVector3& other) const;
TVector3& operator -= (const TVector3& other);
TVector3& operator = (const Vec3f& u);
TVector3 operator * (const TaylorModel& d) const;
TVector3& operator *= (const TaylorModel& d);
TVector3 operator * (FCL_REAL d) const;
TVector3& operator *= (FCL_REAL d);
const TaylorModel& operator [] (size_t i) const;
TaylorModel& operator [] (size_t i);
TaylorModel dot(const TVector3& other) const;
TVector3 cross(const TVector3& other) const;
TaylorModel dot(const Vec3f& other) const;
TVector3 cross(const Vec3f& other) const;
IVector3 getBound() const;
IVector3 getBound(FCL_REAL t) const;
......@@ -73,6 +83,8 @@ struct TVector3
void setZero();
TaylorModel squareLength() const;
void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
};
void generateTVector3ForLinearFunc(TVector3& v, const Vec3f& position, const Vec3f& velocity);
......
......@@ -167,6 +167,334 @@ static inline bool equal(const Vec3Data<T>& x, const Vec3Data<T>& y, T 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];