core: add assert to epa fallback to check that GJK has found the origin

Author: lmontaut

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

src/narrowphase/gjk.cpp

@@ -1621,6 +1621,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
   // 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)