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

trunk/fcl/include/fcl/AABB.h

@@ -54,12 +54,15 @@ public:
   /** \brief The max point in the AABB */
   Vec3f max_;
 
+  /** \brief Constructor creating an AABB with infinite size */
   AABB();
 
+  /** \brief Constructor 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)
   {
     min_ = min(a, b);

trunk/fcl/include/fcl/BVH_model.h

@@ -85,6 +85,7 @@ public:
       return BVH_MODEL_UNKNOWN;
   }
 
+  /** \brief Constructing an empty BVH */
   BVHModel()
   {
     vertices = NULL;
@@ -122,25 +123,34 @@ public:
     delete [] primitive_indices;
   }
 
+  /** \brief We provide getBV() and getNumBVs() because BVH may be compressed (in future), so
+      we must provide some flexibility here */
+  
+  /** \brief Get the BV of index id */
   const BVNode<BV>& getBV(int id) const
   {
     return bvs[id];
   }
 
+  /** \brief Get the BV of index id */
   BVNode<BV>& getBV(int id)
   {
     return bvs[id];
   }
 
+  /** \brief Get the number of BVs */
   int getNumBVs() const
   {
     return num_bvs;
   }
 
+  /** \brief Get the object type: it is a BVH */
   OBJECT_TYPE getObjectType() const { return OT_BVH; }
 
+  /** \brief Get the BV type */
   NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
 
+  /** \brief Compute the AABB for the BVH, used for broad-phase collision */
   void computeAABB();
 
   /** \brief Geometry point data */
@@ -241,6 +251,8 @@ private:
 
 };
 
+
+/** Specialization of getNodeType() for BVHModel with different BV types */
 template<>
 NODE_TYPE BVHModel<AABB>::getNodeType() const;
 

trunk/fcl/include/fcl/BV_splitter.h

@@ -49,16 +49,21 @@
 namespace fcl
 {
 
+/** \brief Base class for BV splitting operation */
 template<typename BV>
 class BVSplitterBase
 {
 public:
+  /** \brief Set the geometry data needed by the split rule */
   virtual void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_) = 0;
 
+  /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
   virtual void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives) = 0;
 
+  /** \brief Apply the split rule on a given point */
   virtual bool apply(const Vec3f& q) const = 0;
 
+  /** \brief Clear the geometry data set before */
   virtual void clear() = 0;
 };
 
@@ -85,7 +90,7 @@ public:
     type = type_;
   }
 
-  /** \brief Compute the split rule according to a subset of geometry and the the corresponding BV node */
+  /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
   void computeRule(const BV& bv, unsigned int* primitive_indices, int num_primitives)
   {
     switch(split_method)
@@ -239,7 +244,7 @@ public:
     type = type_;
   }
 
-  /** \brief Compute the split rule according to a subset of geometry and the the corresponding BV node */
+  /** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
   void computeRule(const OBB& bv, unsigned int* primitive_indices, int num_primitives)
   {
     Vec3f center = bv.center();

trunk/fcl/include/fcl/broad_phase_collision.h

@@ -76,6 +76,8 @@ public:
   virtual void collide(void* cdata, CollisionCallBack callback) const = 0;
 
   virtual bool empty() const = 0;
+  
+  virtual size_t size() const = 0;
 };
 
 class NaiveCollisionManager : public BroadPhaseCollisionManager
@@ -100,6 +102,8 @@ public:
   void collide(void* cdata, CollisionCallBack callback) const;
 
   bool empty() const;
+  
+  inline size_t size() const { return objs.size(); }
 
 protected:
 
@@ -139,6 +143,8 @@ public:
   void collide(void* cdata, CollisionCallBack callback) const;
 
   bool empty() const;
+  
+  inline size_t size() const { return AABB_arr.size(); }
 
 protected:
 
@@ -249,6 +255,8 @@ public:
   void collide(void* cdata, CollisionCallBack callback) const;
 
   bool empty() const;
+  
+  inline size_t size() const { return objs_x.size(); }
 
 protected:
 
@@ -335,6 +343,8 @@ public:
   void collide(void* cdata, CollisionCallBack callback) const;
 
   bool empty() const;
+  
+  inline size_t size() const { return endpoints[0].size() / 2; }
 
 protected:
 

trunk/fcl/include/fcl/collision.h

@@ -42,9 +42,15 @@
 #include "fcl/collision_object.h"
 #include "fcl/collision_data.h"
 
+/** \brief Main namespace */
 namespace fcl
 {
 
+/** \brief Main collision interface: given two collision objects, and the requirements for contacts, including num of max contacts, whether perform exhaustive collision (i.e., returning 
+ * returning all the contact points), whether return detailed contact information (i.e., normal, contact point, depth; otherwise only contact primitive id is returned), this function
+ * performs the collision between them. 
+ * Return value is the number of contacts returned 
+ */
 int collide(const CollisionObject* o1, const CollisionObject* o2,
              int num_max_contacts, bool exhaustive, bool enable_contact,
              std::vector<Contact>& contacts);

trunk/fcl/include/fcl/geometric_shape_to_BVH_model.h

@@ -42,10 +42,11 @@
 #include "fcl/BVH_model.h"
 #include <boost/math/constants/constants.hpp>
 
+/** \brief Main namespace */
 namespace fcl
 {
 
-
+/** \brief Generate BVH model from box */
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Box& shape)
 {
@@ -89,6 +90,7 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape)
   model.computeAABB();
 }
 
