Commit 940017f6 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK/EPA] Fix some numerical issues (and update unit-test accordingly)

parent 59bf5727
......@@ -194,7 +194,7 @@ struct GJK
/// from GJK. Calling EPA has an undefined behaviour.
bool hasPenetrationInformation (const MinkowskiDiff& shape)
{
return distance >= - shape.inflation.sum();
return distance > - shape.inflation.sum();
}
/// Get the closest points on each object.
......
......@@ -90,9 +90,8 @@ namespace fcl
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
return true;
}
// TODO EPA failed but we know there is a collision so we should
// return true;
return false;
// EPA failed but we know there is a collision so we should
return true;
}
break;
default:
......@@ -228,7 +227,10 @@ namespace fcl
details::EPA epa(epa_max_face_num, epa_max_vertex_num,
epa_max_iterations, epa_tolerance);
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status & details::EPA::Valid)
if(epa_status & details::EPA::Valid
|| epa_status == details::EPA::OutOfFaces // Warnings
|| epa_status == details::EPA::OutOfVertices // Warnings
)
{
Vec3f w0, w1;
epa.getClosestPoints (shape, w0, w1);
......
......@@ -167,16 +167,24 @@ void getShapeSupport(const Cone* cone, const Vec3f& dir, Vec3f& support)
void getShapeSupport(const Cylinder* cylinder, const Vec3f& dir, Vec3f& support)
{
static const FCL_REAL eps (sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
// The inflation makes the object look strictly convex to GJK and EPA. This
// helps solving particular cases (e.g. a cylinder with itself at the same
// position...)
static const FCL_REAL inflate = 1.00001;
FCL_REAL half_h = cylinder->halfLength;
if (dir [2] > eps) support[2] = half_h;
else if (dir [2] < -eps) support[2] = -half_h;
else support[2] = 0;
if (dir.head<2>().isZero())
FCL_REAL r = cylinder->radius;
if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero()) half_h *= inflate;
if (dir[2] > 0) support[2] = half_h;
else if (dir[2] < 0) support[2] = -half_h;
else { support[2] = 0; r *= inflate; }
if (dir.head<2>() == Eigen::Matrix<FCL_REAL,2,1>::Zero())
support.head<2>().setZero();
else
support.head<2>() = dir.head<2>().normalized() * cylinder->radius;
assert (fabs (support [0] * dir [1] - support [1] * dir [0]) < eps);
support.head<2>() = dir.head<2>().normalized() * r;
assert (fabs (support [0] * dir [1] - support [1] * dir [0])
< sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
}
void getShapeSupport(const ConvexBase* convex, const Vec3f& dir, Vec3f& support)
......@@ -1303,7 +1311,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
valid &= expand(pass, w, best->f[j], best->e[j], horizon);
if(!valid || horizon.nf < 3) {
status = InvalidHull;
// The status has already been set by the expand function.
assert(!(status & Valid));
break;
}
// need to add the edge connectivity between first and last faces
......@@ -1343,8 +1352,11 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
static const size_t previ[] = {2, 0, 1};
if(f->pass == pass)
{
status = InvalidHull;
return false;
}
const size_t e1 = nexti[e];
// case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
......
......@@ -3108,19 +3108,18 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_cylindercylinder)
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(9.9, 0, 0));
normal << 1, 0, 0;
testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, 3e-1); // built-in GJK solver requires larger tolerance than libccd
testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(9.9, 0, 0));
normal = transform.getRotation() * Vec3f(1, 0, 0);
testShapeIntersection(s1, tf1, s2, tf2, true);
// built-in GJK solver returns incorrect normal.
// testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = Transform3f();
tf2 = Transform3f(Vec3f(10, 0, 0));
normal << 1, 0, 0;
testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal);
testShapeIntersection(s1, tf1, s2, tf2, true, NULL, NULL, &normal, false, tol_gjk);
tf1 = transform;
tf2 = transform * Transform3f(Vec3f(10.01, 0, 0));
......
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