Skip to content

GitLab

Commit 6e699b0c authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add squared distance lower bound as a reference parameter.

parent 1fea9f3f
Hide whitespace changes
Inline Side-by-side
include/fcl/traversal/traversal_node_base.h
View file @ 6e699b0c
......@@ -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;
......
include/fcl/traversal/traversal_node_bvh_shape.h
View file @ 6e699b0c
......@@ -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
}
......
include/fcl/traversal/traversal_node_shapes.h
View file @ 6e699b0c
......@@ -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;
......
src/traversal/traversal_node_base.cpp
View file @ 6e699b0c
......@@ -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
{
}
......
src/traversal/traversal_recurse.cpp
View file @ 6e699b0c
......@@ -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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment