Add Boolean parameter in MeshCollisionTraversalNode derived constructors.

  enable_distance_lower_bound.

include/fcl/traversal/traversal_node_bvhs.h

@@ -62,7 +62,8 @@ template<typename BV>
 class BVHCollisionTraversalNode : public CollisionTraversalNodeBase
 {
 public:
-  BVHCollisionTraversalNode() : CollisionTraversalNodeBase()
+  BVHCollisionTraversalNode(bool enable_distance_lower_bound) :
+  CollisionTraversalNodeBase (enable_distance_lower_bound)
   {
     model1 = NULL;
     model2 = NULL;
@@ -156,7 +157,8 @@ template<typename BV>
 class MeshCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
 {
 public:
-  MeshCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
+  MeshCollisionTraversalNode(bool enable_distance_lower_bound) :
+  BVHCollisionTraversalNode<BV> (enable_distance_lower_bound)
   {
     vertices1 = NULL;
     vertices2 = NULL;
@@ -261,7 +263,7 @@ public:
 class MeshCollisionTraversalNodeOBB : public MeshCollisionTraversalNode<OBB>
 {
 public:
-  MeshCollisionTraversalNodeOBB();
+  MeshCollisionTraversalNodeOBB (bool enable_distance_lower_bound);
 
   bool BVTesting(int b1, int b2) const;
 
@@ -278,7 +280,7 @@ public:
 class MeshCollisionTraversalNodeRSS : public MeshCollisionTraversalNode<RSS>
 {
 public:
-  MeshCollisionTraversalNodeRSS();
+  MeshCollisionTraversalNodeRSS (bool enable_distance_lower_bound);
 
   bool BVTesting(int b1, int b2) const;
 
@@ -295,7 +297,7 @@ public:
 class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
 {
 public:
-  MeshCollisionTraversalNodekIOS();
+  MeshCollisionTraversalNodekIOS (bool enable_distance_lower_bound);
  
   bool BVTesting(int b1, int b2) const;
 
@@ -308,7 +310,7 @@ public:
 class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode<OBBRSS>
 {
 public:
-  MeshCollisionTraversalNodeOBBRSS();
+  MeshCollisionTraversalNodeOBBRSS (bool enable_distance_lower_bound);
  
   bool BVTesting(int b1, int b2) const;
 
@@ -342,7 +344,7 @@ template<typename BV>
 class MeshContinuousCollisionTraversalNode : public BVHCollisionTraversalNode<BV>
 {
 public:
-  MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>()
+  MeshContinuousCollisionTraversalNode() : BVHCollisionTraversalNode<BV>(false)
   {
     vertices1 = NULL;
     vertices2 = NULL;

src/collision_func_matrix.cpp

@@ -409,7 +409,7 @@ std::size_t BVHCollide(const CollisionGeometry* o1, const Transform3f& tf1, cons
 {
   if(request.isSatisfied(result)) return result.numContacts();
   
-  MeshCollisionTraversalNode<T_BVH> node;
+  MeshCollisionTraversalNode<T_BVH> node (request.enable_distance_lower_bound);
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
   BVHModel<T_BVH>* obj1_tmp = new BVHModel<T_BVH>(*obj1);
@@ -435,7 +435,7 @@ std::size_t orientedMeshCollide(const CollisionGeometry* o1, const Transform3f&
 {
   if(request.isSatisfied(result)) return result.numContacts();
 
-  OrientedMeshCollisionTraversalNode node;
+  OrientedMeshCollisionTraversalNode node (request.enable_distance_lower_bound);
   const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>* >(o1);
   const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>* >(o2);
 

src/traversal/traversal_node_bvhs.cpp

@@ -179,7 +179,9 @@ static inline void meshDistanceOrientedNodeLeafTesting(int b1, int b2,
 
 }
 
-MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB() : MeshCollisionTraversalNode<OBB>()
+MeshCollisionTraversalNodeOBB::MeshCollisionTraversalNodeOBB
+(bool enable_distance_lower_bound) :
+  MeshCollisionTraversalNode<OBB> (enable_distance_lower_bound)
 {
   R.setIdentity();
 }
@@ -221,7 +223,9 @@ void MeshCollisionTraversalNodeOBB::leafTesting(int b1, int b2, const Matrix3f&
 
 
 
-MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS() : MeshCollisionTraversalNode<RSS>()
+MeshCollisionTraversalNodeRSS::MeshCollisionTraversalNodeRSS
+(bool enable_distance_lower_bound) :
+  MeshCollisionTraversalNode<RSS> (enable_distance_lower_bound)
 {
   R.setIdentity();
 }
@@ -246,7 +250,9 @@ void MeshCollisionTraversalNodeRSS::leafTesting(int b1, int b2) const
 
 
 
-MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS() : MeshCollisionTraversalNode<kIOS>()
+MeshCollisionTraversalNodekIOS::MeshCollisionTraversalNodekIOS
+(bool enable_distance_lower_bound) :
+  MeshCollisionTraversalNode<kIOS>(enable_distance_lower_bound)
 {
   R.setIdentity();
 }
@@ -270,7 +276,9 @@ void MeshCollisionTraversalNodekIOS::leafTesting(int b1, int b2) const
 
 
 
-MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS() : MeshCollisionTraversalNode<OBBRSS>()
+MeshCollisionTraversalNodeOBBRSS::MeshCollisionTraversalNodeOBBRSS
+(bool enable_distance_lower_bound) :
+  MeshCollisionTraversalNode<OBBRSS> (enable_distance_lower_bound)
 {
   R.setIdentity();
 }

test/test_fcl_collision.cpp

@@ -831,7 +831,7 @@ bool collide_Test2(const Transform3f& tf,
   Transform3f pose1, pose2;
 
   CollisionResult local_result;
-  MeshCollisionTraversalNode<BV> node;
+  MeshCollisionTraversalNode<BV> node (false);
 
   if(!initialize<BV>(node, m1, pose1, m2, pose2,
                      CollisionRequest(num_max_contacts, enable_contact), local_result))
@@ -891,7 +891,7 @@ bool collide_Test(const Transform3f& tf,
   Transform3f pose1(tf), pose2;
 
   CollisionResult local_result;
-  MeshCollisionTraversalNode<BV> node;
+  MeshCollisionTraversalNode<BV> node (false);
 
   if(!initialize<BV>(node, m1, pose1, m2, pose2,
                      CollisionRequest(num_max_contacts, enable_contact), local_result))
@@ -950,7 +950,7 @@ bool collide_Test_Oriented(const Transform3f& tf,
   Transform3f pose1(tf), pose2;
 
   CollisionResult local_result;
-  TraversalNode node;
+  TraversalNode node (false);
   if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2, 
                  CollisionRequest(num_max_contacts, enable_contact), local_result))
     std::cout << "initialize error" << std::endl;

test/test_fcl_distance.cpp

@@ -411,7 +411,7 @@ bool collide_Test_OBB(const Transform3f& tf,
   m2.endModel();
 
   CollisionResult local_result;	
-  MeshCollisionTraversalNodeOBB node;
+  MeshCollisionTraversalNodeOBB node (false);
   if(!initialize(node, (const BVHModel<OBB>&)m1, tf, (const BVHModel<OBB>&)m2, Transform3f(),
                  CollisionRequest(), local_result))
     std::cout << "initialize error" << std::endl;

test/test_fcl_frontlist.cpp

@@ -227,7 +227,7 @@ bool collide_front_list_Test(const Transform3f& tf1, const Transform3f& tf2,
   Transform3f pose1, pose2;
 
   CollisionResult local_result;	
-  MeshCollisionTraversalNode<BV> node;
+  MeshCollisionTraversalNode<BV> node (false);
 
   if(!initialize<BV>(node, m1, pose1, m2, pose2,
                      CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
@@ -292,7 +292,7 @@ bool collide_front_list_Test_Oriented(const Transform3f& tf1, const Transform3f&
   Transform3f pose1(tf1), pose2;
 
   CollisionResult local_result;	
-  TraversalNode node;
+  TraversalNode node (false);
 
   if(!initialize(node, (const BVHModel<BV>&)m1, pose1, (const BVHModel<BV>&)m2, pose2,
                  CollisionRequest(std::numeric_limits<int>::max(), false), local_result))
@@ -343,7 +343,7 @@ bool collide_Test(const Transform3f& tf,
   Transform3f pose1(tf), pose2;
 
   CollisionResult local_result;	
-  MeshCollisionTraversalNode<BV> node;
+  MeshCollisionTraversalNode<BV> node (false);
 
   if(!initialize<BV>(node, m1, pose1, m2, pose2,
                      CollisionRequest(std::numeric_limits<int>::max(), false), local_result))