Skip to content
Snippets Groups Projects
Commit 6e699b0c authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Add squared distance lower bound as a reference parameter.

parent 1fea9f3f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/fcl/traversal/traversal_node_base.h
+ 1
− 1
View file @ 6e699b0c
...@@ -106,7 +106,7 @@ public: ...@@ -106,7 +106,7 @@ public:
virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const; virtual bool BVTesting(int b1, int b2, FCL_REAL& sqrDistLowerBound) const;
/// @brief Leaf test between node b1 and b2, if they are both leafs /// @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 /// @brief Check whether the traversal can stop
virtual bool canStop() const; virtual bool canStop() const;
......
include/fcl/traversal/traversal_node_bvh_shape.h
+ 76
− 40
View file @ 6e699b0c
...@@ -272,17 +272,13 @@ public: ...@@ -272,17 +272,13 @@ public:
namespace details namespace details
{ {
template<typename BV, typename S, typename NarrowPhaseSolver> template<typename BV, typename S, typename NarrowPhaseSolver>
static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, static inline void meshShapeCollisionOrientedNodeLeafTesting
const BVHModel<BV>* model1, const S& model2, (int b1, int b2, const BVHModel<BV>* model1, const S& model2,
Vec3f* vertices, Triangle* tri_indices, Vec3f* vertices, Triangle* tri_indices, const Transform3f& tf1,
const Transform3f& tf1, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const Transform3f& tf2, bool enable_statistics, FCL_REAL cost_density, int& num_leaf_tests,
const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result,
bool enable_statistics, FCL_REAL& sqrDistLowerBound)
FCL_REAL cost_density,
int& num_leaf_tests,
const CollisionRequest& request,
CollisionResult& result)
{ {
if(enable_statistics) num_leaf_tests++; if(enable_statistics) num_leaf_tests++;
const BVNode<BV>& node = model1->getBV(b1); const BVNode<BV>& node = model1->getBV(b1);
...@@ -301,11 +297,27 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2, ...@@ -301,11 +297,27 @@ static inline void meshShapeCollisionOrientedNodeLeafTesting(int b1, int b2,
if(!request.enable_contact) // only interested in collision or not if(!request.enable_contact) // only interested in collision or not
{ {
if(nsolver->shapeTriangleIntersect(model2, tf2, p1, p2, p3, tf1, NULL, NULL, NULL)) if (request.enable_distance_lower_bound) {
{ FCL_REAL dist;
is_intersect = true; if (nsolver->shapeTriangleDistance (model2, tf2, p1, p2, p3, tf1,
if(request.num_max_contacts > result.numContacts()) &dist, 0x0, 0x0)) {
result.addContact(Contact(model1, &model2, primitive_id, Contact::NONE)); 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 else
...@@ -366,10 +378,13 @@ public: ...@@ -366,10 +378,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); (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: ...@@ -390,10 +405,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); (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: ...@@ -414,10 +432,13 @@ public:
return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), this->model2_bv, this->model1->getBV(b1).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); (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: ...@@ -447,10 +468,13 @@ public:
sqrDistLowerBound); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf1, this->tf2, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->result)); (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: ...@@ -561,10 +585,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); (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 // may need to change the order in pairs
} }
...@@ -585,10 +612,13 @@ public: ...@@ -585,10 +612,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); (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 // may need to change the order in pairs
} }
...@@ -610,10 +640,13 @@ public: ...@@ -610,10 +640,13 @@ public:
return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), this->model1_bv, this->model2->getBV(b2).bv); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); (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 // may need to change the order in pairs
} }
...@@ -643,10 +676,13 @@ public: ...@@ -643,10 +676,13 @@ public:
sqrDistLowerBound); 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, details::meshShapeCollisionOrientedNodeLeafTesting
this->tf2, this->tf1, this->nsolver, this->enable_statistics, this->cost_density, this->num_leaf_tests, this->request, *(this->request)); (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 // may need to change the order in pairs
} }
......
include/fcl/traversal/traversal_node_shapes.h
+ 1
− 0
View file @ 6e699b0c
...@@ -79,6 +79,7 @@ public: ...@@ -79,6 +79,7 @@ public:
/// @brief Intersection testing between leaves (two shapes) /// @brief Intersection testing between leaves (two shapes)
void leafTesting(int, int) const void leafTesting(int, int) const
{ {
abort ();
if(model1->isOccupied() && model2->isOccupied()) if(model1->isOccupied() && model2->isOccupied())
{ {
bool is_collision = false; bool is_collision = false;
......
src/traversal/traversal_node_base.cpp
+ 1
− 1
View file @ 6e699b0c
...@@ -95,7 +95,7 @@ bool CollisionTraversalNodeBase::BVTesting(int b1, int b2, FCL_REAL&) const ...@@ -95,7 +95,7 @@ bool CollisionTraversalNodeBase::BVTesting(int b1, int b2, FCL_REAL&) const
throw std::runtime_error ("Not implemented yet"); 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
+ 3
− 3
View file @ 6e699b0c
...@@ -43,7 +43,7 @@ namespace fcl ...@@ -43,7 +43,7 @@ namespace fcl
void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound) BVHFrontList* front_list, FCL_REAL& sqrDistLowerBound)
{ {
FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0; FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
bool l1 = node->isFirstNodeLeaf(b1); bool l1 = node->isFirstNodeLeaf(b1);
bool l2 = node->isSecondNodeLeaf(b2); bool l2 = node->isSecondNodeLeaf(b2);
if(l1 && l2) if(l1 && l2)
...@@ -56,7 +56,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2, ...@@ -56,7 +56,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
if(node->BVTesting(b1, b2)) return; if(node->BVTesting(b1, b2)) return;
} }
node->leafTesting(b1, b2); node->leafTesting(b1, b2, sqrDistLowerBound);
return; return;
} }
...@@ -406,7 +406,7 @@ void propagateBVHFrontListCollisionRecurse ...@@ -406,7 +406,7 @@ void propagateBVHFrontListCollisionRecurse
(CollisionTraversalNodeBase* node, BVHFrontList* front_list, (CollisionTraversalNodeBase* node, BVHFrontList* front_list,
FCL_REAL& sqrDistLowerBound) FCL_REAL& sqrDistLowerBound)
{ {
FCL_REAL sqrDistLowerBound1, sqrDistLowerBound2 = 0; FCL_REAL sqrDistLowerBound1 = 0, sqrDistLowerBound2 = 0;
BVHFrontList::iterator front_iter; BVHFrontList::iterator front_iter;
BVHFrontList append; BVHFrontList append;
for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter) for(front_iter = front_list->begin(); front_iter != front_list->end(); ++front_iter)
......
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