Merge pull request #325 from lmontaut/epa_fallback

core: add EPA::FallBack condition to shapeDistance computation

Author: jcarpent

include/hpp/fcl/narrowphase/gjk.h

@@ -254,6 +254,9 @@ struct HPP_FCL_DLLAPI GJK {
   /// @brief Get GJK number of iterations.
   inline size_t getIterations() { return iterations; }
 
+  /// @brief Get GJK tolerance.
+  inline FCL_REAL getTolerance() { return tolerance; }
+
  private:
   SimplexV store_v[4];
   SimplexV* free_v[4];

include/hpp/fcl/narrowphase/narrowphase.h

@@ -313,7 +313,7 @@ struct HPP_FCL_DLLAPI GJKSolver {
         if (epa_status & details::EPA::Valid ||
             epa_status == details::EPA::OutOfFaces        // Warnings
             || epa_status == details::EPA::OutOfVertices  // Warnings
-        ) {
+            || epa_status == details::EPA::FallBack) {
           Vec3f w0, w1;
           epa.getClosestPoints(shape, w0, w1);
           assert(epa.depth >= -eps);

src/narrowphase/gjk.cpp

@@ -1615,7 +1615,13 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
     }
   }
 
+  // FallBack when the simplex given by GJK is of rank 1.
+  // Since the simplex only contains support points which convex
+  // combination describe the origin, the point in the simplex is actually
+  // the origin.
   status = FallBack;
+  // TODO: define a better normal
+  assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
   normal = -guess;
   FCL_REAL nl = normal.norm();
   if (nl > 0)