+/** Generate BVH model from sphere */
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg = 16, unsigned int ring = 16)
 {
@@ -155,6 +157,8 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, unsigned int seg
   model.computeAABB();
 }
 
+
+/** \brief Generate BVH model from cylinder */
 template<typename BV>
 void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, unsigned int tot = 16)
 {

trunk/fcl/include/fcl/geometric_shapes.h

@@ -50,6 +50,7 @@ namespace fcl
 class ShapeBase : public CollisionObject
 {
 public:
+  /** \brief Default Constructor */
   ShapeBase()
   {
     Rloc[0][0] = 1;
@@ -57,6 +58,7 @@ public:
     Rloc[2][2] = 1;
   }
 
+  /** \brief Set the local frame of the shape */
   void setLocalTransform(const Vec3f R_[3], const Vec3f& T_)
   {
     Rloc[0] = R_[0];
@@ -65,6 +67,7 @@ public:
     Tloc = T_;
   }
 
+  /** \brief Set the local orientation of the shape */
   void setLocalRotation(const Vec3f R[3])
   {
     Rloc[0] = R[0];
@@ -72,12 +75,14 @@ public:
     Rloc[2] = R[2];
   }
 
+  /** \brief Set the local position of the shape */
   void setLocalTranslation(const Vec3f& T)
   {
     Tloc = T;
   }
 
-  void advanceLocalTransform(const Vec3f R[3], const Vec3f& T)
+  /** \brief Append additional transform to the local transform */
+  void appendLocalTransform(const Vec3f R[3], const Vec3f& T)
   {
     Vec3f R0[3];
     for(int i = 0; i < 3; ++i)
@@ -86,6 +91,7 @@ public:
     Tloc = MxV(R, Tloc) + T;
   }
 
+  /** \brief Get local transform */
   void getLocalTransfrom(Vec3f R[3], Vec3f& T) const
   {
     T = Tloc;
@@ -94,16 +100,19 @@ public:
     R[2] = Rloc[2];
   }
 
+  /** \brief Get local position */
   inline const Vec3f& getLocalPosition() const
   {
     return Tloc;
   }
 
+  /** \brief Get local orientation */
   inline const Vec3f* getLocalRotation() const
   {
     return Rloc;
   }
 
+  /** \brief Get object type: a geometric shape */
   OBJECT_TYPE getObjectType() const { return OT_GEOM; }
 
 protected:
@@ -122,8 +131,10 @@ public:
   /** box side length */
   Vec3f side;
 
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a box */
   NODE_TYPE getNodeType() const { return GEOM_BOX; }
 };
 
@@ -132,10 +143,14 @@ class Sphere : public ShapeBase
 {
 public:
   Sphere(BVH_REAL radius_) : ShapeBase(), radius(radius_) {}
+  
+  /** \brief Radius of the sphere */
   BVH_REAL radius;
 
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a sphere */
   NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
 };
 
@@ -144,11 +159,17 @@ class Capsule : public ShapeBase
 {
 public:
   Capsule(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+
+  /** \brief Radius of capsule */
   BVH_REAL radius;
-  BVH_REAL lz; // length along z axis
 
+  /** \brief Length along z axis */
+  BVH_REAL lz;
+
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a capsule */
   NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
 };
 
@@ -157,11 +178,17 @@ class Cone : public ShapeBase
 {
 public:
   Cone(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+  
+  /** \brief Radius of the cone */
   BVH_REAL radius;
-  BVH_REAL lz; // length along z axis
 
+  /** \brief Length along z axis */
+  BVH_REAL lz;
+
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a cone */
   NODE_TYPE getNodeType() const { return GEOM_CONE; }
 };
 
@@ -170,11 +197,17 @@ class Cylinder : public ShapeBase
 {
 public:
   Cylinder(BVH_REAL radius_, BVH_REAL lz_) : ShapeBase(), radius(radius_), lz(lz_) {}
+  
+  /** \brief Radius of the cylinder */
   BVH_REAL radius;
-  BVH_REAL lz; // length along z axis
 
+  /** \brief Length along z axis */
+  BVH_REAL lz;
+
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a cylinder */
   NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
 };
 
@@ -182,6 +215,7 @@ public:
 class Convex : public ShapeBase
 {
 public:
+  /** Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information */
   Convex(Vec3f* plane_normals_,
          BVH_REAL* plane_dis_,
          int num_planes_,
@@ -207,6 +241,7 @@ public:
     fillEdges();
   }
 
+  /** Copy constructor */
   Convex(const Convex& other) : ShapeBase(other)
   {
     plane_normals = other.plane_normals;
@@ -223,10 +258,13 @@ public:
     delete [] edges;
   }
 
+  /** Compute AABB */
   void computeAABB();
 
+  /** Get node type: a conex polytope */
   NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
 
+  
   Vec3f* plane_normals;
   BVH_REAL* plane_dis;
 
@@ -247,9 +285,11 @@ public:
 
   Edge* edges;
 
+  /** \brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) */
   Vec3f center;
 
 protected:
+  /** \brief Get edge information */
   void fillEdges();
 };
 
@@ -257,7 +297,10 @@ protected:
 class Plane : public ShapeBase
 {
 public:
+  /** \brief Construct a plane with normal direction and offset */
   Plane(const Vec3f& n_, BVH_REAL d_) : ShapeBase(), n(n_), d(d_) { unitNormalTest(); }
+  
+  /** \brief Construct a plane with normal direction and offset */
   Plane(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d_)
   {
     n = Vec3f(a, b, c);
@@ -265,15 +308,21 @@ public:
     unitNormalTest();
   }
 
+  /** \brief Compute AABB */
   void computeAABB();
 
+  /** \brief Get node type: a plane */
   NODE_TYPE getNodeType() const { return GEOM_PLANE; }
 
+  /** \brief Plane normal */
   Vec3f n;
+
+  /** \brief Plane offset */
   BVH_REAL d;
 
 protected:
-
+  
+  /** \brief Turn non-unit normal into unit */
   void unitNormalTest();
 };
 

trunk/fcl/include/fcl/simple_setup.h

@@ -43,10 +43,11 @@
 #include "fcl/traversal_node_bvh_shape.h"
 #include "fcl/BVH_utility.h"
 
+/** \brief Main namespace */
 namespace fcl
 {
 
-
+/** \brief Initialize traversal node for collision between two geometric shapes */
 template<typename S1, typename S2>
 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2, bool enable_contact = false)
 {
@@ -54,6 +55,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
   return true;
 }
 
+/** \brief Initialize traversal node for collision between two geometric shapes, given the current transforms of the two shapes */
 template<typename S1, typename S2>
 bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, const S2& shape2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -71,7 +73,7 @@ bool initialize(ShapeCollisionTraversalNode<S1, S2>& node, const S1& shape1, con
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between one mesh and one shape */
 template<typename BV, typename S>
 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, const BVHModel<BV>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -90,7 +92,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, const BVHModel<BV>
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between one mesh and one shape, given current object transform */
 template<typename BV, typename S>
 bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& model1, S& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -115,7 +117,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
   model1.replaceSubModel(vertices_transformed1);
   model1.endReplaceModel(use_refit, refit_bottomup);
 
-  model2.advanceLocalTransform(R2, T2);
+  model2.appendLocalTransform(R2, T2);
 
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
@@ -127,7 +129,7 @@ bool initialize(MeshShapeCollisionTraversalNode<BV, S>& node, BVHModel<BV>& mode
 }
 
 
-
+/** \brief Initialize the traversal node for collision between one mesh and one shape, specialized for OBB type */
 template<typename S>
 bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, const S& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -146,7 +148,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between one mesh and one shape, given the current transform, specialized for OBB type */
 template<typename S>
 bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>& model1, S& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -158,7 +160,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
   node.model1 = &model1;
   node.model2 = &model2;
 
-  model2.advanceLocalTransform(R2, T2);
+  model2.appendLocalTransform(R2, T2);
 
   node.vertices = model1.vertices;
   node.tri_indices = model1.tri_indices;
@@ -171,7 +173,7 @@ bool initialize(MeshShapeCollisionTraversalNodeOBB<S>& node, const BVHModel<OBB>
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between two meshes */
 template<typename BV>
 bool initialize(MeshCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -194,7 +196,7 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between two meshes, given the current transforms */
 template<typename BV>
 bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false,
@@ -243,14 +245,18 @@ bool initialize(MeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHM
   return true;
 }
 
+
+/** \brief Initialize traversal node for collision between two meshes, specialized for OBB type */
 bool initialize(MeshCollisionTraversalNodeOBB& node, const BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
 bool initialize(MeshCollisionTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
 
+
 #if USE_SVMLIGHT
 
+/** \brief Initialize traversal node for collision between two point clouds */
 template<typename BV>
 bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -290,6 +296,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
   return true;
 }
 
+/** \brief Initialize traversal node for collision between two point clouds, given current transforms */
 template<typename BV, bool use_refit, bool refit_bottomup>
 bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -353,7 +360,7 @@ bool initialize(PointCloudCollisionTraversalNode<BV>& node, BVHModel<BV>& model1
   return true;
 }
 
-
+/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for OBB type */ 
 bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, BVHModel<OBB>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -363,6 +370,7 @@ bool initialize(PointCloudCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1
                 bool enable_contact = false,
                 BVH_REAL expand_r = 1);
 
+/** \brief Initialize traversal node for collision between two point clouds, given current transforms, specialized for RSS type */
 bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, BVHModel<RSS>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -372,6 +380,7 @@ bool initialize(PointCloudCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1
                 bool enable_contact = false,
                 BVH_REAL expand_r = 1);
 
+/** \brief Initialize traversal node for collision between one point cloud and one mesh */
 template<typename BV>
 bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, const BVHModel<BV>& model2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -406,6 +415,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
   return true;
 }
 
+/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms */
 template<typename BV, bool use_refit, bool refit_bottomup>
 bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -467,7 +477,7 @@ bool initialize(PointCloudMeshCollisionTraversalNode<BV>& node, BVHModel<BV>& mo
 }
 
 
-
+/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for OBB type */
 bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& model1, const BVHModel<OBB>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -477,7 +487,7 @@ bool initialize(PointCloudMeshCollisionTraversalNodeOBB& node, BVHModel<OBB>& mo
                 bool enable_contact = false,
                 BVH_REAL expand_r = 1);
 
-
+/** \brief Initialize traversal node for collision between one point cloud and one mesh, given current transforms, specialized for RSS type */
 bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
                 BVH_REAL collision_prob_threshold = 0.5,
@@ -489,6 +499,7 @@ bool initialize(PointCloudMeshCollisionTraversalNodeRSS& node, BVHModel<RSS>& mo
 
 #endif
 
+/** \brief Initialize traversal node for continuous collision detection between two meshes */
 template<typename BV>
 bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -514,6 +525,7 @@ bool initialize(MeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<B
   return true;
 }
 
+/** \brief Initialize traversal node for continuous collision detection between one mesh and one point cloud */
 template<typename BV>
 bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -537,6 +549,7 @@ bool initialize(MeshPointCloudContinuousCollisionTraversalNode<BV>& node, const
   return true;
 }
 
+/** \brief Initialize traversal node for continuous collision detection between one point cloud and one mesh */
 template<typename BV>
 bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2, int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false)
 {
@@ -560,6 +573,7 @@ bool initialize(PointCloudMeshContinuousCollisionTraversalNode<BV>& node, const
   return true;
 }
 
+/** \brief Initialize traversal node for distance computation between two meshes */
 template<typename BV>
 bool initialize(MeshDistanceTraversalNode<BV>& node, const BVHModel<BV>& model1, const BVHModel<BV>& model2)
 {
@@ -578,6 +592,8 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, const BVHModel<BV>& model1,
   return true;
 }
 
+
+/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
 template<typename BV>
 bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2,
@@ -623,9 +639,12 @@ bool initialize(MeshDistanceTraversalNode<BV>& node, BVHModel<BV>& model1, BVHMo
   return true;
 }
 
+
+/** \brief Initialize traversal node for distance computation between two meshes, given the current transforms */
 bool initialize(MeshDistanceTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2);
 
+/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms */
 template<typename BV>
 bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>& model1, BVHModel<BV>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1,
@@ -678,6 +697,7 @@ bool initialize(MeshConservativeAdvancementTraversalNode<BV>& node, BVHModel<BV>
 }
 
 
+/** \brief Initialize traversal node for conservative advancement computation between two meshes, given the current transforms, specialized for RSS */
 inline bool initialize(MeshConservativeAdvancementTraversalNodeRSS& node, const BVHModel<RSS>& model1, const BVHModel<RSS>& model2,
                 const Vec3f R1[3], const Vec3f& T1, const Vec3f R2[3], const Vec3f& T2, BVH_REAL w = 1)
 {

trunk/fcl/include/fcl/traversal_node_bvhs.h

@@ -50,9 +50,11 @@
 #include <limits>
 #include <vector>
 
+/** \brief Main namespace */
 namespace fcl
 {
 
+/** \brief Traversal node for collision between BVH models */
 template<typename BV>
 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
 {
@@ -67,16 +69,19 @@ public:
     query_time_seconds = 0.0;
   }
 
+  /** \brief Whether the BV node in the first BVH tree is leaf */
   bool isFirstNodeLeaf(int b) const
   {
     return model1->getBV(b).isLeaf();
   }
 
+  /** \brief Whether the BV node in the second BVH tree is leaf */
   bool isSecondNodeLeaf(int b) const
   {
     return model2->getBV(b).isLeaf();
   }
 
+  /** \brief Determine the traversal order, is the first BVTT subtree better */
   bool firstOverSecond(int b1, int b2) const
   {
     BVH_REAL sz1 = model1->getBV(b1).bv.size();
@@ -90,35 +95,43 @@ public:
     return false;
   }
 
+  /** \brief Obtain the left child of BV node in the first BVH */
   int getFirstLeftChild(int b) const
   {
     return model1->getBV(b).leftChild();
   }
 
+  /** \brief Obtain the right child of BV node in the first BVH */
   int getFirstRightChild(int b) const
   {
     return model1->getBV(b).rightChild();
   }
 
+  /** \brief Obtain the left child of BV node in the second BVH */
   int getSecondLeftChild(int b) const
   {
     return model2->getBV(b).leftChild();
   }
 
+  /** \brief Obtain the right child of BV node in the second BVH */
   int getSecondRightChild(int b) const
   {
     return model2->getBV(b).rightChild();
   }
 
+  /** \brief BV culling test in one BVTT node */
   bool BVTesting(int b1, int b2) const
   {
     if(enable_statistics) num_bv_tests++;
     return !model1->getBV(b1).overlap(model2->getBV(b2));
   }
-
+  
+  /** \brief The first BVH model */
   const BVHModel<BV>* model1;
+  /** \brief The second BVH model */
   const BVHModel<BV>* model2;
 
+  /** \brief statistical information */
   mutable int num_bv_tests;
   mutable int num_leaf_tests;
   mutable BVH_REAL query_time_seconds;
@@ -126,7 +139,7 @@ public:
 };
 
 
-/** \brief The indices of in-collision primitives of objects */
+/** \brief The collision/contact information between two primitives */
 struct BVHCollisionPair
 {
   BVHCollisionPair() {}
@@ -152,6 +165,7 @@ struct BVHCollisionPair
   BVH_REAL penetration_depth;
 };
 
+/** \brief Sorting rule between two BVHCollisionPair, for testing */
 struct BVHCollisionPairComp
 {
   bool operator()(const BVHCollisionPair& a, const BVHCollisionPair& b)
@@ -162,7 +176,7 @@ struct BVHCollisionPairComp
   }
 };
 
-
+/** \brief Traversal node for collision between two meshes */
 template<typename BV>
 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
 {
@@ -179,6 +193,7 @@ public:
     enable_contact = false;
   }
 
+  /** \brief Intersection testing between leaves (two triangles) */
   void leafTesting(int b1, int b2) const
   {
     if(this->enable_statistics) this->num_leaf_tests++;
@@ -229,6 +244,7 @@ public:
     }
   }
 
+  /** \brief Whether the traversal process can stop early */
   bool canStop() const
   {
     return (pairs.size() > 0) && (!exhaustive) && (num_max_contacts <= (int)pairs.size());