Commit 68d90644 authored by jpan's avatar jpan
Browse files

rewriting part of the code. more documentation.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@150 253336fb-580f-4252-a368-f3cef5a2a82b
parent 029259cc
......@@ -41,7 +41,7 @@ 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/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/BV/kDOP.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)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES})
......
......@@ -50,7 +50,11 @@
namespace fcl
{
/// @cond IGNORE
namespace details
{
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to a bounding volume of type BV2 in I configuration.
template<typename BV1, typename BV2>
class Converter
{
......@@ -61,6 +65,8 @@ private:
}
};
/// @brief Convert from AABB to AABB, not very tight but is fast.
template<>
class Converter<AABB, AABB>
{
......@@ -81,17 +87,10 @@ class Converter<AABB, OBB>
{
public:
static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
{
/*
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.To = tf1.transform(bv1.center());
const Matrix3f& R = tf1.getRotation();
bv2.axis[0] = R.getColumn(0);
bv2.axis[1] = R.getColumn(1);
bv2.axis[2] = R.getColumn(2);
*/
{
bv2.To = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
std::size_t id[3] = {0, 1, 2};
......@@ -242,6 +241,8 @@ public:
static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
std::size_t id[3] = {0, 1, 2};
......@@ -278,13 +279,16 @@ public:
}
};
}
/// @endcond
/// @brief Convert a bounding volume of type BV1 in configuration tf1 to bounding volume of type BV2 in identity configuration.
template<typename BV1, typename BV2>
static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
Converter<BV1, BV2>::convert(bv1, tf1, bv2);
details::Converter<BV1, BV2>::convert(bv1, tf1, bv2);
}
}
......
......@@ -41,48 +41,45 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
/** \brief Main namespace */
namespace fcl
{
/** \brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points */
/// @brief A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points
class AABB
{
public:
/** \brief The min point in the AABB */
/// @brief The min point in the AABB
Vec3f min_;
/** \brief The max point in the AABB */
/// @brief The max point in the AABB
Vec3f max_;
/** \brief Constructor creating an AABB with infinite size */
/// @brief Creating an AABB with zero size (low bound +inf, upper bound -inf)
AABB();
/** \brief Constructor creating an AABB at position v with zero size */
/// @brief Creating an AABB at position v with zero size
AABB(const Vec3f& v) : min_(v), max_(v)
{
}
/** \brief Constructor creating an AABB with two endpoints a and b */
AABB(const Vec3f& a, const Vec3f&b)
/// @brief Creating an AABB with two endpoints a and b
AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)),
max_(max(a, b))
{
min_ = min(a, b);
max_ = max(a, b);
}
AABB(const AABB& core, const Vec3f& delta)
/// @brief Creating an AABB centered as core and is of half-dimension delta
AABB(const AABB& core, const Vec3f& delta) : min_(core.min_ - delta),
max_(core.max_ + delta)
{
min_ = core.min_ - delta;
max_ = core.max_ + delta;
}
/** \brief Constructor creating an AABB with three points */
AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c)
/// @brief Creating an AABB contains three points
AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)),
max_(max(max(a, b), c))
{
min_ = min(min(a, b), c);
max_ = max(max(a, b), c);
}
/** \brief Check whether two AABB are overlap */
/// @brief Check whether two AABB are overlap
inline bool overlap(const AABB& other) const
{
if(min_[0] > other.max_[0]) return false;
......@@ -96,13 +93,14 @@ public:
return true;
}
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
{
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
}
/** \brief Check whether two AABB are overlapped along specific axis */
/// @brief Check whether two AABB are overlapped along specific axis
inline bool axisOverlap(const AABB& other, int axis_id) const
{
if(min_[axis_id] > other.max_[axis_id]) return false;
......@@ -112,7 +110,7 @@ public:
return true;
}
/** \brief Check whether two AABB are overlap and return the overlap part */
/// @brief Check whether two AABB are overlap and return the overlap part
inline bool overlap(const AABB& other, AABB& overlap_part) const
{
if(!overlap(other))
......@@ -126,7 +124,7 @@ public:
}
/** \brief Check whether the AABB contains a point */
/// @brief Check whether the AABB contains a point
inline bool contain(const Vec3f& p) const
{
if(p[0] < min_[0] || p[0] > max_[0]) return false;
......@@ -136,7 +134,7 @@ public:
return true;
}
/** \brief Merge the AABB and a point */
/// @brief Merge the AABB and a point
inline AABB& operator += (const Vec3f& p)
{
min_.ubound(p);
......@@ -144,7 +142,7 @@ public:
return *this;
}
/** \brief Merge the AABB and another AABB */
/// @brief Merge the AABB and another AABB
inline AABB& operator += (const AABB& other)
{
min_.ubound(other.min_);
......@@ -152,58 +150,62 @@ public:
return *this;
}
/** \brief Return the merged AABB of current AABB and the other one */
/// @brief Return the merged AABB of current AABB and the other one
inline AABB operator + (const AABB& other) const
{
AABB res(*this);
return res += other;
}
/** \brief Width of the AABB */
/// @brief Width of the AABB
inline FCL_REAL width() const
{
return max_[0] - min_[0];
}
/** \brief Height of the AABB */
/// @brief Height of the AABB
inline FCL_REAL height() const
{
return max_[1] - min_[1];
}
/** \brief Depth of the AABB */
/// @brief Depth of the AABB
inline FCL_REAL depth() const
{
return max_[2] - min_[2];
}
/** \brief Volume of the AABB */
/// @brief Volume of the AABB
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/** \brief Size of the AABB, for split order */
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline FCL_REAL size() const
{
return (max_ - min_).sqrLength();
}
/** \brief Center of the AABB */
/// @brief Center of the AABB
inline Vec3f center() const
{
return (min_ + max_) * 0.5;
}
/// @brief Distance between two AABBs; P and Q, should not be NULL, return the nearest points
FCL_REAL distance(const AABB& other, Vec3f* P, Vec3f* Q) const;
/// @brief Distance between two AABBs
FCL_REAL distance(const AABB& other) const;
/// @brief whether two AABB are equal
inline bool equal(const AABB& other) const
{
return min_.equal(other.min_) && max_.equal(other.max_);
}
/// @brief expand the half size of the AABB by delta, and keep the center unchanged.
inline AABB& expand(const Vec3f& delta)
{
min_ -= delta;
......@@ -211,7 +213,7 @@ public:
return *this;
}
/**\brief expand the aabb by increase the 'thickness of the plate by a ratio */
/// @brief expand the aabb by increase the thickness of the plate by a ratio
inline AABB& expand(const AABB& core, FCL_REAL ratio)
{
min_ = min_ * ratio - core.min_;
......@@ -220,6 +222,7 @@ public:
}
};
/// @brief translate the center of AABB by t
static inline AABB translate(const AABB& aabb, const Vec3f& t)
{
AABB res(aabb);
......
......@@ -41,114 +41,97 @@
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
/** \brief Main namespace */
namespace fcl
{
/** \brief OBB class */
/// @brief Oriented bounding box class
class OBB
{
public:
/** \brief Orientation of OBB */
Vec3f axis[3]; // R[i] is the ith column of the orientation matrix, or the axis of the OBB
/// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box.
/// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
/** \brief center of OBB */
/// @brief Center of OBB
Vec3f To;
/** \brief Half dimensions of OBB */
/// @brief Half dimensions of OBB
Vec3f extent;
OBB() {}
/** \brief Check collision between two OBB */
/// @brief Check collision between two OBB, return true if collision happens.
bool overlap(const OBB& other) const;
/** \brief Check collision between two OBB and return the overlap part.
* For OBB, we return nothing, as the overlap part of two obbs usually is not an obb
*/
/// @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
{
return overlap(other);
}
/** \brief Check whether the OBB contains a point */
/// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const;
/** \brief A simple way to merge the OBB and a point, not compact. */
/// @brief A simple way to merge the OBB and a point (the result is not compact).
OBB& operator += (const Vec3f& p);
/** \brief Merge the OBB and another OBB */
/// @brief Merge the OBB and another OBB (the result is not compact).
OBB& operator += (const OBB& other)
{
*this = *this + other;
return *this;
}
/** \brief Return the merged OBB of current OBB and the other one */
/// @brief Return the merged OBB of current OBB and the other one (the result is not compact).
OBB operator + (const OBB& other) const;
/** \brief Width of the OBB */
/// @brief Width of the OBB.
inline FCL_REAL width() const
{
return 2 * extent[0];
}
/** \brief Height of the OBB */
/// @brief Height of the OBB.
inline FCL_REAL height() const
{
return 2 * extent[1];
}
/** \brief Depth of the OBB */
/// @brief Depth of the OBB
inline FCL_REAL depth() const
{
return 2 * extent[2];
}
/** \brief Volume of the OBB */
/// @brief Volume of the OBB
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
/** \brief Size of the OBB, for split order */
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline FCL_REAL size() const
{
return extent.sqrLength();
}
/** \brief Center of the OBB */
/// @brief Center of the OBB
inline const Vec3f& center() const
{
return To;
}
/** \brief The distance between two OBB
* Not implemented
*/
FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
/** Compute the 8 vertices of a OBB */
void computeVertices(Vec3f vertex[8]) const;
/** \brief OBB merge method when the centers of two smaller OBB are far away */
static OBB merge_largedist(const OBB& b1, const OBB& b2);
/** \brief OBB merge method when the centers of two smaller OBB are close */
static OBB merge_smalldist(const OBB& b1, const OBB& b2);
public:
/** Kernel check whether two OBB are disjoint */
static bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
/// @brief Distance between two OBBs, not implemented.
FCL_REAL distance(const OBB& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
};
/// @brief Check collision between two obbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2);
/// @brief Check collision between two boxes: the first box is in configuration (R, T) and its half dimension is set by a;
/// the second box is in identity configuration and its half dimension is set by b.
bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b);
}
#endif
......@@ -46,30 +46,37 @@
namespace fcl
{
/// @brief Class merging the OBB and RSS, can handle collision and distance simultaneously
class OBBRSS
{
public:
/// @brief OBB member, for rotation
OBB obb;
/// @brief RSS member, for distance
RSS rss;
OBBRSS() {}
/// @brief Check collision between two OBBRSS
bool overlap(const OBBRSS& other) const
{
return obb.overlap(other.obb);
}
/// @brief Check collision between two OBBRSS and return the overlap part.
bool overlap(const OBBRSS& other, OBBRSS& overlap_part) const
{
return overlap(other);
}
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
{
return obb.contain(p);
}
/// @brief Merge the OBBRSS and a point
OBBRSS& operator += (const Vec3f& p)
{
obb += p;
......@@ -77,12 +84,14 @@ public:
return *this;
}
/// @brief Merge two OBBRSS
OBBRSS& operator += (const OBBRSS& other)
{
*this = *this + other;
return *this;
}
/// @brief Merge two OBBRSS
OBBRSS operator + (const OBBRSS& other) const
{
OBBRSS result;
......@@ -91,36 +100,43 @@ public:
return result;
}
/// @brief Width of the OBRSS
inline FCL_REAL width() const
{
return obb.width();
}
/// @brief Height of the OBBRSS
inline FCL_REAL height() const
{
return obb.height();
}
/// @brief Depth of the OBBRSS
inline FCL_REAL depth() const
{
return obb.depth();
}
/// @brief Volume of the OBBRSS
inline FCL_REAL volume() const
{
return obb.volume();
}
/// @brief Size of the OBBRSS (used in BV_Splitter to order two OBBRSS)
inline FCL_REAL size() const
{
return obb.size();
}
/// @brief Center of the OBBRSS
inline const Vec3f& center() const
{
return obb.center();
}
/// @brief Distance between two OBBRSS; P and Q , is not NULL, returns the nearest points
FCL_REAL distance(const OBBRSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
{
return rss.distance(other.rss, P, Q);
......@@ -128,8 +144,10 @@ public:
};
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
/// @brief Computate distance between two OBBRSS, b1 is in configuation (R0, T0) and b2 is in indentity; P and Q, is not NULL, returns the nearest points
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
......
......@@ -40,137 +40,103 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
#include <boost/math/constants/constants.hpp>
namespace fcl
{
/// @brief A class for rectangle sphere-swept bounding volume
class RSS
{
public:
/** \brief Orientation of RSS */
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS.
/// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
/** \brief position of rectangle (origin of the rectangle) */
/// @brief Origin of the rectangle in RSS
Vec3f Tr;
/** \brief side lengths of rectangle */
/// @brief Side lengths of rectangle
FCL_REAL l[2];
/** \brief radius of sphere summed with rectangle to form RSS */
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
RSS() {}
/** \brief Check collision between two RSS */
/// @brief Check collision between two RSS
bool overlap(const RSS& other) const;
/** \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
*/
/// @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
{
return overlap(other);
}
/** \brief Check whether the RSS contains a point */
/// @brief Check whether the RSS contains a point
inline bool contain(const Vec3f& p) const;
/** \brief A simple way to merge the RSS and a point, not compact.
* THIS FUNCTION STILL HAS PROBLEM!!
*/
/// @brief A simple way to merge the RSS and a point, not compact.
/// @todo This function may have some bug.
RSS& operator += (const Vec3f& p);
/** \brief Merge the RSS and another RSS */
/// @brief Merge the RSS and another RSS
inline RSS& operator += (const RSS& other)
{
*this = *this + other;
return *this;
}
/** \brief Return the merged RSS of current RSS and the other one */
/// @brief Return the merged RSS of current RSS and the other one
RSS operator + (const RSS& other) const;
/** \brief Width of the RSS */
/// @brief Width of the RSS
inline FCL_REAL width() const
{
return l[0] + 2 * r;
}
/** \brief Height of the RSS */
/// @brief Height of the RSS
inline FCL_REAL height() const
{
return l[1] + 2 * r;
}
/** \brief Depth of the RSS */
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
return 2 * r;
}
/** \brief Volume of the RSS */
/// @brief Volume of the RSS
inline FCL_REAL volume() const
{
return (l[0] * l[1] * 2 * r + 4 * 3.1415926 * r *