From 68d9064430c6a9ef32dece974936f165cfe04828 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 1 Aug 2012 01:18:00 +0000
Subject: [PATCH] rewriting part of the code. more documentation.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@150 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/BV.h | 26 +-
trunk/fcl/include/fcl/BV/AABB.h | 65 +-
trunk/fcl/include/fcl/BV/OBB.h | 69 +-
trunk/fcl/include/fcl/BV/OBBRSS.h | 24 +-
trunk/fcl/include/fcl/BV/RSS.h | 92 +-
trunk/fcl/include/fcl/BV/kDOP.h | 232 +---
trunk/fcl/include/fcl/BV/kIOS.h | 52 +-
trunk/fcl/include/fcl/BVH_front.h | 28 +-
trunk/fcl/include/fcl/BVH_internal.h | 2 +
trunk/fcl/include/fcl/BVH_model.h | 1 -
trunk/fcl/include/fcl/BVH_utility.h | 11 +-
trunk/fcl/include/fcl/BV_fitter.h | 2 +-
trunk/fcl/include/fcl/BV_splitter.h | 2 +-
trunk/fcl/include/fcl/broad_phase_collision.h | 11 +-
trunk/fcl/include/fcl/collision_data.h | 23 -
trunk/fcl/include/fcl/data_types.h | 34 +
trunk/fcl/include/fcl/input_represent.h | 2 +-
trunk/fcl/include/fcl/intersect.h | 14 +-
trunk/fcl/include/fcl/matrix_3f.h | 53 +
trunk/fcl/include/fcl/primitive.h | 159 ---
trunk/fcl/include/fcl/simple_setup.h | 12 +-
trunk/fcl/include/fcl/traversal_node_base.h | 2 +-
trunk/fcl/include/fcl/traversal_node_bvhs.h | 6 +-
trunk/fcl/include/fcl/traversal_recurse.h | 4 -
trunk/fcl/src/BV/AABB.cpp | 14 +-
trunk/fcl/src/BV/OBB.cpp | 303 ++---
trunk/fcl/src/BV/RSS.cpp | 1115 +++++++++--------
trunk/fcl/src/BV/kDOP.cpp | 232 ++++
trunk/fcl/src/BV/kIOS.cpp | 25 +-
trunk/fcl/src/BVH_utility.cpp | 12 +-
trunk/fcl/src/BV_fitter.cpp | 94 +-
trunk/fcl/src/BV_splitter.cpp | 36 +-
trunk/fcl/src/broad_phase_collision.cpp | 35 -
trunk/fcl/src/geometric_shapes_utility.cpp | 40 +-
trunk/fcl/src/intersect.cpp | 12 +-
trunk/fcl/src/simple_setup.cpp | 12 +-
trunk/fcl/src/traversal_node_bvhs.cpp | 6 +-
38 files changed, 1399 insertions(+), 1465 deletions(-)
delete mode 100644 trunk/fcl/include/fcl/primitive.h
create mode 100644 trunk/fcl/src/BV/kDOP.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index b680d9f2..458bd929 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -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})
diff --git a/trunk/fcl/include/fcl/BV.h b/trunk/fcl/include/fcl/BV.h
index 6dd515b2..549e7fa8 100644
--- a/trunk/fcl/include/fcl/BV.h
+++ b/trunk/fcl/include/fcl/BV.h
@@ -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);
}
}
diff --git a/trunk/fcl/include/fcl/BV/AABB.h b/trunk/fcl/include/fcl/BV/AABB.h
index a83666f8..cbfe1914 100644
--- a/trunk/fcl/include/fcl/BV/AABB.h
+++ b/trunk/fcl/include/fcl/BV/AABB.h
@@ -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);
diff --git a/trunk/fcl/include/fcl/BV/OBB.h b/trunk/fcl/include/fcl/BV/OBB.h
index a3978113..2020b760 100644
--- a/trunk/fcl/include/fcl/BV/OBB.h
+++ b/trunk/fcl/include/fcl/BV/OBB.h
@@ -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
diff --git a/trunk/fcl/include/fcl/BV/OBBRSS.h b/trunk/fcl/include/fcl/BV/OBBRSS.h
index 7d9b42db..9bdf0e52 100644
--- a/trunk/fcl/include/fcl/BV/OBBRSS.h
+++ b/trunk/fcl/include/fcl/BV/OBBRSS.h
@@ -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);
}
diff --git a/trunk/fcl/include/fcl/BV/RSS.h b/trunk/fcl/include/fcl/BV/RSS.h
index 130fcf04..4a9b321d 100644
--- a/trunk/fcl/include/fcl/BV/RSS.h
+++ b/trunk/fcl/include/fcl/BV/RSS.h
@@ -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 * r * r);
+ return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
}
- /** \brief Size of the RSS, for split order */
+ /// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline FCL_REAL size() const
{
- return (sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
+ return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
}
- /** \brief The RSS center */
+ /// @brief The RSS center
inline const Vec3f& center() const
{
return Tr;
}
-
- /** \brief the distance between two RSS */
+ /// @brief the distance between two RSS; P and Q, if not NULL, return the nearest points
FCL_REAL distance(const RSS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
-protected:
-
- /** \brief Clip val between a and b */
- static void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b);
-
- /** \brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments.
- * The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector
- * pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit
- * vector, "a" is the segment's length.
- * The second segment is defined as Pb + B*u, 0 <= u <= b.
- * Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function
- * instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
- * Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
- */
- static void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T);
-
- /** \brief Returns whether the nearest point on rectangle edge
- * Pb + B*u, 0 <= u <= b, to the rectangle edge,
- * Pa + A*t, 0 <= t <= a, is within the half space
- * determined by the point Pa and the direction Anorm.
- *
- * A,B, and Anorm are unit vectors.
- * T is the vector between Pa and Pb.
- */
- static bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T);
-
-public:
-
- /** \brief distance between two oriented rectangles
- * P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle
- */
- static FCL_REAL rectDistance(const Matrix3f& Rab, const Vec3f& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL);
-
};
-/** \brief distance between two RSS bounding volumes
- * P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
- * Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
- */
+/// @brief distance between two RSS bounding volumes
+/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
+/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
+
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index 474730dc..7784f7c6 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -40,250 +40,92 @@
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
-#include <cstdlib>
-#include <limits>
-#include <iostream>
-
-/** \brief Main namespace */
namespace fcl
{
-/** \brief Find the smaller and larger one of two values */
-inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv)
-{
- if(a > b)
- {
- minv = b;
- maxv = a;
- }
- else
- {
- minv = a;
- maxv = b;
- }
-}
-/** \brief Merge the interval [minv, maxv] and value p */
-inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
-{
- if(p > maxv) maxv = p;
- if(p < minv) minv = p;
-}
-
-
-/** \brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes */
-template<size_t N>
-void getDistances(const Vec3f& p, FCL_REAL d[]) {}
-
-/** \brief Specification of getDistances */
-template<>
-inline void getDistances<5>(const Vec3f& p, FCL_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
-}
-
-template<>
-inline void getDistances<6>(const Vec3f& p, FCL_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
- d[5] = p[1] - p[2];
-}
-
-template<>
-inline void getDistances<9>(const Vec3f& p, FCL_REAL d[])
-{
- d[0] = p[0] + p[1];
- d[1] = p[0] + p[2];
- d[2] = p[1] + p[2];
- d[3] = p[0] - p[1];
- d[4] = p[0] - p[2];
- d[5] = p[1] - p[2];
- d[6] = p[0] + p[1] - p[2];
- d[7] = p[0] + p[2] - p[1];
- d[8] = p[1] + p[2] - p[0];
-}
-
-/** \brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
- The KDOP structure is defined by some pairs of parallel planes defined by some axes.
- For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
- (-1,0,0) and (1,0,0) -> indices 0 and 9
- (0,-1,0) and (0,1,0) -> indices 1 and 10
- (0,0,-1) and (0,0,1) -> indices 2 and 11
- (-1,-1,0) and (1,1,0) -> indices 3 and 12
- (-1,0,-1) and (1,0,1) -> indices 4 and 13
- (0,-1,-1) and (0,1,1) -> indices 5 and 14
- (-1,1,0) and (1,-1,0) -> indices 6 and 15
- (-1,0,1) and (1,0,-1) -> indices 7 and 16
- (0,-1,1) and (0,1,-1) -> indices 8 and 17
- */
+/// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
+/// The KDOP structure is defined by some pairs of parallel planes defined by some axes.
+/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
+/// (-1,0,0) and (1,0,0) -> indices 0 and 9
+/// (0,-1,0) and (0,1,0) -> indices 1 and 10
+/// (0,0,-1) and (0,0,1) -> indices 2 and 11
+/// (-1,-1,0) and (1,1,0) -> indices 3 and 12
+/// (-1,0,-1) and (1,0,1) -> indices 4 and 13
+/// (0,-1,-1) and (0,1,1) -> indices 5 and 14
+/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
+/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
+/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
template<size_t N>
class KDOP
{
public:
- KDOP()
- {
- FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- for(size_t i = 0; i < N / 2; ++i)
- {
- dist_[i] = real_max;
- dist_[i + N / 2] = -real_max;
- }
- }
-
-
- KDOP(const Vec3f& v)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- dist_[i] = dist_[N / 2 + i] = v[i];
- }
- FCL_REAL d[(N - 6) / 2];
- getDistances<(N - 6) / 2>(v, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
- }
- }
+ /// @brief Creating kDOP containing nothing
+ KDOP();
- KDOP(const Vec3f& a, const Vec3f& b)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
- }
+ /// @brief Creating kDOP containing only one point
+ KDOP(const Vec3f& v);
- FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
- getDistances<(N - 6) / 2>(a, ad);
- getDistances<(N - 6) / 2>(b, bd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
- }
- }
+ /// @brief Creating kDOP containing two points
+ KDOP(const Vec3f& a, const Vec3f& b);
- /** \brief Check whether two KDOPs are overlapped */
- inline bool overlap(const KDOP<N>& other) const
- {
- for(size_t i = 0; i < N / 2; ++i)
- {
- if(dist_[i] > other.dist_[i + N / 2]) return false;
- if(dist_[i + N / 2] < other.dist_[i]) return false;
- }
+ /// @brief Check whether two KDOPs are overlapped
+ bool overlap(const KDOP<N>& other) const;
- return true;
- }
+ //// @brief Check whether one point is inside the KDOP
+ bool inside(const Vec3f& p) const;
- /** \brief Check whether one point is inside the KDOP */
- inline bool inside(const Vec3f& p) const
- {
- for(size_t i = 0; i < 3; ++i)
- {
- if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
- return false;
- }
+ /// @brief Merge the point and the KDOP
+ KDOP<N>& operator += (const Vec3f& p);
- FCL_REAL d[(N - 6) / 2];
- getDistances(p, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
- return false;
- }
+ /// @brief Merge two KDOPs
+ KDOP<N>& operator += (const KDOP<N>& other);
- return true;
- }
+ /// @brief Create a KDOP by mergin two KDOPs
+ KDOP<N> operator + (const KDOP<N>& other) const;
- /** \brief Merge the point and the KDOP */
- inline KDOP<N>& operator += (const Vec3f& p)
- {
- for(size_t i = 0; i < 3; ++i)
- {
- minmax(p[i], dist_[i], dist_[N / 2 + i]);
- }
-
- FCL_REAL pd[(N - 6) / 2];
- getDistances<(N - 6) / 2>(p, pd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
- }
-
- return *this;
- }
-
- /** \brief Merge two KDOPs */
- inline KDOP<N>& operator += (const KDOP<N>& other)
- {
- for(size_t i = 0; i < N / 2; ++i)
- {
- dist_[i] = std::min(other.dist_[i], dist_[i]);
- dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
- }
- return *this;
- }
-
- /** \brief Create a KDOP by mergin two KDOPs */
- inline KDOP<N> operator + (const KDOP<N>& other) const
- {
- KDOP<N> res(*this);
- return res += other;
- }
-
- /** \brief The (AABB) width */
+ /// @brief The (AABB) width
inline FCL_REAL width() const
{
return dist_[N / 2] - dist_[0];
}
- /** \brief The (AABB) height */
+ /// @brief The (AABB) height
inline FCL_REAL height() const
{
return dist_[N / 2 + 1] - dist_[1];
}
- /** \brief The (AABB) depth */
+ /// @brief The (AABB) depth
inline FCL_REAL depth() const
{
return dist_[N / 2 + 2] - dist_[2];
}
- /** \brief The (AABB) volume */
+ /// @brief The (AABB) volume
inline FCL_REAL volume() const
{
return width() * height() * depth();
}
+ /// @brief Size of the kDOP (used in BV_Splitter to order two kDOPs)
inline FCL_REAL size() const
{
return width() * width() + height() * height() + depth() * depth();
}
- /** \brief The (AABB) center */
+ /// @brief The (AABB) center
inline Vec3f center() const
{
return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
}
- /** \brief The distance between two KDOP<N>
- * Not implemented.
- */
- FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const
- {
- std::cerr << "KDOP distance not implemented!" << std::endl;
- return 0.0;
- }
+ /// @brief The distance between two KDOP<N>. Not implemented.
+ FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
- /** \brief distances to N KDOP planes */
+ /// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/trunk/fcl/include/fcl/BV/kIOS.h
index 99044ce0..bbab38bb 100644
--- a/trunk/fcl/include/fcl/BV/kIOS.h
+++ b/trunk/fcl/include/fcl/BV/kIOS.h
@@ -42,19 +42,21 @@
#include "fcl/matrix_3f.h"
#include "fcl/BV/OBB.h"
-/** \brief Main namespace */
+
namespace fcl
{
-/** \brief kIOS class */
+/// @brief A class describing the kIOS collision structure, which is a set of spheres.
class kIOS
{
+ /// @brief One sphere in kIOS
struct kIOS_Sphere
{
Vec3f o;
FCL_REAL r;
};
+ /// @brief generate one sphere enclosing two spheres
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
@@ -69,7 +71,7 @@ class kIOS
}
else /** spheres partially overlapping or disjoint */
{
- float dist = sqrt(dist2);
+ float dist = std::sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
if(dist > 0)
@@ -81,72 +83,76 @@ class kIOS
}
public:
- /** \brief The (at most) five spheres for intersection */
+ /// @brief The (at most) five spheres for intersection
kIOS_Sphere spheres[5];
- unsigned int num_spheres; // num_spheres <= 5
-
- OBB obb_bv;
+ /// @brief The number of spheres, no larger than 5
+ unsigned int num_spheres;
- kIOS() {}
+ /// @ OBB related with kIOS
+ OBB obb;
- /** \brief Check collision between two kIOS */
+ /// @brief Check collision between two kIOS
bool overlap(const kIOS& other) const;
- /** \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
- */
+ /// @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
{
return overlap(other);
}
- /** \brief Check whether the kIOS contains a point */
+ /// @brief Check whether the kIOS contains a point
inline bool contain(const Vec3f& p) const;
- /** \brief A simple way to merge the kIOS and a point */
+ /// @brief A simple way to merge the kIOS and a point
kIOS& operator += (const Vec3f& p);
- /** \brief Merge the kIOS and another kIOS */
+ /// @brief Merge the kIOS and another kIOS
kIOS& operator += (const kIOS& other)
{
*this = *this + other;
return *this;
}
- /** \brief Return the merged kIOS of current kIOS and the other one */
+ /// @brief Return the merged kIOS of current kIOS and the other one
kIOS operator + (const kIOS& other) const;
- /** \brief Center of the kIOS */
+ /// @brief Center of the kIOS
const Vec3f& center() const
{
return spheres[0].o;
}
- /** \brief width of the kIOS */
+ /// @brief Width of the kIOS
FCL_REAL width() const;
- /** \brief height of the kIOS */
+ /// @brief Height of the kIOS
FCL_REAL height() const;
- /** \brief depth of the kIOS */
+ /// @brief Depth of the kIOS
FCL_REAL depth() const;
- /** \brief volume of the kIOS */
+ /// @brief Volume of the kIOS
FCL_REAL volume() const;
- /** \brief size of the kIOS, for split order */
+ /// @brief size of the kIOS (used in BV_Splitter to order two kIOSs)
FCL_REAL size() const;
- /** \brief The distance between two kIOS */
+ /// @brief The distance between two kIOS
FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
};
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
+/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
+/// @brief Approximate distance between two kIOS bounding volumes
+/// @todo P and Q is not returned, need implementation
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
diff --git a/trunk/fcl/include/fcl/BVH_front.h b/trunk/fcl/include/fcl/BVH_front.h
index ac220815..e37c3cbd 100644
--- a/trunk/fcl/include/fcl/BVH_front.h
+++ b/trunk/fcl/include/fcl/BVH_front.h
@@ -40,28 +40,38 @@
#include <list>
-/** \brief Main namespace */
namespace fcl
{
-/** \brief A class describing the node for BVH front */
+/// @brief Front list acceleration for collision
+/// Front list is a set of internal and leaf nodes in the BVTT hierarchy, where
+/// the traversal terminates while performing a query during a given time instance. The front list reflects the subset of a
+/// BVTT that is traversed for that particular proximity query.
struct BVHFrontNode
{
- bool valid; // not valid when collision detected on the front node
+ /// @brief The nodes to start in the future, i.e. the wave front of the traversal tree.
int left, right;
- BVHFrontNode(int left_, int right_)
- {
- left = left_;
- right = right_;
+ /// @brief The front node is not valid when collision is detected on the front node.
+ bool valid;
- valid = true;
+ BVHFrontNode(int left_, int right_) : left(left_),
+ right(right_),
+ valid(true)
+ {
}
};
-/** \brief A class describing the BVH front list */
+/// @brief BVH front list is a list of front nodes.
typedef std::list<BVHFrontNode> BVHFrontList;
+/// @brief Add new front node into the front list
+inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
+{
+ if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
+}
+
+
}
#endif
diff --git a/trunk/fcl/include/fcl/BVH_internal.h b/trunk/fcl/include/fcl/BVH_internal.h
index 95897374..24f64d4a 100644
--- a/trunk/fcl/include/fcl/BVH_internal.h
+++ b/trunk/fcl/include/fcl/BVH_internal.h
@@ -47,6 +47,8 @@ namespace fcl
* |
* |-> update_begun -> updated -> .....
* */
+
+/// @brief States for BVH construction
enum BVHBuildState
{
BVH_BUILD_STATE_EMPTY, // empty state, immediately after constructor
diff --git a/trunk/fcl/include/fcl/BVH_model.h b/trunk/fcl/include/fcl/BVH_model.h
index 472f6066..67f5f0c6 100644
--- a/trunk/fcl/include/fcl/BVH_model.h
+++ b/trunk/fcl/include/fcl/BVH_model.h
@@ -41,7 +41,6 @@
#include "fcl/BVH_internal.h"
#include "fcl/BV.h"
#include "fcl/BV_node.h"
-#include "fcl/primitive.h"
#include "fcl/vec_3f.h"
#include "fcl/BV_splitter.h"
#include "fcl/BV_fitter.h"
diff --git a/trunk/fcl/include/fcl/BVH_utility.h b/trunk/fcl/include/fcl/BVH_utility.h
index 059d94e5..bbe39353 100644
--- a/trunk/fcl/include/fcl/BVH_utility.h
+++ b/trunk/fcl/include/fcl/BVH_utility.h
@@ -38,7 +38,6 @@
#ifndef FCL_BVH_UTILITY_H
#define FCL_BVH_UTILITY_H
-#include "fcl/primitive.h"
#include "fcl/vec_3f.h"
#include "fcl/BVH_model.h"
@@ -47,7 +46,7 @@ namespace fcl
{
/** \brief Expand the BVH bounding boxes according to uncertainty */
template<typename BV>
-void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, FCL_REAL r)
+void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
{
for(int i = 0; i < model.num_bvs; ++i)
{
@@ -57,7 +56,7 @@ void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, FCL_REAL r)
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
+ const Variance3f& uc = ucs[v_id];
Vec3f& v = model.vertices[bvnode.first_primitive + j];
@@ -73,13 +72,13 @@ void BVHExpand(BVHModel<BV>& model, const Uncertainty* ucs, FCL_REAL r)
}
/** \brief Expand the BVH bounding boxes according to uncertainty, for OBB */
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r);
+void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
/** \brief Expand the BVH bounding boxes according to uncertainty, for RSS */
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r);
+void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
/** \brief Estimate the uncertainty of point clouds due to sampling procedure */
-void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs);
+void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Variance3f* ucs);
/** \brief Compute the covariance matrix for a set or subset of points. if ts = null, then indices refer to points directly; otherwise refer to triangles */
diff --git a/trunk/fcl/include/fcl/BV_fitter.h b/trunk/fcl/include/fcl/BV_fitter.h
index cd312166..024391ca 100644
--- a/trunk/fcl/include/fcl/BV_fitter.h
+++ b/trunk/fcl/include/fcl/BV_fitter.h
@@ -39,7 +39,7 @@
#define FCL_BV_FITTER_H
#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
+#include "fcl/data_types.h"
#include "fcl/vec_3f.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
diff --git a/trunk/fcl/include/fcl/BV_splitter.h b/trunk/fcl/include/fcl/BV_splitter.h
index d3e1025a..ae38295c 100644
--- a/trunk/fcl/include/fcl/BV_splitter.h
+++ b/trunk/fcl/include/fcl/BV_splitter.h
@@ -39,7 +39,7 @@
#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
+#include "fcl/data_types.h"
#include "fcl/vec_3f.h"
#include "fcl/BV/OBB.h"
#include "fcl/BV/RSS.h"
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index a7388d05..b3d7d626 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -57,13 +57,6 @@ namespace fcl
{
-/** \brief collision function for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now */
-bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata);
-
-/** \brief distance function for two objects o1 and o2 in broad phase. return value means whether the broad phase can stop now. also return dist, i.e. the bmin distance till now */
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata, FCL_REAL& dist);
-
-
/** \brief return value is whether can stop now */
typedef bool (*CollisionCallBack)(CollisionObject* o1, CollisionObject* o2, void* cdata);
@@ -453,7 +446,7 @@ void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_
}
}
else if(!scene_limit.contain(new_aabb)) // previous completely in scenelimit, now not
- objs_outside_scene_limit.push_back(updated_obj);
+ objs_outside_scene_limit.push_back(updated_obj);
AABB old_overlap_aabb;
if(scene_limit.overlap(old_aabb, old_overlap_aabb))
@@ -461,7 +454,7 @@ void SpatialHashingCollisionManager<HashTable>::update(CollisionObject* updated_
AABB new_overlap_aabb;
if(scene_limit.overlap(new_aabb, new_overlap_aabb))
- hash_table->insert(new_overlap_aabb, updated_obj);
+ hash_table->insert(new_overlap_aabb, updated_obj);
obj_aabb_map[updated_obj] = new_aabb;
}
diff --git a/trunk/fcl/include/fcl/collision_data.h b/trunk/fcl/include/fcl/collision_data.h
index 585f5231..3dd5e93e 100644
--- a/trunk/fcl/include/fcl/collision_data.h
+++ b/trunk/fcl/include/fcl/collision_data.h
@@ -187,17 +187,6 @@ public:
};
-struct CollisionData
-{
- CollisionData()
- {
- done = false;
- }
-
- CollisionRequest request;
- CollisionResult result;
- bool done;
-};
struct DistanceRequest
@@ -291,18 +280,6 @@ public:
}
};
-struct DistanceData
-{
- DistanceData()
- {
- done = false;
- }
-
- bool done;
-
- DistanceRequest request;
- DistanceResult result;
-};
diff --git a/trunk/fcl/include/fcl/data_types.h b/trunk/fcl/include/fcl/data_types.h
index 16835928..f7818774 100644
--- a/trunk/fcl/include/fcl/data_types.h
+++ b/trunk/fcl/include/fcl/data_types.h
@@ -37,12 +37,46 @@
#ifndef FCL_DATA_TYPES_H
#define FCL_DATA_TYPES_H
+#include <cstddef>
#include <inttypes.h>
+namespace fcl
+{
+
typedef double FCL_REAL;
typedef uint64_t FCL_INT64;
typedef int64_t FCL_UINT64;
typedef unsigned int FCL_UINT32;
typedef int FCL_INT32;
+/// @brief Triangle with 3 indices for points
+class Triangle
+{
+ /// @brief indices for each vertex of triangle
+ std::size_t vids[3];
+
+public:
+ /// @brief Default constructor
+ Triangle() {}
+
+ /// @brief Create a triangle with given vertex indices
+ Triangle(std::size_t p1, std::size_t p2, std::size_t p3)
+ {
+ set(p1, p2, p3);
+ }
+
+ /// @brief Set the vertex indices of the triangle
+ inline void set(std::size_t p1, std::size_t p2, std::size_t p3)
+ {
+ vids[0] = p1; vids[1] = p2; vids[2] = p3;
+ }
+
+ /// @access the triangle index
+ inline std::size_t operator[](int i) const { return vids[i]; }
+
+ inline std::size_t& operator[](int i) { return vids[i]; }
+};
+
+}
+
#endif
diff --git a/trunk/fcl/include/fcl/input_represent.h b/trunk/fcl/include/fcl/input_represent.h
index 4fd86cf4..b168d6f0 100644
--- a/trunk/fcl/include/fcl/input_represent.h
+++ b/trunk/fcl/include/fcl/input_represent.h
@@ -37,7 +37,7 @@
#ifndef FCL_INPUT_REPRESENT
#define FCL_INPUT_REPRESENT
-#include "fcl/primitive.h"
+#include "fcl/data_types.h"
#include "fcl/vec_3f.h"
diff --git a/trunk/fcl/include/fcl/intersect.h b/trunk/fcl/include/fcl/intersect.h
index fdd00cc3..551f5f79 100644
--- a/trunk/fcl/include/fcl/intersect.h
+++ b/trunk/fcl/include/fcl/intersect.h
@@ -39,7 +39,7 @@
#include "fcl/transform.h"
#include "fcl/BVH_internal.h"
-#include "fcl/primitive.h"
+#include "fcl/data_types.h"
#if USE_SVMLIGHT
extern "C"
@@ -154,18 +154,18 @@ public:
#if USE_SVMLIGHT
- static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
+ static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
+ Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
const CloudClassifierParam& solver, bool scaling = true);
- static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
+ static FCL_REAL intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
+ Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling = true);
- static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3);
- static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+ static FCL_REAL intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Matrix3f& R, const Vec3f& T);
#endif
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
index 763e4268..5eb4754b 100644
--- a/trunk/fcl/include/fcl/matrix_3f.h
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -417,6 +417,59 @@ static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
return o;
}
+
+
+/// @brief Class for variance matrix in 3d
+class Variance3f
+{
+public:
+ /// @brief Variation matrix
+ Matrix3f Sigma;
+
+ /// @brief Variations along the eign axes
+ FCL_REAL sigma[3];
+
+ /// @brief Eigen axes of the variation matrix
+ Vec3f axis[3];
+
+ Variance3f() {}
+
+ Variance3f(const Matrix3f& S) : Sigma(S)
+ {
+ init();
+ }
+
+ /// @brief init the Variance
+ void init()
+ {
+ eigen(Sigma, sigma, axis);
+ }
+
+ /// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized as a square variation matrix
+ Variance3f& sqrt()
+ {
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ if(sigma[i] < 0) sigma[i] = 0;
+ sigma[i] = std::sqrt(sigma[i]);
+ }
+
+
+ Sigma.setZero();
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ for(std::size_t j = 0; j < 3; ++j)
+ {
+ Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
+ Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
+ Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
+ }
+ }
+
+ return *this;
+ }
+};
+
}
diff --git a/trunk/fcl/include/fcl/primitive.h b/trunk/fcl/include/fcl/primitive.h
deleted file mode 100644
index f5396422..00000000
--- a/trunk/fcl/include/fcl/primitive.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2011, Willow Garage, Inc.
- * 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 Willow Garage, Inc. 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.
- */
-
-/** \author Jia Pan */
-
-#ifndef COLLISION_CHECKING_PRIMITIVE_H
-#define COLLISION_CHECKING_PRIMITIVE_H
-
-#include "fcl/BVH_internal.h"
-#include "fcl/vec_3f.h"
-#include "fcl/matrix_3f.h"
-
-/** \brief Main namespace */
-namespace fcl
-{
-
-/** \brief Uncertainty information */
-struct Uncertainty
-{
- Uncertainty() {}
-
- Uncertainty(Matrix3f& Sigma_) : Sigma(Sigma_)
- {
- preprocess();
- }
-
- /** preprocess performs the eigen decomposition on the Sigma matrix */
- void preprocess()
- {
- eigen(Sigma, sigma, axis);
- }
-
- /** sqrt performs the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized
- * as a square variation matrix
- */
- void sqrt()
- {
- for(int i = 0; i < 3; ++i)
- {
- if(sigma[i] < 0) sigma[i] = 0;
- sigma[i] = std::sqrt(sigma[i]);
- }
-
-
- Sigma.setZero();
- for(int i = 0; i < 3; ++i)
- {
- for(int j = 0; j < 3; ++j)
- {
- Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
- Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
- Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
- }
- }
- }
-
- /** \brief Variation matrix for uncertainty */
- Matrix3f Sigma;
-
- /** \brief Variations along the eigen axes */
- FCL_REAL sigma[3];
-
- /** \brief eigen axes of uncertainty matrix */
- Vec3f axis[3];
-};
-
-/** \brief Simple triangle with 3 indices for points */
-struct Triangle
-{
- unsigned int vids[3];
-
- Triangle() {}
-
- Triangle(unsigned int p1, unsigned int p2, unsigned int p3)
- {
- set(p1, p2, p3);
- }
-
- inline void set(unsigned int p1, unsigned int p2, unsigned int p3)
- {
- vids[0] = p1; vids[1] = p2; vids[2] = p3;
- }
-
- inline unsigned int operator[](int i) const { return vids[i]; }
-
- inline unsigned int& operator[](int i) { return vids[i]; }
-};
-
-
-/** \brief Simple edge with two indices for its endpoints */
-struct Edge
-{
- unsigned int vids[2];
- unsigned int fids[2];
-
- Edge()
- {
- vids[0] = -1; vids[1] = -1;
- fids[0] = -1; fids[1] = -1;
- }
-
- Edge(unsigned int vid0, unsigned int vid1, unsigned int fid)
- {
- vids[0] = vid0;
- vids[1] = vid1;
- fids[0] = fid;
- }
-
- /** \brief Whether two edges are the same, assuming belongs to the same object */
- bool operator == (const Edge& other) const
- {
- return (vids[0] == other.vids[0]) && (vids[1] == other.vids[1]);
- }
-
- bool operator < (const Edge& other) const
- {
- if(vids[0] == other.vids[0])
- return vids[1] < other.vids[1];
-
- return vids[0] < other.vids[0];
- }
-
-};
-
-}
-
-
-#endif
diff --git a/trunk/fcl/include/fcl/simple_setup.h b/trunk/fcl/include/fcl/simple_setup.h
index 1d1a6518..e926b672 100644
--- a/trunk/fcl/include/fcl/simple_setup.h
+++ b/trunk/fcl/include/fcl/simple_setup.h
@@ -728,11 +728,11 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node,
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
+ node.uc1.reset(new Variance3f[model1.num_vertices]);
+ node.uc2.reset(new Variance3f[model2.num_vertices]);
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
+ estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
+ estimateSamplingVariance(model2.vertices, model2.num_vertices, node.uc2.get());
BVHExpand(model1, node.uc1.get(), expand_r);
BVHExpand(model2, node.uc2.get(), expand_r);
@@ -819,9 +819,9 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node,
node.vertices2 = model2.vertices;
node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
+ node.uc1.reset(new Variance3f[model1.num_vertices]);
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
+ estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
BVHExpand(model1, node.uc1.get(), expand_r);
diff --git a/trunk/fcl/include/fcl/traversal_node_base.h b/trunk/fcl/include/fcl/traversal_node_base.h
index c85631fb..d7ca412b 100644
--- a/trunk/fcl/include/fcl/traversal_node_base.h
+++ b/trunk/fcl/include/fcl/traversal_node_base.h
@@ -37,7 +37,7 @@
#ifndef FCL_TRAVERSAL_NODE_BASE_H
#define FCL_TRAVERSAL_NODE_BASE_H
-#include "fcl/primitive.h"
+#include "fcl/data_types.h"
#include "fcl/transform.h"
#include "fcl/collision_data.h"
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 673b706c..19ad2120 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -393,8 +393,8 @@ public:
Vec3f* vertices1;
Vec3f* vertices2;
- boost::shared_array<Uncertainty> uc1;
- boost::shared_array<Uncertainty> uc2;
+ boost::shared_array<Variance3f> uc1;
+ boost::shared_array<Variance3f> uc2;
mutable std::vector<BVHPointCollisionPair> pairs;
@@ -491,7 +491,7 @@ public:
Vec3f* vertices1;
Vec3f* vertices2;
- boost::shared_array<Uncertainty> uc1;
+ boost::shared_array<Variance3f> uc1;
Triangle* tri_indices2;
mutable std::vector<BVHPointCollisionPair> pairs;
diff --git a/trunk/fcl/include/fcl/traversal_recurse.h b/trunk/fcl/include/fcl/traversal_recurse.h
index 604258a1..c28fcbad 100644
--- a/trunk/fcl/include/fcl/traversal_recurse.h
+++ b/trunk/fcl/include/fcl/traversal_recurse.h
@@ -46,10 +46,6 @@
namespace fcl
{
-inline void updateFrontList(BVHFrontList* front_list, int b1, int b2)
-{
- if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
-}
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, BVHFrontList* front_list);
diff --git a/trunk/fcl/src/BV/AABB.cpp b/trunk/fcl/src/BV/AABB.cpp
index ed3e0e92..78e41f7a 100644
--- a/trunk/fcl/src/BV/AABB.cpp
+++ b/trunk/fcl/src/BV/AABB.cpp
@@ -42,17 +42,15 @@
namespace fcl
{
-AABB::AABB()
+AABB::AABB() : min_(std::numeric_limits<FCL_REAL>::max()),
+ max_(-std::numeric_limits<FCL_REAL>::max())
{
- FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- min_.setValue(real_max, real_max, real_max);
- max_.setValue(-real_max, -real_max, -real_max);
}
FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
FCL_REAL result = 0;
- for(size_t i = 0; i < 3; ++i)
+ for(std::size_t i = 0; i < 3; ++i)
{
const FCL_REAL& amin = min_[i];
const FCL_REAL& amax = max_[i];
@@ -99,13 +97,13 @@ FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
}
}
- return sqrt(result);
+ return std::sqrt(result);
}
FCL_REAL AABB::distance(const AABB& other) const
{
FCL_REAL result = 0;
- for(size_t i = 0; i < 3; ++i)
+ for(std::size_t i = 0; i < 3; ++i)
{
const FCL_REAL& amin = min_[i];
const FCL_REAL& amax = max_[i];
@@ -124,7 +122,7 @@ FCL_REAL AABB::distance(const AABB& other) const
}
}
- return sqrt(result);
+ return std::sqrt(result);
}
}
diff --git a/trunk/fcl/src/BV/OBB.cpp b/trunk/fcl/src/BV/OBB.cpp
index dbe3c074..68fc42c3 100644
--- a/trunk/fcl/src/BV/OBB.cpp
+++ b/trunk/fcl/src/BV/OBB.cpp
@@ -44,71 +44,135 @@
namespace fcl
{
-bool OBB::overlap(const OBB& other) const
+/// @brief Compute the 8 vertices of a OBB
+inline void computeVertices(const OBB& b, Vec3f vertices[8])
{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.To - To; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ const Vec3f* axis = b.axis;
+ const Vec3f& extent = b.extent;
+ const Vec3f& To = b.To;
- // R is row first
- return !obbDisjoint(R, T, extent, other.extent);
-}
+ Vec3f extAxis0 = axis[0] * extent[0];
+ Vec3f extAxis1 = axis[1] * extent[1];
+ Vec3f extAxis2 = axis[2] * extent[2];
+ vertices[0] = To - extAxis0 - extAxis1 - extAxis2;
+ vertices[1] = To + extAxis0 - extAxis1 - extAxis2;
+ vertices[2] = To + extAxis0 + extAxis1 - extAxis2;
+ vertices[3] = To - extAxis0 + extAxis1 - extAxis2;
+ vertices[4] = To - extAxis0 - extAxis1 + extAxis2;
+ vertices[5] = To + extAxis0 - extAxis1 + extAxis2;
+ vertices[6] = To + extAxis0 + extAxis1 + extAxis2;
+ vertices[7] = To - extAxis0 + extAxis1 + extAxis2;
+}
-bool OBB::contain(const Vec3f& p) const
+/// @brief OBB merge method when the centers of two smaller OBB are far away
+inline OBB merge_largedist(const OBB& b1, const OBB& b2)
{
- Vec3f local_p = p - To;
- FCL_REAL proj = local_p.dot(axis[0]);
- if((proj > extent[0]) || (proj < -extent[0]))
- return false;
+ OBB b;
+ Vec3f vertex[16];
+ computeVertices(b1, vertex);
+ computeVertices(b2, vertex + 8);
+ Matrix3f M;
+ Vec3f E[3];
+ FCL_REAL s[3] = {0, 0, 0};
- proj = local_p.dot(axis[1]);
- if((proj > extent[1]) || (proj < -extent[1]))
- return false;
+ // obb axes
+ Vec3f& R0 = b.axis[0];
+ Vec3f& R1 = b.axis[1];
+ Vec3f& R2 = b.axis[2];
- proj = local_p.dot(axis[2]);
- if((proj > extent[2]) || (proj < -extent[2]))
- return false;
+ R0 = b1.To - b2.To;
+ R0.normalize();
- return true;
-}
+ Vec3f vertex_proj[16];
+ for(int i = 0; i < 16; ++i)
+ vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
-/** \brief A simple way to add new point to the OBB, not compact. */
-OBB& OBB::operator += (const Vec3f& p)
-{
- OBB bvp;
- bvp.To = p;
- bvp.axis[0] = axis[0];
- bvp.axis[1] = axis[1];
- bvp.axis[2] = axis[2];
- bvp.extent.setValue(0);
+ getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
+ eigen(M, s, E);
- *this += bvp;
- return *this;
+ int min, mid, max;
+ if (s[0] > s[1]) { max = 0; min = 1; }
+ else { min = 0; max = 1; }
+ if (s[2] < s[min]) { mid = min; min = 2; }
+ else if (s[2] > s[max]) { mid = max; max = 2; }
+ else { mid = 2; }
+
+
+ R1.setValue(E[0][max], E[1][max], E[2][max]);
+ R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
+
+ // set obb centers and extensions
+ Vec3f center, extent;
+ getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent);
+
+ b.To = center;
+ b.extent = extent;
+
+ return b;
}
-OBB OBB::operator + (const OBB& other) const
+
+/// @brief OBB merge method when the centers of two smaller OBB are close
+inline OBB merge_smalldist(const OBB& b1, const OBB& b2)
{
- Vec3f center_diff = To - other.To;
- FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
- FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
- if(center_diff.length() > 2 * (max_extent + max_extent2))
+ OBB b;
+ b.To = (b1.To + b2.To) * 0.5;
+ Quaternion3f q0, q1;
+ q0.fromAxes(b1.axis);
+ q1.fromAxes(b2.axis);
+ if(q0.dot(q1) < 0)
+ q1 = -q1;
+
+ Quaternion3f q = q0 + q1;
+ FCL_REAL inv_length = 1.0 / std::sqrt(q.dot(q));
+ q = q * inv_length;
+ q.toAxes(b.axis);
+
+
+ Vec3f vertex[8], diff;
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
+ Vec3f pmin(real_max, real_max, real_max);
+ Vec3f pmax(-real_max, -real_max, -real_max);
+
+ computeVertices(b1, vertex);
+ for(int i = 0; i < 8; ++i)
{
- return merge_largedist(*this, other);
+ diff = vertex[i] - b.To;
+ for(int j = 0; j < 3; ++j)
+ {
+ FCL_REAL dot = diff.dot(b.axis[j]);
+ if(dot > pmax[j])
+ pmax[j] = dot;
+ else if(dot < pmin[j])
+ pmin[j] = dot;
+ }
}
- else
+
+ computeVertices(b2, vertex);
+ for(int i = 0; i < 8; ++i)
{
- return merge_smalldist(*this, other);
+ diff = vertex[i] - b.To;
+ for(int j = 0; j < 3; ++j)
+ {
+ FCL_REAL dot = diff.dot(b.axis[j]);
+ if(dot > pmax[j])
+ pmax[j] = dot;
+ else if(dot < pmin[j])
+ pmin[j] = dot;
+ }
}
-}
+ for(int j = 0; j < 3; ++j)
+ {
+ b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
+ b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
+ }
+
+ return b;
+}
-bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
+bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f& b)
{
register FCL_REAL t, s;
const FCL_REAL reps = 1e-6;
@@ -234,125 +298,66 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
}
-void OBB::computeVertices(Vec3f vertex[8]) const
+
+bool OBB::overlap(const OBB& other) const
{
- Vec3f extAxis0 = axis[0] * extent[0];
- Vec3f extAxis1 = axis[1] * extent[1];
- Vec3f extAxis2 = axis[2] * extent[2];
+ /// compute what transform [R,T] that takes us from cs1 to cs2.
+ /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+ /// First compute the rotation part, then translation part
+ Vec3f t = other.To - To; // T2 - T1
+ Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
- vertex[0] = To - extAxis0 - extAxis1 - extAxis2;
- vertex[1] = To + extAxis0 - extAxis1 - extAxis2;
- vertex[2] = To + extAxis0 + extAxis1 - extAxis2;
- vertex[3] = To - extAxis0 + extAxis1 - extAxis2;
- vertex[4] = To - extAxis0 - extAxis1 + extAxis2;
- vertex[5] = To + extAxis0 - extAxis1 + extAxis2;
- vertex[6] = To + extAxis0 + extAxis1 + extAxis2;
- vertex[7] = To - extAxis0 + extAxis1 + extAxis2;
+ return !obbDisjoint(R, T, extent, other.extent);
}
-OBB OBB::merge_largedist(const OBB& b1, const OBB& b2)
+bool OBB::contain(const Vec3f& p) const
{
- OBB b;
- Vec3f vertex[16];
- b1.computeVertices(vertex);
- b2.computeVertices(vertex + 8);
- Matrix3f M;
- Vec3f E[3];
- FCL_REAL s[3] = {0, 0, 0};
-
- // obb axes
- Vec3f& R0 = b.axis[0];
- Vec3f& R1 = b.axis[1];
- Vec3f& R2 = b.axis[2];
-
- R0 = b1.To - b2.To;
- R0.normalize();
-
- Vec3f vertex_proj[16];
- for(int i = 0; i < 16; ++i)
- vertex_proj[i] = vertex[i] - R0 * vertex[i].dot(R0);
-
- getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
- eigen(M, s, E);
-
- int min, mid, max;
- if (s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if (s[2] < s[min]) { mid = min; min = 2; }
- else if (s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
-
- R1.setValue(E[0][max], E[1][max], E[2][max]);
- R2.setValue(E[0][mid], E[1][mid], E[2][mid]);
+ Vec3f local_p = p - To;
+ FCL_REAL proj = local_p.dot(axis[0]);
+ if((proj > extent[0]) || (proj < -extent[0]))
+ return false;
- // set obb centers and extensions
- Vec3f center, extent;
- getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axis, center, extent);
+ proj = local_p.dot(axis[1]);
+ if((proj > extent[1]) || (proj < -extent[1]))
+ return false;
- b.To = center;
- b.extent = extent;
+ proj = local_p.dot(axis[2]);
+ if((proj > extent[2]) || (proj < -extent[2]))
+ return false;
- return b;
+ return true;
}
-OBB OBB::merge_smalldist(const OBB& b1, const OBB& b2)
+OBB& OBB::operator += (const Vec3f& p)
{
- OBB b;
- b.To = (b1.To + b2.To) * 0.5;
- Quaternion3f q0, q1;
- q0.fromAxes(b1.axis);
- q1.fromAxes(b2.axis);
- if(q0.dot(q1) < 0)
- q1 = -q1;
-
- Quaternion3f q = q0 + q1;
- FCL_REAL inv_length = 1.0 / sqrt(q.dot(q));
- q = q * inv_length;
- q.toAxes(b.axis);
-
-
- Vec3f vertex[8], diff;
- FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- Vec3f pmin(real_max, real_max, real_max);
- Vec3f pmax(-real_max, -real_max, -real_max);
+ OBB bvp;
+ bvp.To = p;
+ bvp.axis[0] = axis[0];
+ bvp.axis[1] = axis[1];
+ bvp.axis[2] = axis[2];
+ bvp.extent.setValue(0);
- b1.computeVertices(vertex);
- for(int i = 0; i < 8; ++i)
- {
- diff = vertex[i] - b.To;
- for(int j = 0; j < 3; ++j)
- {
- FCL_REAL dot = diff.dot(b.axis[j]);
- if(dot > pmax[j])
- pmax[j] = dot;
- else if(dot < pmin[j])
- pmin[j] = dot;
- }
- }
+ *this += bvp;
+ return *this;
+}
- b2.computeVertices(vertex);
- for(int i = 0; i < 8; ++i)
+OBB OBB::operator + (const OBB& other) const
+{
+ Vec3f center_diff = To - other.To;
+ FCL_REAL max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
+ FCL_REAL max_extent2 = std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
+ if(center_diff.length() > 2 * (max_extent + max_extent2))
{
- diff = vertex[i] - b.To;
- for(int j = 0; j < 3; ++j)
- {
- FCL_REAL dot = diff.dot(b.axis[j]);
- if(dot > pmax[j])
- pmax[j] = dot;
- else if(dot < pmin[j])
- pmin[j] = dot;
- }
+ return merge_largedist(*this, other);
}
-
- for(int j = 0; j < 3; ++j)
+ else
{
- b.To += (b.axis[j] * (0.5 * (pmax[j] + pmin[j])));
- b.extent[j] = 0.5 * (pmax[j] - pmin[j]);
+ return merge_smalldist(*this, other);
}
-
- return b;
}
@@ -362,7 +367,7 @@ FCL_REAL OBB::distance(const OBB& other, Vec3f* P, Vec3f* Q) const
return 0.0;
}
-// R is row first
+
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
@@ -376,7 +381,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
- return !OBB::obbDisjoint(R, T, b1.extent, b2.extent);
+ return !obbDisjoint(R, T, b1.extent, b2.extent);
}
}
diff --git a/trunk/fcl/src/BV/RSS.cpp b/trunk/fcl/src/BV/RSS.cpp
index 1cf86b47..ae98c25b 100644
--- a/trunk/fcl/src/BV/RSS.cpp
+++ b/trunk/fcl/src/BV/RSS.cpp
@@ -40,465 +40,221 @@
namespace fcl
{
-bool RSS::overlap(const RSS& other) const
+/// @brief Clip value between a and b
+void clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b)
{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.Tr - Tr; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ if(val < a) val = a;
+ else if(val > b) val = b;
+}
- FCL_REAL dist = rectDistance(R, T, l, other.l);
- if(dist <= (r + other.r)) return true;
- return false;
+/// @brief Finds the parameters t & u corresponding to the two closest points on a pair of line segments.
+/// The first segment is defined as Pa + A*t, 0 <= t <= a, where "Pa" is one endpoint of the segment, "A" is a unit vector
+/// pointing to the other endpoint, and t is a scalar that produces all the points between the two endpoints. Since "A" is a unit
+/// vector, "a" is the segment's length.
+/// The second segment is defined as Pb + B*u, 0 <= u <= b.
+/// Many of the terms needed by the algorithm are already computed for other purposes,so we just pass these terms into the function
+/// instead of complete specifications of each segment. "T" in the dot products is the vector betweeen Pa and Pb.
+/// Reference: "On fast computation of distance between line segments." Vladimir J. Lumelsky, in Information Processing Letters, no. 21, pages 55-61, 1985.
+void segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
+{
+ FCL_REAL denom = 1 - A_dot_B * A_dot_B;
+
+ if(denom == 0) t = 0;
+ else
+ {
+ t = (A_dot_T - B_dot_T * A_dot_B) / denom;
+ clipToRange(t, 0, a);
+ }
+
+ u = t * A_dot_B - B_dot_T;
+ if(u < 0)
+ {
+ u = 0;
+ t = A_dot_T;
+ clipToRange(t, 0, a);
+ }
+ else if(u > b)
+ {
+ u = b;
+ t = u * A_dot_B + A_dot_T;
+ clipToRange(t, 0, a);
+ }
}
-bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
+/// @brief Returns whether the nearest point on rectangle edge
+/// Pb + B*u, 0 <= u <= b, to the rectangle edge,
+/// Pa + A*t, 0 <= t <= a, is within the half space
+/// determined by the point Pa and the direction Anorm.
+/// A,B, and Anorm are unit vectors. T is the vector between Pa and Pb.
+bool inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
{
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+ if(fabs(Anorm_dot_B) < 1e-7) return false;
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ FCL_REAL t, u, v;
- Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
- Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
+ u = -Anorm_dot_T / Anorm_dot_B;
+ clipToRange(u, 0, b);
+
+ t = u * A_dot_B + A_dot_T;
+ clipToRange(t, 0, a);
+
+ v = t * A_dot_B - B_dot_T;
- FCL_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l);
- if(dist <= (b1.r + b2.r)) return true;
+ if(Anorm_dot_B > 0)
+ {
+ if(v > (u + 1e-7)) return true;
+ }
+ else
+ {
+ if(v < (u - 1e-7)) return true;
+ }
return false;
}
-bool RSS::contain(const Vec3f& p) const
+
+/// @brief Distance between two oriented rectangles; P and Q (optional return values) are the closest points in the rectangles, both are in the local frame of the first rectangle.
+FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P = NULL, Vec3f* Q = NULL)
{
- Vec3f local_p = p - Tr;
- FCL_REAL proj0 = local_p.dot(axis[0]);
- FCL_REAL proj1 = local_p.dot(axis[1]);
- FCL_REAL proj2 = local_p.dot(axis[2]);
- FCL_REAL abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
+ FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
- // projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ A0_dot_B0 = Rab(0, 0);
+ A0_dot_B1 = Rab(0, 1);
+ A1_dot_B0 = Rab(1, 0);
+ A1_dot_B1 = Rab(1, 1);
+
+ FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
+ FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
+
+ aA0_dot_B0 = a[0] * A0_dot_B0;
+ aA0_dot_B1 = a[0] * A0_dot_B1;
+ aA1_dot_B0 = a[1] * A1_dot_B0;
+ aA1_dot_B1 = a[1] * A1_dot_B1;
+ bA0_dot_B0 = b[0] * A0_dot_B0;
+ bA1_dot_B0 = b[0] * A1_dot_B0;
+ bA0_dot_B1 = b[1] * A0_dot_B1;
+ bA1_dot_B1 = b[1] * A1_dot_B1;
+
+ Vec3f Tba = Rab.transposeTimes(Tab);
+
+ Vec3f S;
+ FCL_REAL t, u;
+
+ // determine if any edge pair contains the closest points
+
+ FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x;
+ FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x;
+ FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
+
+ ALL_x = -Tba[0];
+ ALU_x = ALL_x + aA1_dot_B0;
+ AUL_x = ALL_x + aA0_dot_B0;
+ AUU_x = ALU_x + aA0_dot_B0;
+
+ if(ALL_x < ALU_x)
{
- if(abs_proj2 < r)
- return true;
- else
- return false;
+ LA1_lx = ALL_x;
+ LA1_ux = ALU_x;
+ UA1_lx = AUL_x;
+ UA1_ux = AUU_x;
}
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+ else
{
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(proj0, y, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
+ LA1_lx = ALU_x;
+ LA1_ux = ALL_x;
+ UA1_lx = AUU_x;
+ UA1_ux = AUL_x;
}
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+
+ BLL_x = Tab[0];
+ BLU_x = BLL_x + bA0_dot_B1;
+ BUL_x = BLL_x + bA0_dot_B0;
+ BUU_x = BLU_x + bA0_dot_B0;
+
+ if(BLL_x < BLU_x)
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- Vec3f v(x, proj1, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
+ LB1_lx = BLL_x;
+ LB1_ux = BLU_x;
+ UB1_lx = BUL_x;
+ UB1_ux = BUU_x;
}
else
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(x, y, 0);
- if((proj - v).sqrLength() < r * r)
- return true;
- else
- return false;
+ LB1_lx = BLU_x;
+ LB1_ux = BLL_x;
+ UB1_lx = BUU_x;
+ UB1_ux = BUL_x;
}
-}
-RSS& RSS::operator += (const Vec3f& p)
-{
- Vec3f local_p = p - Tr;
- FCL_REAL proj0 = local_p.dot(axis[0]);
- FCL_REAL proj1 = local_p.dot(axis[1]);
- FCL_REAL proj2 = local_p.dot(axis[2]);
- FCL_REAL abs_proj2 = fabs(proj2);
- Vec3f proj(proj0, proj1, proj2);
+ // UA1, UB1
- // projection is within the rectangle
- if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ if((UA1_ux > b[0]) && (UB1_ux > a[0]))
{
- if(abs_proj2 < r)
- ; // do nothing
- else
+ if(((UA1_lx > b[0]) ||
+ inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
+ A1_dot_B1, aA0_dot_B1 - Tba[1],
+ -Tab[1] - bA1_dot_B0))
+ &&
+ ((UB1_lx > a[0]) ||
+ inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
+ A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
{
- r = 0.5 * (r + abs_proj2); // enlarge the r
- // change RSS origin position
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
+ Tba[1] - aA0_dot_B1);
+
+ S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
+ S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
+
+ if(P && Q)
+ {
+ P->setValue(a[0], t, 0);
+ *Q = S + (*P);
+ }
+
+ return S.length();
}
}
- else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+
+
+ // UA1, LB1
+
+ if((UA1_lx < 0) && (LB1_ux > a[0]))
{
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(proj0, y, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
+ if(((UA1_ux < 0) ||
+ inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
+ A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
+ &&
+ ((LB1_lx > a[0]) ||
+ inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
+ A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
{
- if(abs_proj2 < r)
+ segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
+
+ S[0] = Tab[0] + Rab(0, 1) * u - a[0];
+ S[1] = Tab[1] + Rab(1, 1) * u - t;
+ S[2] = Tab[2] + Rab(2, 1) * u;
+
+ if(P && Q)
{
- FCL_REAL delta_y = - sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
- l[1] += delta_y;
- if(proj1 < 0)
- Tr[1] -= delta_y;
+ P->setValue(a[0], t, 0);
+ *Q = S + (*P);
}
- else
- {
- FCL_REAL delta_y = fabs(proj1 - y);
- l[1] += delta_y;
- if(proj1 < 0)
- Tr[1] -= delta_y;
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
+ return S.length();
}
}
- else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+
+ // LA1, UB1
+
+ if((LA1_ux > b[0]) && (UB1_lx < 0))
{
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- Vec3f v(x, proj1, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
- {
- if(abs_proj2 < r)
- {
- FCL_REAL delta_x = - sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
- l[0] += delta_x;
- if(proj0 < 0)
- Tr[0] -= delta_x;
- }
- else
- {
- FCL_REAL delta_x = fabs(proj0 - x);
- l[0] += delta_x;
- if(proj0 < 0)
- Tr[0] -= delta_x;
-
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- }
- else
- {
- FCL_REAL x = (proj0 > 0) ? l[0] : 0;
- FCL_REAL y = (proj1 > 0) ? l[1] : 0;
- Vec3f v(x, y, 0);
- FCL_REAL new_r_sqr = (proj - v).sqrLength();
- if(new_r_sqr < r * r)
- ; // do nothing
- else
- {
- if(abs_proj2 < r)
- {
- FCL_REAL diag = sqrt(new_r_sqr - proj2 * proj2);
- FCL_REAL delta_diag = - sqrt(r * r - proj2 * proj2) + diag;
-
- FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
- FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
-
- if(proj0 < 0 && proj1 < 0)
- {
- Tr[0] -= delta_x;
- Tr[1] -= delta_y;
- }
- }
- else
- {
- FCL_REAL delta_x = fabs(proj0 - x);
- FCL_REAL delta_y = fabs(proj1 - y);
-
- l[0] += delta_x;
- l[1] += delta_y;
-
- if(proj0 < 0 && proj1 < 0)
- {
- Tr[0] -= delta_x;
- Tr[1] -= delta_y;
- }
-
- if(proj2 > 0)
- Tr[2] += 0.5 * (abs_proj2 - r);
- else
- Tr[2] -= 0.5 * (abs_proj2 - r);
- }
- }
- }
-
- return *this;
-}
-
-RSS RSS::operator + (const RSS& other) const
-{
- RSS bv;
-
- Vec3f v[16];
- Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
- Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
- Vec3f d0_neg = other.axis[0] * (-other.r);
- Vec3f d1_neg = other.axis[1] * (-other.r);
- Vec3f d2_pos = other.axis[2] * other.r;
- Vec3f d2_neg = other.axis[2] * (-other.r);
-
- v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
- v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
- v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
- v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
- v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
- v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
- v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
- v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
-
- d0_pos = axis[0] * (l[0] + r);
- d1_pos = axis[1] * (l[1] + r);
- d0_neg = axis[0] * (-r);
- d1_neg = axis[1] * (-r);
- d2_pos = axis[2] * r;
- d2_neg = axis[2] * (-r);
-
- v[8] = Tr + d0_pos + d1_pos + d2_pos;
- v[9] = Tr + d0_pos + d1_pos + d2_neg;
- v[10] = Tr + d0_pos + d1_neg + d2_pos;
- v[11] = Tr + d0_pos + d1_neg + d2_neg;
- v[12] = Tr + d0_neg + d1_pos + d2_pos;
- v[13] = Tr + d0_neg + d1_pos + d2_neg;
- v[14] = Tr + d0_neg + d1_neg + d2_pos;
- v[15] = Tr + d0_neg + d1_neg + d2_neg;
-
-
- Matrix3f M; // row first matrix
- Vec3f E[3]; // row first eigen-vectors
- FCL_REAL s[3] = {0, 0, 0};
-
- getCovariance(v, NULL, NULL, NULL, 16, M);
- eigen(M, s, E);
-
- int min, mid, max;
- if(s[0] > s[1]) { max = 0; min = 1; }
- else { min = 0; max = 1; }
- if(s[2] < s[min]) { mid = min; min = 2; }
- else if(s[2] > s[max]) { mid = max; max = 2; }
- else { mid = 2; }
-
- // column first matrix, as the axis in RSS
- bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
- bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
- bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
- E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
- E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-
- // set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
-
- return bv;
-}
-
-FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
-{
- // compute what transform [R,T] that takes us from cs1 to cs2.
- // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
- // First compute the rotation part, then translation part
- Vec3f t = other.Tr - Tr; // T2 - T1
- Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
-
- FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
- dist -= (r + other.r);
- return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
-}
-
-FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
-{
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
-
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
-
- Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
-
- Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
-
- FCL_REAL dist = RSS::rectDistance(R, T, b1.l, b2.l, P, Q);
- dist -= (b1.r + b2.r);
- return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
-}
-
-
-FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2], const FCL_REAL b[2], Vec3f* P, Vec3f* Q)
-{
- FCL_REAL A0_dot_B0, A0_dot_B1, A1_dot_B0, A1_dot_B1;
-
- A0_dot_B0 = Rab(0, 0);
- A0_dot_B1 = Rab(0, 1);
- A1_dot_B0 = Rab(1, 0);
- A1_dot_B1 = Rab(1, 1);
-
- FCL_REAL aA0_dot_B0, aA0_dot_B1, aA1_dot_B0, aA1_dot_B1;
- FCL_REAL bA0_dot_B0, bA0_dot_B1, bA1_dot_B0, bA1_dot_B1;
-
- aA0_dot_B0 = a[0] * A0_dot_B0;
- aA0_dot_B1 = a[0] * A0_dot_B1;
- aA1_dot_B0 = a[1] * A1_dot_B0;
- aA1_dot_B1 = a[1] * A1_dot_B1;
- bA0_dot_B0 = b[0] * A0_dot_B0;
- bA1_dot_B0 = b[0] * A1_dot_B0;
- bA0_dot_B1 = b[1] * A0_dot_B1;
- bA1_dot_B1 = b[1] * A1_dot_B1;
-
- Vec3f Tba = Rab.transposeTimes(Tab);
-
- Vec3f S;
- FCL_REAL t, u;
-
- // determine if any edge pair contains the closest points
-
- FCL_REAL ALL_x, ALU_x, AUL_x, AUU_x;
- FCL_REAL BLL_x, BLU_x, BUL_x, BUU_x;
- FCL_REAL LA1_lx, LA1_ux, UA1_lx, UA1_ux, LB1_lx, LB1_ux, UB1_lx, UB1_ux;
-
- ALL_x = -Tba[0];
- ALU_x = ALL_x + aA1_dot_B0;
- AUL_x = ALL_x + aA0_dot_B0;
- AUU_x = ALU_x + aA0_dot_B0;
-
- if(ALL_x < ALU_x)
- {
- LA1_lx = ALL_x;
- LA1_ux = ALU_x;
- UA1_lx = AUL_x;
- UA1_ux = AUU_x;
- }
- else
- {
- LA1_lx = ALU_x;
- LA1_ux = ALL_x;
- UA1_lx = AUU_x;
- UA1_ux = AUL_x;
- }
-
- BLL_x = Tab[0];
- BLU_x = BLL_x + bA0_dot_B1;
- BUL_x = BLL_x + bA0_dot_B0;
- BUU_x = BLU_x + bA0_dot_B0;
-
- if(BLL_x < BLU_x)
- {
- LB1_lx = BLL_x;
- LB1_ux = BLU_x;
- UB1_lx = BUL_x;
- UB1_ux = BUU_x;
- }
- else
- {
- LB1_lx = BLU_x;
- LB1_ux = BLL_x;
- UB1_lx = BUU_x;
- UB1_ux = BUL_x;
- }
-
- // UA1, UB1
-
- if((UA1_ux > b[0]) && (UB1_ux > a[0]))
- {
- if(((UA1_lx > b[0]) ||
- inVoronoi(b[1], a[1], A1_dot_B0, aA0_dot_B0 - b[0] - Tba[0],
- A1_dot_B1, aA0_dot_B1 - Tba[1],
- -Tab[1] - bA1_dot_B0))
- &&
- ((UB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] + bA0_dot_B0 - a[0],
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1] - aA0_dot_B1)))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0,
- Tba[1] - aA0_dot_B1);
-
- S[0] = Tab[0] + Rab(0, 0) * b[0] + Rab(0, 1) * u - a[0] ;
- S[1] = Tab[1] + Rab(1, 0) * b[0] + Rab(1, 1) * u - t;
- S[2] = Tab[2] + Rab(2, 0) * b[0] + Rab(2, 1) * u;
-
- if(P && Q)
- {
- P->setValue(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
-
- // UA1, LB1
-
- if((UA1_lx < 0) && (LB1_ux > a[0]))
- {
- if(((UA1_ux < 0) ||
- inVoronoi(b[1], a[1], -A1_dot_B0, Tba[0] - aA0_dot_B0,
- A1_dot_B1, aA0_dot_B1 - Tba[1], -Tab[1]))
- &&
- ((LB1_lx > a[0]) ||
- inVoronoi(a[1], b[1], A0_dot_B1, Tab[0] - a[0],
- A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1)))
- {
- segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1], Tba[1] - aA0_dot_B1);
-
- S[0] = Tab[0] + Rab(0, 1) * u - a[0];
- S[1] = Tab[1] + Rab(1, 1) * u - t;
- S[2] = Tab[2] + Rab(2, 1) * u;
-
- if(P && Q)
- {
- P->setValue(a[0], t, 0);
- *Q = S + (*P);
- }
-
- return S.length();
- }
- }
-
- // LA1, UB1
-
- if((LA1_ux > b[0]) && (UB1_lx < 0))
- {
- if(((LA1_lx > b[0]) ||
- inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
- A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
- &&
- ((UB1_ux < 0) ||
- inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
- A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
+ if(((LA1_lx > b[0]) ||
+ inVoronoi(b[1], a[1], A1_dot_B0, -Tba[0] - b[0],
+ A1_dot_B1, -Tba[1], -Tab[1] - bA1_dot_B0))
+ &&
+ ((UB1_ux < 0) ||
+ inVoronoi(a[1], b[1], -A0_dot_B1, -Tab[0] - bA0_dot_B0,
+ A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1])))
{
segCoords(t, u, a[1], b[1], A1_dot_B1, Tab[1] + bA1_dot_B0, Tba[1]);
@@ -964,173 +720,426 @@ FCL_REAL RSS::rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL
*Q = S + (*P);
}
- return S.length();
+ return S.length();
+ }
+ }
+
+ // LA0, LB0
+
+ if((LA0_ly < 0) && (LB0_ly < 0))
+ {
+ if(((LA0_uy < 0) ||
+ inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
+ -Tba[0], -Tab[0]))
+ &&
+ ((LB0_uy < 0) ||
+ inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
+ Tab[0], Tba[0])))
+ {
+ segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
+
+ S[0] = Tab[0] + Rab(0, 0) * u - t;
+ S[1] = Tab[1] + Rab(1, 0) * u;
+ S[2] = Tab[2] + Rab(2, 0) * u;
+
+ if(P && Q)
+ {
+ P->setValue(t, 0, 0);
+ *Q = S + (*P);
+ }
+
+ return S.length();
+ }
+ }
+
+ // no edges passed, take max separation along face normals
+
+ FCL_REAL sep1, sep2;
+
+ if(Tab[2] > 0.0)
+ {
+ sep1 = Tab[2];
+ if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
+ if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
+ }
+ else
+ {
+ sep1 = -Tab[2];
+ if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
+ if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
+ }
+
+ if(Tba[2] < 0)
+ {
+ sep2 = -Tba[2];
+ if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
+ if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
+ }
+ else
+ {
+ sep2 = Tba[2];
+ if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
+ if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
+ }
+
+ if(sep1 >= sep2 && sep1 >= 0)
+ {
+ if(Tab[2] > 0)
+ S.setValue(0, 0, sep1);
+ else
+ S.setValue(0, 0, -sep1);
+
+ if(P && Q)
+ {
+ *Q = S;
+ P->setValue(0);
+ }
+ }
+
+ if(sep2 >= sep1 && sep2 >= 0)
+ {
+ Vec3f Q_(Tab[0], Tab[1], Tab[2]);
+ Vec3f P_;
+ if(Tba[2] < 0)
+ {
+ P_[0] = Rab(0, 2) * sep2 + Tab[0];
+ P_[1] = Rab(1, 2) * sep2 + Tab[1];
+ P_[2] = Rab(2, 2) * sep2 + Tab[2];
+ }
+ else
+ {
+ P_[0] = -Rab(0, 2) * sep2 + Tab[0];
+ P_[1] = -Rab(1, 2) * sep2 + Tab[1];
+ P_[2] = -Rab(2, 2) * sep2 + Tab[2];
+ }
+
+ S = Q_ - P_;
+
+ if(P && Q)
+ {
+ *P = P_;
+ *Q = Q_;
+ }
+ }
+
+ FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2);
+ return (sep > 0 ? sep : 0);
+}
+
+
+
+bool RSS::overlap(const RSS& other) const
+{
+ /// compute what transform [R,T] that takes us from cs1 to cs2.
+ /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+ /// First compute the rotation part, then translation part
+
+ /// First compute T2 - T1
+ Vec3f t = other.Tr - Tr;
+
+ /// Then compute R1'(T2 - T1)
+ Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2]));
+
+ /// Now compute R1'R2
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+
+ FCL_REAL dist = rectDistance(R, T, l, other.l);
+ return (dist <= (r + other.r));
+}
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
+{
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+
+ Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
+ Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
+
+ FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
+ return (dist <= (b1.r + b2.r));
+}
+
+bool RSS::contain(const Vec3f& p) const
+{
+ Vec3f local_p = p - Tr;
+ FCL_REAL proj0 = local_p.dot(axis[0]);
+ FCL_REAL proj1 = local_p.dot(axis[1]);
+ FCL_REAL proj2 = local_p.dot(axis[2]);
+ FCL_REAL abs_proj2 = fabs(proj2);
+ Vec3f proj(proj0, proj1, proj2);
+
+ /// projection is within the rectangle
+ if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ {
+ return (abs_proj2 < r);
+ }
+ else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+ {
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ Vec3f v(proj0, y, 0);
+ return ((proj - v).sqrLength() < r * r);
+ }
+ else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+ {
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ Vec3f v(x, proj1, 0);
+ return ((proj - v).sqrLength() < r * r);
+ }
+ else
+ {
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ Vec3f v(x, y, 0);
+ return ((proj - v).sqrLength() < r * r);
+ }
+}
+
+RSS& RSS::operator += (const Vec3f& p)
+{
+ Vec3f local_p = p - Tr;
+ FCL_REAL proj0 = local_p.dot(axis[0]);
+ FCL_REAL proj1 = local_p.dot(axis[1]);
+ FCL_REAL proj2 = local_p.dot(axis[2]);
+ FCL_REAL abs_proj2 = fabs(proj2);
+ Vec3f proj(proj0, proj1, proj2);
+
+ // projection is within the rectangle
+ if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
+ {
+ if(abs_proj2 < r)
+ ; // do nothing
+ else
+ {
+ r = 0.5 * (r + abs_proj2); // enlarge the r
+ // change RSS origin position
+ if(proj2 > 0)
+ Tr[2] += 0.5 * (abs_proj2 - r);
+ else
+ Tr[2] -= 0.5 * (abs_proj2 - r);
+ }
+ }
+ else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
+ {
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ Vec3f v(proj0, y, 0);
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ if(new_r_sqr < r * r)
+ ; // do nothing
+ else
+ {
+ if(abs_proj2 < r)
+ {
+ FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
+ l[1] += delta_y;
+ if(proj1 < 0)
+ Tr[1] -= delta_y;
+ }
+ else
+ {
+ FCL_REAL delta_y = fabs(proj1 - y);
+ l[1] += delta_y;
+ if(proj1 < 0)
+ Tr[1] -= delta_y;
+
+ if(proj2 > 0)
+ Tr[2] += 0.5 * (abs_proj2 - r);
+ else
+ Tr[2] -= 0.5 * (abs_proj2 - r);
+ }
+ }
+ }
+ else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
+ {
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ Vec3f v(x, proj1, 0);
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ if(new_r_sqr < r * r)
+ ; // do nothing
+ else
+ {
+ if(abs_proj2 < r)
+ {
+ FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
+ l[0] += delta_x;
+ if(proj0 < 0)
+ Tr[0] -= delta_x;
+ }
+ else
+ {
+ FCL_REAL delta_x = fabs(proj0 - x);
+ l[0] += delta_x;
+ if(proj0 < 0)
+ Tr[0] -= delta_x;
+
+ if(proj2 > 0)
+ Tr[2] += 0.5 * (abs_proj2 - r);
+ else
+ Tr[2] -= 0.5 * (abs_proj2 - r);
+ }
}
}
-
- // LA0, LB0
-
- if((LA0_ly < 0) && (LB0_ly < 0))
+ else
{
- if(((LA0_uy < 0) ||
- inVoronoi(b[0], a[0], -A0_dot_B1, Tba[1], A0_dot_B0,
- -Tba[0], -Tab[0]))
- &&
- ((LB0_uy < 0) ||
- inVoronoi(a[0], b[0], -A1_dot_B0, -Tab[1], A0_dot_B0,
- Tab[0], Tba[0])))
+ FCL_REAL x = (proj0 > 0) ? l[0] : 0;
+ FCL_REAL y = (proj1 > 0) ? l[1] : 0;
+ Vec3f v(x, y, 0);
+ FCL_REAL new_r_sqr = (proj - v).sqrLength();
+ if(new_r_sqr < r * r)
+ ; // do nothing
+ else
{
- segCoords(t, u, a[0], b[0], A0_dot_B0, Tab[0], Tba[0]);
+ if(abs_proj2 < r)
+ {
+ FCL_REAL diag = std::sqrt(new_r_sqr - proj2 * proj2);
+ FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag;
- S[0] = Tab[0] + Rab(0, 0) * u - t;
- S[1] = Tab[1] + Rab(1, 0) * u;
- S[2] = Tab[2] + Rab(2, 0) * u;
+ FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
+ FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
+ l[0] += delta_x;
+ l[1] += delta_y;
- if(P && Q)
- {
- P->setValue(t, 0, 0);
- *Q = S + (*P);
+ if(proj0 < 0 && proj1 < 0)
+ {
+ Tr[0] -= delta_x;
+ Tr[1] -= delta_y;
+ }
}
+ else
+ {
+ FCL_REAL delta_x = fabs(proj0 - x);
+ FCL_REAL delta_y = fabs(proj1 - y);
- return S.length();
+ l[0] += delta_x;
+ l[1] += delta_y;
+
+ if(proj0 < 0 && proj1 < 0)
+ {
+ Tr[0] -= delta_x;
+ Tr[1] -= delta_y;
+ }
+
+ if(proj2 > 0)
+ Tr[2] += 0.5 * (abs_proj2 - r);
+ else
+ Tr[2] -= 0.5 * (abs_proj2 - r);
+ }
}
}
- // no edges passed, take max separation along face normals
+ return *this;
+}
- FCL_REAL sep1, sep2;
+RSS RSS::operator + (const RSS& other) const
+{
+ RSS bv;
- if(Tab[2] > 0.0)
- {
- sep1 = Tab[2];
- if (Rab(2, 0) < 0.0) sep1 += b[0] * Rab(2, 0);
- if (Rab(2, 1) < 0.0) sep1 += b[1] * Rab(2, 1);
- }
- else
- {
- sep1 = -Tab[2];
- if (Rab(2, 0) > 0.0) sep1 -= b[0] * Rab(2, 0);
- if (Rab(2, 1) > 0.0) sep1 -= b[1] * Rab(2, 1);
- }
+ Vec3f v[16];
+ Vec3f d0_pos = other.axis[0] * (other.l[0] + other.r);
+ Vec3f d1_pos = other.axis[1] * (other.l[1] + other.r);
+ Vec3f d0_neg = other.axis[0] * (-other.r);
+ Vec3f d1_neg = other.axis[1] * (-other.r);
+ Vec3f d2_pos = other.axis[2] * other.r;
+ Vec3f d2_neg = other.axis[2] * (-other.r);
- if(Tba[2] < 0)
- {
- sep2 = -Tba[2];
- if (Rab(0, 2) < 0.0) sep2 += a[0] * Rab(0, 2);
- if (Rab(1, 2) < 0.0) sep2 += a[1] * Rab(1, 2);
- }
- else
- {
- sep2 = Tba[2];
- if (Rab(0, 2) > 0.0) sep2 -= a[0] * Rab(0, 2);
- if (Rab(1, 2) > 0.0) sep2 -= a[1] * Rab(1, 2);
- }
+ v[0] = other.Tr + d0_pos + d1_pos + d2_pos;
+ v[1] = other.Tr + d0_pos + d1_pos + d2_neg;
+ v[2] = other.Tr + d0_pos + d1_neg + d2_pos;
+ v[3] = other.Tr + d0_pos + d1_neg + d2_neg;
+ v[4] = other.Tr + d0_neg + d1_pos + d2_pos;
+ v[5] = other.Tr + d0_neg + d1_pos + d2_neg;
+ v[6] = other.Tr + d0_neg + d1_neg + d2_pos;
+ v[7] = other.Tr + d0_neg + d1_neg + d2_neg;
- if(sep1 >= sep2 && sep1 >= 0)
- {
- if(Tab[2] > 0)
- S.setValue(0, 0, sep1);
- else
- S.setValue(0, 0, -sep1);
+ d0_pos = axis[0] * (l[0] + r);
+ d1_pos = axis[1] * (l[1] + r);
+ d0_neg = axis[0] * (-r);
+ d1_neg = axis[1] * (-r);
+ d2_pos = axis[2] * r;
+ d2_neg = axis[2] * (-r);
- if(P && Q)
- {
- *Q = S;
- P->setValue(0);
- }
- }
+ v[8] = Tr + d0_pos + d1_pos + d2_pos;
+ v[9] = Tr + d0_pos + d1_pos + d2_neg;
+ v[10] = Tr + d0_pos + d1_neg + d2_pos;
+ v[11] = Tr + d0_pos + d1_neg + d2_neg;
+ v[12] = Tr + d0_neg + d1_pos + d2_pos;
+ v[13] = Tr + d0_neg + d1_pos + d2_neg;
+ v[14] = Tr + d0_neg + d1_neg + d2_pos;
+ v[15] = Tr + d0_neg + d1_neg + d2_neg;
- if(sep2 >= sep1 && sep2 >= 0)
- {
- Vec3f Q_(Tab[0], Tab[1], Tab[2]);
- Vec3f P_;
- if(Tba[2] < 0)
- {
- P_[0] = Rab(0, 2) * sep2 + Tab[0];
- P_[1] = Rab(1, 2) * sep2 + Tab[1];
- P_[2] = Rab(2, 2) * sep2 + Tab[2];
- }
- else
- {
- P_[0] = -Rab(0, 2) * sep2 + Tab[0];
- P_[1] = -Rab(1, 2) * sep2 + Tab[1];
- P_[2] = -Rab(2, 2) * sep2 + Tab[2];
- }
- S = Q_ - P_;
+ Matrix3f M; // row first matrix
+ Vec3f E[3]; // row first eigen-vectors
+ FCL_REAL s[3] = {0, 0, 0};
- if(P && Q)
- {
- *P = P_;
- *Q = Q_;
- }
- }
+ getCovariance(v, NULL, NULL, NULL, 16, M);
+ eigen(M, s, E);
- FCL_REAL sep = (sep1 > sep2 ? sep1 : sep2);
- return (sep > 0 ? sep : 0);
-}
+ int min, mid, max;
+ if(s[0] > s[1]) { max = 0; min = 1; }
+ else { min = 0; max = 1; }
+ if(s[2] < s[min]) { mid = min; min = 2; }
+ else if(s[2] > s[max]) { mid = max; max = 2; }
+ else { mid = 2; }
+ // column first matrix, as the axis in RSS
+ bv.axis[0].setValue(E[0][max], E[1][max], E[2][max]);
+ bv.axis[1].setValue(E[0][mid], E[1][mid], E[2][mid]);
+ bv.axis[2].setValue(E[1][max]*E[2][mid] - E[1][mid]*E[2][max],
+ E[0][mid]*E[2][max] - E[0][max]*E[2][mid],
+ E[0][max]*E[1][mid] - E[0][mid]*E[1][max]);
-void RSS::clipToRange(FCL_REAL& val, FCL_REAL a, FCL_REAL b)
-{
- if(val < a) val = a;
- else if(val > b) val = b;
-}
+ // set rss origin, rectangle size and radius
+ getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axis, bv.Tr, bv.l, bv.r);
+ return bv;
+}
-void RSS::segCoords(FCL_REAL& t, FCL_REAL& u, FCL_REAL a, FCL_REAL b, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
+FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
{
- FCL_REAL denom = 1 - A_dot_B * A_dot_B;
-
- if(denom == 0) t = 0;
- else
- {
- t = (A_dot_T - B_dot_T * A_dot_B) / denom;
- clipToRange(t, 0, a);
- }
+ // compute what transform [R,T] that takes us from cs1 to cs2.
+ // [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
+ // First compute the rotation part, then translation part
+ Vec3f t = other.Tr - Tr; // T2 - T1
+ Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
+ Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
- u = t * A_dot_B - B_dot_T;
- if(u < 0)
- {
- u = 0;
- t = A_dot_T;
- clipToRange(t, 0, a);
- }
- else if(u > b)
- {
- u = b;
- t = u * A_dot_B + A_dot_T;
- clipToRange(t, 0, a);
- }
+ FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
+ dist -= (r + other.r);
+ return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
-bool RSS::inVoronoi(FCL_REAL a, FCL_REAL b, FCL_REAL Anorm_dot_B, FCL_REAL Anorm_dot_T, FCL_REAL A_dot_B, FCL_REAL A_dot_T, FCL_REAL B_dot_T)
+FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
- if(fabs(Anorm_dot_B) < 1e-7) return false;
-
- FCL_REAL t, u, v;
+ Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
+ R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
+ R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
- u = -Anorm_dot_T / Anorm_dot_B;
- clipToRange(u, 0, b);
+ Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
+ R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
+ R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
- t = u * A_dot_B + A_dot_T;
- clipToRange(t, 0, a);
+ Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
- v = t * A_dot_B - B_dot_T;
+ Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
- if(Anorm_dot_B > 0)
- {
- if(v > (u + 1e-7)) return true;
- }
- else
- {
- if(v < (u - 1e-7)) return true;
- }
- return false;
+ FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
+ dist -= (b1.r + b2.r);
+ return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
+
+
}
diff --git a/trunk/fcl/src/BV/kDOP.cpp b/trunk/fcl/src/BV/kDOP.cpp
new file mode 100644
index 00000000..d99c7727
--- /dev/null
+++ b/trunk/fcl/src/BV/kDOP.cpp
@@ -0,0 +1,232 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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.
+ */
+
+/** \author Jia Pan */
+
+#include "fcl/BV/kDOP.h"
+#include <limits>
+#include <iostream>
+
+namespace fcl
+{
+
+/// @brief Find the smaller and larger one of two values
+inline void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL& minv, FCL_REAL& maxv)
+{
+ if(a > b)
+ {
+ minv = b;
+ maxv = a;
+ }
+ else
+ {
+ minv = a;
+ maxv = b;
+ }
+}
+/// @brief Merge the interval [minv, maxv] and value p/
+inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
+{
+ if(p > maxv) maxv = p;
+ if(p < minv) minv = p;
+}
+
+
+/// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes
+template<std::size_t N>
+void getDistances(const Vec3f& p, FCL_REAL* d) {}
+
+/// @brief Specification of getDistances
+template<>
+inline void getDistances<5>(const Vec3f& p, FCL_REAL* d)
+{
+ d[0] = p[0] + p[1];
+ d[1] = p[0] + p[2];
+ d[2] = p[1] + p[2];
+ d[3] = p[0] - p[1];
+ d[4] = p[0] - p[2];
+}
+
+template<>
+inline void getDistances<6>(const Vec3f& p, FCL_REAL* d)
+{
+ d[0] = p[0] + p[1];
+ d[1] = p[0] + p[2];
+ d[2] = p[1] + p[2];
+ d[3] = p[0] - p[1];
+ d[4] = p[0] - p[2];
+ d[5] = p[1] - p[2];
+}
+
+template<>
+inline void getDistances<9>(const Vec3f& p, FCL_REAL* d)
+{
+ d[0] = p[0] + p[1];
+ d[1] = p[0] + p[2];
+ d[2] = p[1] + p[2];
+ d[3] = p[0] - p[1];
+ d[4] = p[0] - p[2];
+ d[5] = p[1] - p[2];
+ d[6] = p[0] + p[1] - p[2];
+ d[7] = p[0] + p[2] - p[1];
+ d[8] = p[1] + p[2] - p[0];
+}
+
+
+template<size_t N>
+KDOP<N>::KDOP()
+{
+ FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
+ for(size_t i = 0; i < N / 2; ++i)
+ {
+ dist_[i] = real_max;
+ dist_[i + N / 2] = -real_max;
+ }
+}
+
+template<size_t N>
+KDOP<N>::KDOP(const Vec3f& v)
+{
+ for(size_t i = 0; i < 3; ++i)
+ {
+ dist_[i] = dist_[N / 2 + i] = v[i];
+ }
+
+ FCL_REAL d[(N - 6) / 2];
+ getDistances<(N - 6) / 2>(v, d);
+ for(size_t i = 0; i < (N - 6) / 2; ++i)
+ {
+ dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
+ }
+}
+
+template<size_t N>
+KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
+{
+ for(size_t i = 0; i < 3; ++i)
+ {
+ minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
+ }
+
+ FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
+ getDistances<(N - 6) / 2>(a, ad);
+ getDistances<(N - 6) / 2>(b, bd);
+ for(size_t i = 0; i < (N - 6) / 2; ++i)
+ {
+ minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
+ }
+}
+
+template<size_t N>
+bool KDOP<N>::overlap(const KDOP<N>& other) const
+{
+ for(size_t i = 0; i < N / 2; ++i)
+ {
+ if(dist_[i] > other.dist_[i + N / 2]) return false;
+ if(dist_[i + N / 2] < other.dist_[i]) return false;
+ }
+
+ return true;
+}
+
+template<size_t N>
+bool KDOP<N>::inside(const Vec3f& p) const
+{
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
+ return false;
+ }
+
+ FCL_REAL d[(N - 6) / 2];
+ getDistances<(N - 6) / 2>(p, d);
+ for(size_t i = 0; i < (N - 6) / 2; ++i)
+ {
+ if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
+ return false;
+ }
+
+ return true;
+}
+
+template<size_t N>
+KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
+{
+ for(size_t i = 0; i < 3; ++i)
+ {
+ minmax(p[i], dist_[i], dist_[N / 2 + i]);
+ }
+
+ FCL_REAL pd[(N - 6) / 2];
+ getDistances<(N - 6) / 2>(p, pd);
+ for(size_t i = 0; i < (N - 6) / 2; ++i)
+ {
+ minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
+ }
+
+ return *this;
+}
+
+template<size_t N>
+KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
+{
+ for(size_t i = 0; i < N / 2; ++i)
+ {
+ dist_[i] = std::min(other.dist_[i], dist_[i]);
+ dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
+ }
+ return *this;
+}
+
+template<size_t N>
+KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
+{
+ KDOP<N> res(*this);
+ return res += other;
+}
+
+
+template<size_t N>
+FCL_REAL KDOP<N>::distance(const KDOP<N>& other, Vec3f* P, Vec3f* Q) const
+{
+ std::cerr << "KDOP distance not implemented!" << std::endl;
+ return 0.0;
+}
+
+
+template class KDOP<16>;
+template class KDOP<18>;
+template class KDOP<24>;
+
+}
diff --git a/trunk/fcl/src/BV/kIOS.cpp b/trunk/fcl/src/BV/kIOS.cpp
index 6e808cf0..94e3bcd3 100644
--- a/trunk/fcl/src/BV/kIOS.cpp
+++ b/trunk/fcl/src/BV/kIOS.cpp
@@ -57,7 +57,7 @@ bool kIOS::overlap(const kIOS& other) const
}
}
- return obb_bv.overlap(other.obb_bv);
+ return obb.overlap(other.obb);
return true;
}
@@ -87,7 +87,7 @@ kIOS& kIOS::operator += (const Vec3f& p)
}
}
- obb_bv += p;
+ obb += p;
}
@@ -102,29 +102,29 @@ kIOS kIOS::operator + (const kIOS& other) const
result.num_spheres = new_num_spheres;
- result.obb_bv = obb_bv + other.obb_bv;
+ result.obb = obb + other.obb;
return result;
}
FCL_REAL kIOS::width() const
{
- return obb_bv.width();
+ return obb.width();
}
FCL_REAL kIOS::height() const
{
- return obb_bv.height();
+ return obb.height();
}
FCL_REAL kIOS::depth() const
{
- return obb_bv.depth();
+ return obb.depth();
}
FCL_REAL kIOS::volume() const
{
- return obb_bv.volume();
+ return obb.volume();
}
FCL_REAL kIOS::size() const
@@ -175,12 +175,11 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
}
-
- // crap
- b2_temp.obb_bv.To = R0 * b2_temp.obb_bv.To + T0;
- b2_temp.obb_bv.axis[0] = R0 * b2_temp.obb_bv.axis[0];
- b2_temp.obb_bv.axis[1] = R0 * b2_temp.obb_bv.axis[1];
- b2_temp.obb_bv.axis[2] = R0 * b2_temp.obb_bv.axis[2];
+
+ b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
+ b2_temp.obb.axis[0] = R0 * b2_temp.obb.axis[0];
+ b2_temp.obb.axis[1] = R0 * b2_temp.obb.axis[1];
+ b2_temp.obb.axis[2] = R0 * b2_temp.obb.axis[2];
return b1.overlap(b2_temp);
}
diff --git a/trunk/fcl/src/BVH_utility.cpp b/trunk/fcl/src/BVH_utility.cpp
index b5d197aa..43b362f7 100644
--- a/trunk/fcl/src/BVH_utility.cpp
+++ b/trunk/fcl/src/BVH_utility.cpp
@@ -43,7 +43,7 @@
namespace fcl
{
-void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
+void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
@@ -54,7 +54,7 @@ void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
+ const Variance3f& uc = ucs[v_id];
Vec3f&v = model.vertices[bvnode.first_primitive + j];
@@ -74,7 +74,7 @@ void BVHExpand(BVHModel<OBB>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
}
}
-void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
+void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
{
for(int i = 0; i < model.getNumBVs(); ++i)
{
@@ -85,7 +85,7 @@ void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
- const Uncertainty& uc = ucs[v_id];
+ const Variance3f& uc = ucs[v_id];
Vec3f&v = model.vertices[bvnode.first_primitive + j];
@@ -106,7 +106,7 @@ void BVHExpand(BVHModel<RSS>& model, const Uncertainty* ucs, FCL_REAL r = 1.0)
}
-void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty* ucs)
+void estimateSamplingVariance(Vec3f* vertices, int num_vertices, Variance3f* ucs)
{
int nPts = num_vertices;
@@ -183,7 +183,7 @@ void estimateSamplingUncertainty(Vec3f* vertices, int num_vertices, Uncertainty*
}
}
- ucs[i].preprocess();
+ ucs[i].init();
ucs[i].sqrt();
}
diff --git a/trunk/fcl/src/BV_fitter.cpp b/trunk/fcl/src/BV_fitter.cpp
index a423b0b6..68859b61 100644
--- a/trunk/fcl/src/BV_fitter.cpp
+++ b/trunk/fcl/src/BV_fitter.cpp
@@ -244,11 +244,11 @@ void fit1(Vec3f* ps, kIOS& bv)
bv.spheres[0].o = ps[0];
bv.spheres[0].r = 0;
- bv.obb_bv.axis[0].setValue(1, 0, 0);
- bv.obb_bv.axis[1].setValue(0, 1, 0);
- bv.obb_bv.axis[2].setValue(0, 0, 1);
- bv.obb_bv.extent.setValue(0);
- bv.obb_bv.To = ps[0];
+ bv.obb.axis[0].setValue(1, 0, 0);
+ bv.obb.axis[1].setValue(0, 1, 0);
+ bv.obb.axis[2].setValue(0, 0, 1);
+ bv.obb.extent.setValue(0);
+ bv.obb.To = ps[0];
}
void fit2(Vec3f* ps, kIOS& bv)
@@ -261,15 +261,15 @@ void fit2(Vec3f* ps, kIOS& bv)
FCL_REAL len_p1p2 = p1p2.length();
p1p2.normalize();
- Vec3f* axis = bv.obb_bv.axis;
+ Vec3f* axis = bv.obb.axis;
axis[0] = p1p2;
generateCoordinateSystem(axis[0], axis[1], axis[2]);
FCL_REAL r0 = len_p1p2 * 0.5;
- bv.obb_bv.extent.setValue(r0, 0, 0);
- bv.obb_bv.To = (p1 + p2) * 0.5;
+ bv.obb.extent.setValue(r0, 0, 0);
+ bv.obb.To = (p1 + p2) * 0.5;
- bv.spheres[0].o = bv.obb_bv.To;
+ bv.spheres[0].o = bv.obb.To;
bv.spheres[0].r = r0;
FCL_REAL r1 = r0 * invSinA;
@@ -307,9 +307,9 @@ void fit3(Vec3f* ps, kIOS& bv)
if(len[1] > len[0]) imax = 1;
if(len[2] > len[imax]) imax = 2;
- Vec3f& u = bv.obb_bv.axis[0];
- Vec3f& v = bv.obb_bv.axis[1];
- Vec3f& w = bv.obb_bv.axis[2];
+ Vec3f& u = bv.obb.axis[0];
+ Vec3f& v = bv.obb.axis[1];
+ Vec3f& w = bv.obb.axis[2];
w = e[0].cross(e[1]);
w.normalize();
@@ -317,7 +317,7 @@ void fit3(Vec3f* ps, kIOS& bv)
u.normalize();
v = w.cross(u);
- getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb_bv.axis, bv.obb_bv.To, bv.obb_bv.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, 3, bv.obb.axis, bv.obb.To, bv.obb.extent);
// compute radius and center
FCL_REAL r0;
@@ -328,7 +328,7 @@ void fit3(Vec3f* ps, kIOS& bv)
bv.spheres[0].r = r0;
FCL_REAL r1 = r0 * invSinA;
- Vec3f delta = bv.obb_bv.axis[2] * (r1 * cosA);
+ Vec3f delta = bv.obb.axis[2] * (r1 * cosA);
bv.spheres[1].r = r1;
bv.spheres[1].o = center - delta;
@@ -345,14 +345,14 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
- Vec3f* axis = bv.obb_bv.axis;
+ Vec3f* axis = bv.obb.axis;
axisFromEigen(E, s, axis);
- getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb_bv.To, bv.obb_bv.extent);
+ getExtentAndCenter(ps, NULL, NULL, NULL, n, axis, bv.obb.To, bv.obb.extent);
// get center and extension
- const Vec3f& center = bv.obb_bv.To;
- const Vec3f& extent = bv.obb_bv.extent;
+ const Vec3f& center = bv.obb.To;
+ const Vec3f& extent = bv.obb.extent;
FCL_REAL r0 = maximumDistance(ps, NULL, NULL, NULL, n, center);
// decide the k in kIOS
@@ -440,20 +440,20 @@ void fit(Vec3f* ps, int n, OBB& bv)
{
switch(n)
{
- case 1:
- OBB_fit_functions::fit1(ps, bv);
- break;
- case 2:
- OBB_fit_functions::fit2(ps, bv);
- break;
- case 3:
- OBB_fit_functions::fit3(ps, bv);
- break;
- case 6:
- OBB_fit_functions::fit6(ps, bv);
- break;
- default:
- OBB_fit_functions::fitn(ps, n, bv);
+ case 1:
+ OBB_fit_functions::fit1(ps, bv);
+ break;
+ case 2:
+ OBB_fit_functions::fit2(ps, bv);
+ break;
+ case 3:
+ OBB_fit_functions::fit3(ps, bv);
+ break;
+ case 6:
+ OBB_fit_functions::fit6(ps, bv);
+ break;
+ default:
+ OBB_fit_functions::fitn(ps, n, bv);
}
}
@@ -463,17 +463,17 @@ void fit(Vec3f* ps, int n, RSS& bv)
{
switch(n)
{
- case 1:
- RSS_fit_functions::fit1(ps, bv);
- break;
- case 2:
- RSS_fit_functions::fit2(ps, bv);
- break;
- case 3:
- RSS_fit_functions::fit3(ps, bv);
- break;
- default:
- RSS_fit_functions::fitn(ps, n, bv);
+ case 1:
+ RSS_fit_functions::fit1(ps, bv);
+ break;
+ case 2:
+ RSS_fit_functions::fit2(ps, bv);
+ break;
+ case 3:
+ RSS_fit_functions::fit3(ps, bv);
+ break;
+ default:
+ RSS_fit_functions::fitn(ps, n, bv);
}
}
@@ -604,14 +604,14 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
- Vec3f* axis = bv.obb_bv.axis;
+ Vec3f* axis = bv.obb.axis;
axisFromEigen(E, s, axis);
// get centers and extensions
- getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb_bv.To, bv.obb_bv.extent);
+ getExtentAndCenter(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, axis, bv.obb.To, bv.obb.extent);
- const Vec3f& center = bv.obb_bv.To;
- const Vec3f& extent = bv.obb_bv.extent;
+ const Vec3f& center = bv.obb.To;
+ const Vec3f& extent = bv.obb.extent;
FCL_REAL r0 = maximumDistance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, center);
// decide k in kIOS
diff --git a/trunk/fcl/src/BV_splitter.cpp b/trunk/fcl/src/BV_splitter.cpp
index f66a8bc9..d2840532 100644
--- a/trunk/fcl/src/BV_splitter.cpp
+++ b/trunk/fcl/src/BV_splitter.cpp
@@ -50,34 +50,34 @@ template<>
void computeSplitVector<kIOS>(const kIOS& bv, Vec3f& split_vector)
{
/*
- switch(bv.num_spheres)
- {
- case 1:
+ switch(bv.num_spheres)
+ {
+ case 1:
split_vector = Vec3f(1, 0, 0);
break;
- case 3:
+ case 3:
{
- Vec3f v[3];
- v[0] = bv.spheres[1].o - bv.spheres[0].o;
- v[0].normalize();
- generateCoordinateSystem(v[0], v[1], v[2]);
- split_vector = v[1];
+ Vec3f v[3];
+ v[0] = bv.spheres[1].o - bv.spheres[0].o;
+ v[0].normalize();
+ generateCoordinateSystem(v[0], v[1], v[2]);
+ split_vector = v[1];
}
break;
- case 5:
+ case 5:
{
- Vec3f v[2];
- v[0] = bv.spheres[1].o - bv.spheres[0].o;
- v[1] = bv.spheres[3].o - bv.spheres[0].o;
- split_vector = v[0].cross(v[1]);
- split_vector.normalize();
+ Vec3f v[2];
+ v[0] = bv.spheres[1].o - bv.spheres[0].o;
+ v[1] = bv.spheres[3].o - bv.spheres[0].o;
+ split_vector = v[0].cross(v[1]);
+ split_vector.normalize();
}
break;
- default:
+ default:
;
- }
+ }
*/
- split_vector = bv.obb_bv.axis[0];
+ split_vector = bv.obb.axis[0];
}
template<>
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index eb68ff73..ae45d64e 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -48,41 +48,6 @@
namespace fcl
{
-
-bool defaultCollisionFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_)
-{
- CollisionData* cdata = static_cast<CollisionData*>(cdata_);
- const CollisionRequest& request = cdata->request;
- CollisionResult& result = cdata->result;
-
- if(cdata->done) return true;
-
- collide(o1, o2, request, result);
-
- if(!request.enable_cost && (result.isCollision()) && (result.numContacts() >= request.num_max_contacts))
- cdata->done = true;
-
- return cdata->done;
-}
-
-bool defaultDistanceFunction(CollisionObject* o1, CollisionObject* o2, void* cdata_, FCL_REAL& dist)
-{
- DistanceData* cdata = static_cast<DistanceData*>(cdata_);
- const DistanceRequest& request = cdata->request;
- DistanceResult& result = cdata->result;
-
- if(cdata->done) { dist = result.min_distance; return true; }
-
- distance(o1, o2, request, result);
-
- dist = result.min_distance;
-
- if(dist <= 0) return true; // in collision or in touch
-
- return cdata->done;
-}
-
-
void NaiveCollisionManager::registerObjects(const std::vector<CollisionObject*>& other_objs)
{
std::copy(other_objs.begin(), other_objs.end(), std::back_inserter(objs));
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 8c9c6602..6a88184a 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -485,32 +485,32 @@ void constructBox(const OBB& bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
}
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
- bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
{
- box = Box(bv.obb_bv.extent * 2);
- tf = Transform3f(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
- bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
- bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
+ box = Box(bv.obb.extent * 2);
+ tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
void constructBox(const RSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
}
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
@@ -543,32 +543,32 @@ void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f
{
box = Box(bv.extent * 2);
tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
}
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
- bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
- box = Box(bv.obb_bv.extent * 2);
- tf = tf_bv * Transform3f(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
- bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
- bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
+ box = Box(bv.obb.extent * 2);
+ tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
}
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
}
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
diff --git a/trunk/fcl/src/intersect.cpp b/trunk/fcl/src/intersect.cpp
index 6a2ab62e..46a10dc3 100644
--- a/trunk/fcl/src/intersect.cpp
+++ b/trunk/fcl/src/intersect.cpp
@@ -1339,8 +1339,8 @@ void Intersect::kernelGradient(KERNEL_PARM *kernel_parm, DOC *a, DOC *b, Vec3f&
}
}
-FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
+FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
+ Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
const CloudClassifierParam& solver, bool scaling)
{
KERNEL_CACHE *kernel_cache;
@@ -1526,8 +1526,8 @@ FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
return max_collision_prob;
}
-FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
- Vec3f* cloud2, Uncertainty* uc2, int size_cloud2,
+FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
+ Vec3f* cloud2, Variance3f* uc2, int size_cloud2,
const Matrix3f& R, const Vec3f& T, const CloudClassifierParam& solver, bool scaling)
{
KERNEL_CACHE *kernel_cache;
@@ -1716,7 +1716,7 @@ FCL_REAL Intersect::intersect_PointClouds(Vec3f* cloud1, Uncertainty* uc1, int s
}
-FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3)
{
// get the plane x * n - t = 0 and the compute the projection matrix according to (I - nn^t) y + t * n = y'
@@ -1830,7 +1830,7 @@ FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc
}
-FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Uncertainty* uc1, int size_cloud1,
+FCL_REAL Intersect::intersect_PointCloudsTriangle(Vec3f* cloud1, Variance3f* uc1, int size_cloud1,
const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3,
const Matrix3f& R, const Vec3f& T)
{
diff --git a/trunk/fcl/src/simple_setup.cpp b/trunk/fcl/src/simple_setup.cpp
index 815d5dd2..2f3ac7fb 100644
--- a/trunk/fcl/src/simple_setup.cpp
+++ b/trunk/fcl/src/simple_setup.cpp
@@ -142,11 +142,11 @@ static inline bool setupPointCloudCollisionOrientedNode(OrientedNode& node,
node.vertices1 = model1.vertices;
node.vertices2 = model2.vertices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
- node.uc2.reset(new Uncertainty[model2.num_vertices]);
+ node.uc1.reset(new Variance3f[model1.num_vertices]);
+ node.uc2.reset(new Variance3f[model2.num_vertices]);
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
- estimateSamplingUncertainty(model2.vertices, model2.num_vertices, node.uc2.get());
+ estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
+ estimateSamplingVariance(model2.vertices, model2.num_vertices, node.uc2.get());
BVHExpand(model1, node.uc1.get(), expand_r);
BVHExpand(model2, node.uc2.get(), expand_r);
@@ -210,9 +210,9 @@ static inline bool setupPointCloudMeshCollisionOrientedNode(OrientedNode& node,
node.vertices2 = model2.vertices;
node.tri_indices2 = model2.tri_indices;
- node.uc1.reset(new Uncertainty[model1.num_vertices]);
+ node.uc1.reset(new Variance3f[model1.num_vertices]);
- estimateSamplingUncertainty(model1.vertices, model1.num_vertices, node.uc1.get());
+ estimateSamplingVariance(model1.vertices, model1.num_vertices, node.uc1.get());
BVHExpand(model1, node.uc1.get(), expand_r);
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 73a6d6eb..7b136fbf 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -204,7 +204,7 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2) const
bool MeshCollisionTraversalNodeOBB::BVTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
{
if(enable_statistics) num_bv_tests++;
- return OBB::obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent);
+ return obbDisjoint(Rc, Tc, model1->getBV(b1).bv.extent, model2->getBV(b2).bv.extent);
}
void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f& Rc, const Vec3f& Tc) const
@@ -304,7 +304,7 @@ static inline void pointCloudCollisionOrientedNodeLeafTesting(int b1, int b2,
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
FCL_REAL collision_prob_threshold,
- const boost::shared_arry<Uncertainty>& uc1, const boost::shared_array<Uncertainty>& uc2,
+ const boost::shared_arry<Variance3f>& uc1, const boost::shared_array<Variance3f>& uc2,
const CloudClassifierParam classifier_param,
int& num_leaf_tests,
FCL_REAL& max_collision_prob,
@@ -394,7 +394,7 @@ static inline void pointCloudMeshCollisionOrientedNodeLeafTesting(int b1, int b2
const Matrix3f& R, const Vec3f& T,
bool enable_statistics,
FCL_REAL collision_prob_threshold,
- const boost::shared_array<Uncertainty>& uc1,
+ const boost::shared_array<Variance3f>& uc1,
int& num_leaf_tests,
FCL_REAL& max_collision_prob,
std::vector<BVHPointCollisionPair>& pairs)
--
GitLab