GJK/EPA: fix epsilon in asserts

CHANGELOG.md

@@ -5,6 +5,7 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
+- Fixed too low tolerance in GJK/EPA asserts ([#554](https://github.com/humanoid-path-planner/hpp-fcl/pull/554))
 - Fixed `normal_and_nearest_points` test (no need to have Eigen 3.4) ([#553](https://github.com/humanoid-path-planner/hpp-fcl/pull/553))
 - [#549](https://github.com/humanoid-path-planner/hpp-fcl/pull/549)
   - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation.

include/hpp/fcl/narrowphase/narrowphase.h

@@ -199,8 +199,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
     }
     HPP_FCL_COMPILER_DIAGNOSTIC_POP
 
-    static constexpr FCL_REAL dummy_precision =
-        std::numeric_limits<FCL_REAL>::epsilon() * 100;
+    const FCL_REAL dummy_precision =
+        3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
     HPP_FCL_UNUSED_VARIABLE(dummy_precision);
     switch (gjk.status) {
       case details::GJK::DidNotRun:
@@ -410,11 +410,11 @@ struct HPP_FCL_DLLAPI GJKSolver {
     // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
     // it can safely be normalized.
     distance = gjk.distance;
-    static constexpr FCL_REAL dummy_precision =
-        std::numeric_limits<FCL_REAL>::epsilon() * 100;
+    const FCL_REAL dummy_precision =
+        3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
     HPP_FCL_UNUSED_VARIABLE(dummy_precision);
     HPP_FCL_ASSERT(
-        gjk.ray.norm() > gjk.getTolerance() + dummy_precision,
+        gjk.ray.norm() > gjk.getTolerance() - dummy_precision,
         "The norm of GJK's ray should be bigger than GJK's tolerance.",
         std::logic_error);
     normal.noalias() = -tf1.getRotation() * gjk.ray;
@@ -441,8 +441,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
                                                  FCL_REAL& distance, Vec3f& p1,
                                                  Vec3f& p2,
                                                  Vec3f& normal) const {
-    static constexpr FCL_REAL dummy_precision =
-        std::numeric_limits<FCL_REAL>::epsilon() * 100;
+    const FCL_REAL dummy_precision =
+        3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
     HPP_FCL_UNUSED_VARIABLE(dummy_precision);
     HPP_FCL_ASSERT(gjk.distance <= gjk.getTolerance() + dummy_precision,
                    "The distance should be lower than GJK's tolerance.",

src/narrowphase/gjk.cpp

@@ -1120,10 +1120,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
   const Vec3f a_cross_b = A.cross(B);
   const Vec3f a_cross_c = A.cross(C);
 
-  // dummy_precision is 1e-14 if FCL_REAL is double
-  //                    1e-5  if FCL_REAL is float
-  const FCL_REAL dummy_precision(100 *
-                                 std::numeric_limits<FCL_REAL>::epsilon());
+  const FCL_REAL dummy_precision(
+      3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
   HPP_FCL_UNUSED_VARIABLE(dummy_precision);
 
 #define REGION_INSIDE()               \
@@ -1747,8 +1745,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
         // the support we just added is in the direction of the normal of
         // the closest_face. Therefore, the support point will **always**
         // lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0.
-        assert(closest_face->n.dot(w.w) > 0 &&
-               "The support is not in the right direction.");
+        if (iterations > 0) {
+          assert(closest_face->n.dot(w.w) > -tolerance &&
+                 "The support is not in the right direction.");
+        }
         //
         // 1) First check: `fdist` (see below) is an upper bound of how much
         // more penetration depth we can expect to "gain" by adding `w` to EPA's
@@ -1921,8 +1921,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
   // recursive nature of `expand`, it is safer to go through the first case.
   // This is because `expand` can potentially loop indefinitly if the
   // Minkowski difference is very flat (hence the check above).
-  const FCL_REAL dummy_precision(100 *
-                                 std::numeric_limits<FCL_REAL>::epsilon());
+  const FCL_REAL dummy_precision(
+      3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
   const SimplexVertex& vf = sv_store[f->vertex_id[e]];
   if (f->n.dot(w.w - vf.w) < dummy_precision) {
     // case 1: the support point is "below" `f`.