diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index b680d9f24d4989d2aafe61c8037e6eabf19cb933..458bd929138174042f7c731dc280e03b417f970c 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 6dd515b2df2d3ee54583e2e9879c1153d72a084e..549e7fa8b6052329a30f9fba2d6020064fc1d6bd 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 a83666f8c3c3c4c98d90029761878b9e2738e830..cbfe1914c7c4d19d05c6c3dcd0a79c56c3882037 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 a3978113408fe29fae070a434dd999f57b2f1442..2020b760b1258b30785a1216cd438756a0728030 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 7d9b42dbfa8db760c650669602b616a0d76d6331..9bdf0e5273de38ed02ebad2b6f2c9695d96171d2 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 130fcf047770aeeace3610f7d1f2c0ffb7089d50..4a9b321dd2f05054028a607eccc2a90e2dc8725e 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 474730dc4e35e81ce791607f96a9934345f69afc..7784f7c60ba005b178a2cc7837d5f2e71584663a 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 99044ce09dbce9c5b36665cc201c2575abb52fcc..bbab38bbde9c18334fd7c3efc1b420b0fcf916a2 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 ac22081562b1d56fb7d5d4fe88017170d28c53aa..e37c3cbddc77ea9422fe1112e628680d5f43e62a 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 958973747643d16b5636a4836127e3d8180561af..24f64d4a010263227af16eff1bbeecdb433fe590 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 472f60661a0cb36864055e76de692c5ee244ef3a..67f5f0c6ed433a38faf2ae85f8aa048c1764667f 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 059d94e5659679c4ad0482156001b578b719c739..bbe39353202fdb7bdae14065b40ccf275ddabae8 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 cd312166c54b93e4406f256a985653be45458ae2..024391ca60d7e84d613de8926ba818d62cc31171 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 d3e1025a16d991f9c05bd0ea4242f3187a779791..ae38295cca3b1d6ddb861e840718bdb3e6adf841 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 a7388d05bfcd51271c324877eb1ca91c1c7c1722..b3d7d626ee50c98a3eebb013e2f9766ad3830535 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 585f5231df77776757cd75a0b5fba72ec693e21d..3dd5e93e9ab7f481a16f3bcb52f205453a47e34f 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 1683592898483213c408cda02e82025a4fbaf319..f7818774b6a56d69cb51add6348141b1088f85ba 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 4fd86cf4d20e574c23ec45951a1a61fb59f6fee8..b168d6f05015cd55ecb92a66859f6b4df102ae22 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 fdd00cc3477f3f649bde6d2f5ad3b803e9a6950f..551f5f7984cd4244632ac23b541d4db3f8927b80 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 763e426803c9016810c528edb8bda1cdaa5341d9..5eb4754bc7a8f791f4dbae8f9fc2cf63f90807c5 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 f53964221a6f9d03a5637001df4dc88073ac2467..0000000000000000000000000000000000000000
--- 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 1d1a6518c72ab7c8175d4c622282c668bd334803..e926b672a8f60c4d816f6965c33865bccb396999 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 c85631fb19754bcba6fa6dcc7915c8bc81280afa..d7ca412b697fbbe4babe623188a122a9819965b2 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 673b706cda477bdb362b9aee4480c4a09d646f6b..19ad21205d320a58e6de5dbf09d9ec1ddc372421 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 604258a104c09cd645a25895a399430f9d4712f4..c28fcbad0e5d651460e67d07b2e5df184b558b60 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 ed3e0e92c998e7701da8a8757f1ca3a8a6bb1ae6..78e41f7a17a29cdce1702c34d7efc8b1eb3dd34b 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 dbe3c07406adba4c2e31868c26db55fbf868eae1..68fc42c30f7500fe94a89e8e35573db391e5c781 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 1cf86b4730dc25c90979f4f5585cc66fc8968788..ae98c25bda2092320774215fa9a09e787bb019fd 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 0000000000000000000000000000000000000000..d99c7727d8d9ddeb500846e8c7a18e3ca2318979
--- /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 6e808cf0e944f435e5e4c6e5f19f6e339165c909..94e3bcd3f563c55f87f6448f3e5883fdf37e3578 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 b5d197aab1c52181876b775f0446b771f6ff2925..43b362f75fd723cd7b1faefd651e29282c924c8b 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 a423b0b65d608acb8c509b2e54bcc4a0ff796e88..68859b61f6239e4860fe40f6837ba8c9f24ea826 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 f66a8bc9bd7698db1608d180358db8d611f18007..d28405323b829a8da6efaa37b7e820c34e6ebf93 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 eb68ff739959c8f8601f26530fe8b42f21d8375d..ae45d64e9cd7954fddb1ab5a360e8f4165336bb2 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 8c9c660234a68543a415d46600d1218aa531b3e2..6a88184aebfc05aa4c68f8ff196100d981e35f6e 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 6a2ab62e0c6ad4197a41dc3e87a4c09295ef9e09..46a10dc30870c4f27aa15736a03faeded0f81a29 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 815d5dd2a7a42f444384bc919e2628cc29992508..2f3ac7fbcfa9955450a900bb2612b535393135c1 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 73a6d6eb5bf04b65a70705deee4b0e1b9b6d2685..7b136fbfe61a86438e0ed62c97f0ef804dee7026 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)