Commit 97389ba1 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[GJK] Allow distance to be slightly positive if collision

  in GJKSolver_indep::shapeDistance<TriangleP, TriangleP>.
parent 7de0d557
......@@ -654,7 +654,6 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
}
else if (gjk_status == details::GJK::Inside)
{
dist = 0;
p1 = tf1.transform (w0);
p2 = tf1.transform (w1);
......@@ -662,8 +661,9 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
FCL_REAL penetrationDepth = details::computePenetration
(P1, P2, P3, Q1, Q2, Q3, tf1, tf2, normal);
dist = -penetrationDepth;
assert (dist <= 0);
assert (dist <= 1e-6);
}
dist = 0;
return false;
}
assert (false && "should not reach this point");
......
......@@ -145,7 +145,7 @@ BOOST_AUTO_TEST_CASE(distance_triangle_triangle_1)
// check that moving triangle 2 by the penetration depth in the
// direction of the normal makes the triangles collision free.
FCL_REAL penetration_depth (-distance);
assert (penetration_depth > 0);
assert (penetration_depth >= 0);
tf2.setTranslation ((penetration_depth + 10-4) * normal);
res = solver.shapeDistance (tri1, tf1, tri2, tf2, distance, c1, c2,
normal2);
......
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