[GJK] Make closest points more robust.

include/hpp/fcl/narrowphase/gjk.h

@@ -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;

include/hpp/fcl/narrowphase/narrowphase.h

@@ -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);

src/narrowphase/gjk.cpp

@@ -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;
 }

src/narrowphase/narrowphase.cpp

@@ -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))