Unverified Commit 6ce7e41a authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #231 from jcarpent/devel

Additional fix for collision checking
parents c21cebf4 3a574ad3
Pipeline #14900 passed with stage
in 40 minutes and 4 seconds
......@@ -97,7 +97,7 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
}
num_contacts = result.numContacts();
}
if (distance <= request.security_margin) {
else if (distance <= request.security_margin) {
if (result.numContacts () < request.num_max_contacts) {
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
......
......@@ -121,7 +121,7 @@ namespace fcl {
unit = c1c2/dist;
// Unlike in distance computation, we consider the security margin.
FCL_REAL penetrationDepth = r1 + r2 + margin - dist;
result.updateDistanceLowerBound (-penetrationDepth);
result.updateDistanceLowerBound (-penetrationDepth+margin);
bool collision = (penetrationDepth >= 0);
if (collision) {
// Take contact point at the middle of intersection between each sphere
......
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