Verified Commit 37f46961 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

collision: update distance lower bound

parent b0d0046a
......@@ -82,6 +82,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
FCL_REAL distance = ShapeShapeDistance <T_SH1, T_SH2>
(o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult);
size_t num_contacts = 0;
if (distance <= 0) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
......@@ -94,7 +95,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
result.addContact (contact);
}
return 1;
num_contacts = result.numContacts();
}
if (distance <= request.security_margin) {
if (result.numContacts () < request.num_max_contacts) {
......@@ -107,10 +108,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
-distance+request.security_margin);
result.addContact (contact);
}
return 1;
num_contacts = result.numContacts();
}
result.updateDistanceLowerBound (distance);
return 0;
return num_contacts;
}
namespace details
......
Supports Markdown
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