diff --git a/include/fcl/traversal/traversal_node_base.h b/include/fcl/traversal/traversal_node_base.h
index 6e8559ffbc7f51cbefdd2bd50b1d8f37855b2e17..a2c5f9cdd0b100040087654e50ef519ba7fba962 100644
--- a/include/fcl/traversal/traversal_node_base.h
+++ b/include/fcl/traversal/traversal_node_base.h
@@ -106,7 +106,7 @@ public:
virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs
- virtual void leafTesting(int b1, int b2) const;
+ virtual void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the traversal can stop
virtual bool canStop() const;
diff --git a/include/fcl/traversal/traversal_node_bvh_shape.h b/include/fcl/traversal/traversal_node_bvh_shape.h
index f1f766eee5fc2a60d94b049e508067fdfb2cbf19..e142ce59207e33732b2136f4b419ae03c404b76a 100644
--- a/include/fcl/traversal/traversal_node_bvh_shape.h
+++ b/include/fcl/traversal/traversal_node_bvh_shape.h
@@ -272,17 +272,13 @@ public:
namespace details
{
template<typename BV, typename S, typename NarrowPhaseSolver>
-static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
- const BVHModel<BV>* model1, const S& model2,
- Vec3f* vertices, Triangle* tri_indices,
- const Transform3f& tf1,
- const Transform3f& tf2,
- const NarrowPhaseSolver* nsolver,
- bool enable_statistics,
- FCL_REAL cost_density,
- int& num_leaf_tests,
- const CollisionRequest& request,
- CollisionResult& result)
+static inline void meshShapeCollisionOrientedNodeLeafTesting
+ (int b1, int b2, const BVHModel<BV>* model1, const S& model2,
+ Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,
+ const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
+ bool enable_statistics, FCL_REAL cost_density, int& num_leaf_tests,
+ const CollisionRequest& request, CollisionResult& result,
+ FCL_REAL& sqrDistLowerBound)
{
if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1);
@@ -301,11 +297,27 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
if(!request.enable_contact) // only interested in collision or not
{
- if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL))
- {
- is_intersect = true;
- if(request.num_max_contacts > result.numContacts())
- result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE));
+ if (request.enable_distance_lower_bound) {
+ FCL_REAL dist;
+ if (nsolver->shapeTriangleDistance (model2, tf2, p1, p2, p3, tf1,
+ &dist, 0x0, 0x0)) {
+ sqrDistLowerBound = dist * dist;
+ } else {
+ // collision
+ is_intersect = true;
+ sqrDistLowerBound = 0;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id,
+ Contact::NONE));
+ }
+ } else {
+ if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1,
+ NULL, NULL, NULL)) {
+ is_intersect = true;
+ if(request.num_max_contacts > result.numContacts())
+ result.addContact(Contact(model1, &model2, primitive_id,
+ Contact::NONE));
+ }
}
}
else
@@ -366,10 +378,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->result), sqrDistLowerBound);
}
};
@@ -390,10 +405,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->result), sqrDistLowerBound);
}
};
@@ -414,10 +432,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->result), sqrDistLowerBound);
}
};
@@ -447,10 +468,13 @@ public:
sqrDistLowerBound);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
- this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b1, b2, this->model1, *(this->model2), this->vertices, this->tri_indices,
+ this->tf1, this->tf2, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->result), sqrDistLowerBound);
}
};
@@ -561,10 +585,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
@@ -585,10 +612,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->request));
// may need to change the order in pairs
}
@@ -610,10 +640,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
+ this->tf2, this->tf1, this->nsolver, this->enable_statistics,
+ this->cost_density, this->num_leaf_tests, this->request,
+ *(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
@@ -643,10 +676,13 @@ public:
sqrDistLowerBound);
}
- void leafTesting(int b1, int b2) const
+ void leafTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const
{
- details::meshShapeCollisionOrientedNodeLeafTesting(b2, b1, *(this->model2), this->model1, this->vertices, this->tri_indices,
- this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request));
+ details::meshShapeCollisionOrientedNodeLeafTesting
+ (b2, b1, *(this->model2), this->model1, this->vertices,
+ this->tri_indices, this->tf2, this->tf1, this->nsolver,
+ this->enable_statistics, this->cost_density, this->num_leaf_tests,
+ this->request, *(this->request), sqrDistLowerBound);
// may need to change the order in pairs
}
diff --git a/include/fcl/traversal/traversal_node_shapes.h b/include/fcl/traversal/traversal_node_shapes.h
index a67fe4d4d86be3efb1f158f7d87463a542455412..64c27980dedf7fd1e155ecfa91a78f2842ac71e1 100644
--- a/include/fcl/traversal/traversal_node_shapes.h
+++ b/include/fcl/traversal/traversal_node_shapes.h
@@ -79,6 +79,7 @@ public:
/// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int) const
{
+ abort ();
if(model1->isOccupied() && model2->isOccupied())
{
bool is_collision = false;
diff --git a/src/traversal/traversal_node_base.cpp b/src/traversal/traversal_node_base.cpp
index 66172aeed096a4c775ad36cbe32ec7c98cc4654a..6717a36bdc538aa78dc646de22042e63fbbbc996 100644
--- a/src/traversal/traversal_node_base.cpp
+++ b/src/traversal/traversal_node_base.cpp
@@ -95,7 +95,7 @@ bool CollisionTraversalNodeBase::BVTesting(int b1, int b2, FCL_REAL&) const
throw std::runtime_error ("Not implemented yet");
}
-void CollisionTraversalNodeBase::leafTesting(int b1, int b2) const
+void CollisionTraversalNodeBase::leafTesting(int, int, FCL_REAL&) const
{
}
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index e2c731c42e0e25cbdc0cf03ed12d337d231a56f0..ef652b92fa6773b6db3c9a90192e78c4a48dd1f8 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -43,7 +43,7 @@ namespace fcl
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)
{
- FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0;
+ FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
bool l1 = node->isFirstNodeLeaf(b1);
bool l2 = node->isSecondNodeLeaf(b2);
if(l1 && l2)
@@ -56,7 +56,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
if(node->BVTesting(b1, b2)) return;
}
- node->leafTesting(b1, b2);
+ node->leafTesting(b1, b2, sqrDistLowerBound);
return;
}
@@ -406,7 +406,7 @@ void propagateBVHFrontListCollisionRecurse
(CollisionTraversalNodeBase* node, BVHFrontList* front_list,
FCL_REAL& sqrDistLowerBound)
{
- FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0;
+ FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
BVHFrontList::iterator front_iter;
BVHFrontList append;
for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)