Commit aea29e11 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add CollisionRequest::updateDistanceLowerBound

parent f2d4f45c
...@@ -223,9 +223,16 @@ public: ...@@ -223,9 +223,16 @@ public:
public: public:
CollisionResult() CollisionResult()
: distance_lower_bound (std::numeric_limits<FCL_REAL>::max())
{ {
} }
/// @brief Update the lower bound only if the distance in inferior.
inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
{
if (distance_lower_bound_ < distance_lower_bound)
distance_lower_bound = distance_lower_bound_;
}
/// @brief add one contact into result structure /// @brief add one contact into result structure
inline void addContact(const Contact& c) inline void addContact(const Contact& c)
......
...@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1, ...@@ -89,7 +89,6 @@ std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
solver.cached_guess = request.cached_gjk_guess; solver.cached_guess = request.cached_gjk_guess;
const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable(); const CollisionFunctionMatrix& looktable = getCollisionFunctionLookTable();
result.distance_lower_bound = -1;
std::size_t res; std::size_t res;
if(request.num_max_contacts == 0) if(request.num_max_contacts == 0)
{ {
......
...@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf ...@@ -109,7 +109,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
} }
return 1; return 1;
} }
result.distance_lower_bound = distance; result.updateDistanceLowerBound (distance);
return 0; return 0;
} }
......
...@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node, ...@@ -60,7 +60,7 @@ void collide(CollisionTraversalNodeBase* node,
collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound); collisionRecurse(node, 0, 0, front_list, sqrDistLowerBound);
else else
collisionNonRecurse(node, front_list, sqrDistLowerBound); collisionNonRecurse(node, front_list, sqrDistLowerBound);
result.distance_lower_bound = sqrt (sqrDistLowerBound); result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
} }
} }
......
...@@ -132,7 +132,7 @@ namespace fcl { ...@@ -132,7 +132,7 @@ namespace fcl {
result.addContact (contact); result.addContact (contact);
return 1; return 1;
} }
result.distance_lower_bound = -penetrationDepth; result.updateDistanceLowerBound (-penetrationDepth);
return 0; return 0;
} }
} // namespace fcl } // namespace fcl
......
...@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse ...@@ -399,7 +399,7 @@ void propagateBVHFrontListCollisionRecurse
} }
} }
} }
result.distance_lower_bound = sqrt (sqrDistLowerBound); result.updateDistanceLowerBound (sqrt (sqrDistLowerBound));
} }
......
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