Commit 90bd15ff authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[GJK] Make closest points more robust.

parent 11c6b0eb
......@@ -169,7 +169,7 @@ struct GJK
/// Get the closest points on each object.
/// \return true on success
bool getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const;
static bool getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1);
/// @brief get the guess from current simplex
Vec3f getGuessFromSimplex() const;
......
......@@ -77,11 +77,8 @@ namespace fcl
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
if(penetration_depth) *penetration_depth = -epa.depth;
if(normal) *normal = tf2.getRotation() * epa.normal;
if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
......@@ -127,11 +124,8 @@ 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);
assert (epa_status != details::EPA::Failed); (void) epa_status;
Vec3f w0 (Vec3f::Zero());
for(short i = 0; i < epa.result.rank; ++i)
{
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
distance = -epa.depth;
normal = -epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
......@@ -143,7 +137,7 @@ namespace fcl
{
col = false;
gjk.getClosestPoints (shape, p1, p2);
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
......@@ -188,7 +182,7 @@ namespace fcl
// TODO: understand why GJK fails between cylinder and box
assert (distance * distance < sqrt (eps));
Vec3f w0, w1;
gjk.getClosestPoints (shape, w0, w1);
details::GJK::getClosestPoints (*gjk.getSimplex(), w0, w1);
distance = 0;
p1 = p2 = tf1.transform (.5* (w0 + w1));
normal = Vec3f (0,0,0);
......@@ -196,7 +190,7 @@ namespace fcl
}
else if(gjk_status == details::GJK::Valid)
{
gjk.getClosestPoints (shape, p1, p2);
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
// TODO On degenerated case, the closest point may be wrong
// (i.e. an object face normal is colinear to gjk.ray
// assert (distance == (w0 - w1).norm());
......@@ -216,11 +210,8 @@ namespace fcl
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
Vec3f w0 (Vec3f::Zero());
for(short i = 0; i < epa.result.rank; ++i)
{
w0 += epa.result.vertex[i]->w0 * epa.result.coefficient[i];
}
Vec3f w0, w1;
details::GJK::getClosestPoints (epa.result, w0, w1);
assert (epa.depth >= -eps);
distance = std::min (0., -epa.depth);
normal = tf2.getRotation() * epa.normal;
......@@ -229,7 +220,7 @@ namespace fcl
}
else
{
gjk.getClosestPoints (shape, p1, p2);
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
distance = 0;
p1 = tf1.transform (p1);
......
......@@ -308,15 +308,59 @@ Vec3f GJK::getGuessFromSimplex() const
return ray;
}
bool GJK::getClosestPoints (const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const
bool GJK::getClosestPoints (const Simplex& simplex, Vec3f& w0, Vec3f& w1)
{
SimplexV* const* vs = simplex.vertex;
for (short i = 0; i < simplex.rank; ++i) {
assert (vs[i]->w.isApprox (vs[i]->w0 - vs[i]->w1));
}
Project::ProjectResult projection;
switch (simplex.rank) {
case 1:
w0 = vs[0]->w0;
w1 = vs[0]->w1;
return true;
case 2:
{
const Vec3f& a = vs[0]->w, a0 = vs[0]->w0, a1 = vs[0]->w1,
b = vs[1]->w, b0 = vs[1]->w0, b1 = vs[1]->w1;
FCL_REAL la, lb;
Vec3f N (b - a);
la = N.dot(-a);
if (la <= 0) {
w0 = a0;
w1 = a1;
} else {
lb = N.squaredNorm();
if (la > lb) {
w0 = b0;
w1 = b1;
} else {
lb = la / lb;
la = 1 - lb;
w0 = la * a0 + lb * b0;
w1 = la * a1 + lb * b1;
}
}
}
return true;
case 3:
// TODO avoid the reprojection
projection = Project::projectTriangleOrigin (vs[0]->w, vs[1]->w, vs[2]->w);
break;
case 4: // We are in collision.
projection = Project::projectTetrahedraOrigin (vs[0]->w, vs[1]->w, vs[2]->w, vs[3]->w);
break;
default:
throw std::logic_error ("The simplex rank must be in [ 1, 4 ]");
}
w0.setZero();
w1.setZero();
for(short i = 0; i < getSimplex()->rank; ++i)
{
FCL_REAL p = getSimplex()->coefficient[i];
w0 += getSimplex()->vertex[i]->w0 * p;
w1 += getSimplex()->vertex[i]->w1 * p;
for (short i = 0; i < simplex.rank; ++i) {
w0 += projection.parameterization[i] * vs[i]->w0;
w1 += projection.parameterization[i] * vs[i]->w1;
}
return true;
}
......
......@@ -644,7 +644,7 @@ bool GJKSolver_indep::shapeDistance<Capsule, Capsule>
details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
if(enable_cached_guess) cached_guess = gjk.getGuessFromSimplex();
gjk.getClosestPoints (shape, p1, p2);
details::GJK::getClosestPoints (*gjk.getSimplex(), p1, p2);
if((gjk_status == details::GJK::Valid) ||
(gjk_status == details::GJK::Failed))
......
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