diff --git a/include/fcl/traversal/traversal_node_bvhs.h b/include/fcl/traversal/traversal_node_bvhs.h index eea425a509934a96c942f0ff3b2fe11873e968a4..7ef99756604c71c5a2130444881023ea1006f3a6 100644 --- a/include/fcl/traversal/traversal_node_bvhs.h +++ b/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; diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index 6e2e2be6c37f2343dc83d9dfcbb97d5a3d16f0d9..6cc4853f8e05a51acbf7c7c1b451011e2b5d3ef5 100644 --- a/src/collision_func_matrix.cpp +++ b/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); diff --git a/src/traversal/traversal_node_bvhs.cpp b/src/traversal/traversal_node_bvhs.cpp index a07d99d66f33d39431a2d8abbc8e130de353e9cf..72cfc5e8307affdd3fa05f846dbb7a4d107714f9 100644 --- a/src/traversal/traversal_node_bvhs.cpp +++ b/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(); } diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index d70fe25e897f3fc47936857337545230718c4c8a..94ea55cde7e98563bd859d8b28f2621d7a5f4e2b 100644 --- a/test/test_fcl_collision.cpp +++ b/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; diff --git a/test/test_fcl_distance.cpp b/test/test_fcl_distance.cpp index 181a659286099b0698839e37432219c8dd910dd5..d58d1d0f02c82391abd7903ddf12240338d1e861 100644 --- a/test/test_fcl_distance.cpp +++ b/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; diff --git a/test/test_fcl_frontlist.cpp b/test/test_fcl_frontlist.cpp index 45ad58227b60ba5c212cc4eae786a4fd45cc07e4..0b880c763a211b17c47a78b7e2c9eda14bf36acf 100644 --- a/test/test_fcl_frontlist.cpp +++ b/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))