[GJK] Add GJK::setDistanceEarlyBreak

include/hpp/fcl/narrowphase/gjk.h

@@ -172,6 +172,15 @@ struct GJK
   /// @brief get the guess from current simplex
   Vec3f getGuessFromSimplex() const;
 
+  /// @brief Distance threshold for early break.
+  /// GJK stops when it proved the distance is more than this threshold.
+  /// \note The closest points will be erroneous in this case.
+  ///       If you want the closest points, set this to infinity (the default).
+  void setDistanceEarlyBreak (const FCL_REAL& dup)
+  {
+    distance_upper_bound = dup;
+  }
+
 private:
   SimplexV store_v[4];
   SimplexV* free_v[4];
@@ -182,6 +191,7 @@ private:
 
   unsigned int max_iterations;
   FCL_REAL tolerance;
+  FCL_REAL distance_upper_bound;
 
   /// @brief Project origin (0) onto line a-b
   bool projectLineOrigin(const Simplex& current, Simplex& next);

src/narrowphase/gjk.cpp

@@ -297,6 +297,7 @@ void GJK::initialize()
 {
   nfree = 0;
   status = Failed;
+  distance_upper_bound = std::numeric_limits<FCL_REAL>::max();
   simplex = NULL;
 }
 
@@ -404,8 +405,15 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
     // check removed (by ?): when the new support point is close to previous support points, stop (as the new simplex is degenerated)
     const Vec3f& w = curr_simplex.vertex[curr_simplex.rank - 1]->w;
 
-    // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
+    // check B: no collision if omega > 0
     FCL_REAL omega = ray.dot(w) / rl;
+    if (omega > distance_upper_bound)
+    {
+      distance = omega;
+      break;
+    }
+
+    // check C: when the new support point is close to the sub-simplex where the ray point lies, stop (as the new simplex again is degenerated)
     alpha = std::max(alpha, omega);
     if((rl - alpha) - tolerance * rl <= 0)
     {