rewriting part of the code. more documentation.

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@150 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/CMakeLists.txt

@@ -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})

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);
 }
 
 }

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);

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

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);
 
 }

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