Address EPA failure from issue 493

 - Introduce two regression tests (the two posted in the issue).
 - Correct a few confusing whitepsace formatting issues.
 - Modify the validateNearestFeatureOfPolytopeBeingEdge() function in two
   ways
   - Distance from origin to face must now be greater than epsilon (instead
     of zero). Justificaiton for this change provided.
   - Test to determine if the edge *truly* is the nearest feature has been
     simplified and documentation updated. This led to the removal of the
     is_point_on_line_segment() function.

Author: SeanCurtis-TRI

CHANGELOG.md

@@ -33,6 +33,8 @@
     [#472](https://github.com/flexible-collision-library/fcl/pull/472)
   * Documentation for OcTree no longer mistakenly excluded from doxygen:
     [#472](https://github.com/flexible-collision-library/fcl/pull/472)
+  * Another failure mode in the GJK/EPA signed distance query patched:
+    [#494](https://github.com/flexible-collision-library/fcl/pull/494)
 
 * Build/Test/Misc
 

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

@@ -966,28 +966,6 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
   return false;
 }
 
-/**
- * Determines if the point P is on the line segment AB.
- * If A, B, P are coincident, report true.
- */
-static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a,
-                                     const ccd_vec3_t& b) {
-  if (are_coincident(a, b)) {
-    return are_coincident(a, p);
-  }
-  // A and B are not coincident, if the triangle ABP has non-zero area, then P
-  // is not on the line adjoining AB, and hence not on the line segment AB.
-  if (!triangle_area_is_zero(a, b, p)) {
-    return false;
-  }
-  // P is on the line adjoinging AB. If P is on the line segment AB, then
-  // PA.dot(PB) <= 0.
-  ccd_vec3_t PA, PB;
-  ccdVec3Sub2(&PA, &p, &a);
-  ccdVec3Sub2(&PB, &p, &b);
-  return ccdVec3Dot(&PA, &PB) <= 0;
-}
-
 /**
  * Computes the normal vector of a triangular face on a polytope, and the normal
  * vector points outward from the polytope. Notice we assume that the origin
@@ -1691,6 +1669,8 @@ static int __ccdGJK(const void *obj1, const void *obj2,
  */
 static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
   assert(polytope->nearest_type == CCD_PT_EDGE);
+
+  constexpr ccd_real_t kEps = constants<ccd_real_t>::eps();
   // Only verify the feature if the nearest feature is an edge.
 
   const ccd_pt_edge_t* const nearest_edge =
@@ -1701,6 +1681,14 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
   // normalized.
   std::array<ccd_vec3_t, 2> face_normals;
   std::array<double, 2> origin_to_face_distance;
+
+  // We define the plane equation using vertex[0]. If vertex[0] is far away
+  // from the origin, it can magnify rounding error. We scale epsilon to account
+  // for this possibility.
+  const ccd_real_t v0_dist =
+      std::sqrt(ccdVec3Len2(&nearest_edge->vertex[0]->v.v));
+  const ccd_real_t plane_threshold = kEps * std::max(1.0, v0_dist);
+
   for (int i = 0; i < 2; ++i) {
     face_normals[i] =
         faceNormalPointingOutward(polytope, nearest_edge->faces[i]);
@@ -1709,27 +1697,29 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
     // n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0 (since n̂ ⋅ o = 0).
     origin_to_face_distance[i] =
         -ccdVec3Dot(&face_normals[i], &nearest_edge->vertex[0]->v.v);
-    if (origin_to_face_distance[i] > 0) {
+    // If the origin lies *on* the edge, then it also lies on the two adjacent
+    // faces. Rather than failing on strictly *positive* signed distance, we
+    // introduce an epsilon threshold. This usage of epsilon is to account for a
+    // discrepancy in the signed distance computation. How GJK (and partially
+    // EPA) compute the signed distance from origin to face may *not* be exactly
+    // the same as done in this test (i.e. for a given set of vertices, the
+    // plane equation can be defined various ways. Those ways are
+    // *mathematically* equivalent but numerically will differ due to rounding).
+    // We account for those differences by allowing a very small positive signed
+    // distance to be considered zero. We assume that the GJK/EPA code
+    // ultimately classifies inside/outside around *zero* and *not* epsilon.
+    if (origin_to_face_distance[i] > plane_threshold) {
       FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
           "The origin is outside of the polytope. This should already have "
           "been identified as separating.");
     }
   }
-  // We compute the projection of the origin onto the plane as
-  // -face_normals[i] * origin_to_face_distance[i]
-  // If the projection to both faces are on the edge, the the edge is the
-  // closest feature.
-  bool is_edge_closest_feature = true;
-  for (int i = 0; i < 2; ++i) {
-    ccd_vec3_t origin_projection_to_plane = face_normals[i];
-    ccdVec3Scale(&(origin_projection_to_plane), -origin_to_face_distance[i]);
-    if (!is_point_on_line_segment(origin_projection_to_plane,
-                                  nearest_edge->vertex[0]->v.v,
-                                  nearest_edge->vertex[1]->v.v)) {
-      is_edge_closest_feature = false;
-      break;
-    }
-  }
+
+  // We know the reported squared distance to the edge. If that distance is
+  // functionally zero, then the edge must *truly* be the nearest feature.
+  // If it isn't, then it must be one of the adjacent faces.
+  const bool is_edge_closest_feature = nearest_edge->dist < kEps * kEps;
+
   if (!is_edge_closest_feature) {
     // We assume that libccd is not crazily wrong. Although the closest
     // feature is not the edge, it is near that edge. Hence we select the
@@ -1779,7 +1769,7 @@ static int __ccdEPA(const void *obj1, const void *obj2,
         return -2;
     }
 
-    while (1){
+    while (1) {
       // get triangle nearest to origin
       *nearest = ccdPtNearest(polytope);
       if (polytope->nearest_type == CCD_PT_EDGE) {
@@ -1791,14 +1781,13 @@ static int __ccdEPA(const void *obj1, const void *obj2,
         *nearest = ccdPtNearest(polytope);
       }
 
-        // get next support point
-        if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
-            break;
-        }
+      // get next support point
+      if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
+        break;
+      }
 
-        // expand nearest triangle using new point - supp
-        if (expandPolytope(polytope, *nearest, &supp) != 0)
-            return -2;
+      // expand nearest triangle using new point - supp
+      if (expandPolytope(polytope, *nearest, &supp) != 0) return -2;
     }
 
     return 0;

test/test_fcl_signed_distance.cpp

@@ -460,6 +460,62 @@ void test_distance_box_box_regression6() {
   test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
 }
 
+// This is a *specific* case that has cropped up in the wild. This error was
+// reported in https://github.com/flexible-collision-library/fcl/issues/493.
+// This includes the *first* scenario.
+template <typename S>
+void test_distance_box_box_regression7a() {
+  const Vector3<S> box_size(0.5, 0.5, 0.5);
+  // Both transforms have the same orientation.
+  Matrix3<S> R_WB;
+  // clang-format off
+  R_WB <<
+       0.96193976625564348026,  0.15401279915737359216, 0.22572537250328919556,
+      -0.19134171618254486313,  0.96936494951283913579, 0.15401279915737359216,
+      -0.19509032201612822033, -0.19134171618254486313, 0.96193976625564348026;
+  // clang-format on
+  Transform3<S> X_WB1 = Transform3<S>::Identity();
+  X_WB1.linear() = R_WB;
+  // clang-format off
+  X_WB1.translation() <<
+      2.0770063995786869349, 0.48468247475641951239, 1.1543291419087275962;
+  // clang-format on
+  Transform3<S> X_WB2 = Transform3<S>::Identity();
+  X_WB2.linear() = R_WB;
+  X_WB2.translation() << 2, 0, 1.25;
+  const double expected_distance{0};
+  test_distance_box_box_helper(box_size, X_WB1, box_size, X_WB2,
+                               &expected_distance);
+}
+
+// This is a *specific* case that has cropped up in the wild. This error was
+// reported in https://github.com/flexible-collision-library/fcl/issues/493.
+// This includes the *second* scenario.
+template <typename S>
+void test_distance_box_box_regression7b() {
+  const Vector3<S> box_size(0.25, 0.25, 0.25);
+  // Both transforms have the same orientation.
+  Matrix3<S> R_WB;
+  // clang-format off
+  R_WB <<
+       0.95233240645725869555,   0.27969320707964251405,  0.12179777307008109177,
+      -0.28888687719060901493,   0.95512127579560024415,  0.065480689593520297054,
+      -0.098017140329560589751, -0.097545161008064124042, 0.99039264020161521529;
+  // clang-format on
+  Transform3<S> X_WB1 = Transform3<S>::Identity();
+  X_WB1.linear() = R_WB;
+  // clang-format off
+  X_WB1.translation() <<
+      0.069923301769910642389, 0.23878031894890006104, 1.2256137097479840037;
+  // clang-format on
+  Transform3<S> X_WB2 = Transform3<S>::Identity();
+  X_WB2.linear() = R_WB;
+  X_WB2.translation() << 0, 0, 1.25;
+  const double expected_distance{0};
+  test_distance_box_box_helper(box_size, X_WB1, box_size, X_WB2,
+                               &expected_distance);
+}
+
 // This is a *specific* case that has cropped up in the wild that reaches the
 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
 // reported in https://github.com/flexible-collision-library/fcl/issues/408
@@ -533,6 +589,8 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) {
   test_distance_box_box_regression4<double>();
   test_distance_box_box_regression5<double>();
   test_distance_box_box_regression6<double>();
+  test_distance_box_box_regression7a<double>();
+  test_distance_box_box_regression7b<double>();
   test_distance_sphere_box_regression1<double>();
 }