Address EPA failure from issue 493

CHANGELOG.md

@@ -11,6 +11,9 @@
 
 * Math
 
+  * constants::eps() is now constexpr:
+    [#494](https://github.com/flexible-collision-library/fcl/pull/494)
+
 * Geometry
 
   * OcTree logic for determining free/occupied:
@@ -30,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/math/constants.h

@@ -148,14 +148,18 @@ static Real gjk_default_tolerance() {
 }
 
 /// Returns ε for the precision of the underlying scalar type.
-static Real eps() {
+static constexpr Real eps() {
   static_assert(std::is_floating_point<Real>::value,
                 "Constants can only be evaluated for scalars with floating "
                 "point implementations");
-  static const Real value = std::numeric_limits<Real>::epsilon();
-  return value;
+  return std::numeric_limits<Real>::epsilon();
 }
 
+// TODO(SeanCurtis-TRI) These are *not* declared constexpr because the clang
+//  compiler available in the current CI configuration for ubuntu and mac does
+//  not have std::pow declared as constexpr. When that changes, these can
+//  likewise be declared as constexpr.
+
 /// Returns ε^(7/8) for the precision of the underlying scalar type.
 static Real eps_78() {
   static const Real value = std::pow(eps(), 7./8.);

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

@@ -313,7 +313,7 @@ _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
 static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) {
   // Used to define numerical thresholds near zero; typically scaled to the size
   // of the quantities being tested.
-  const ccd_real_t eps = constants<ccd_real_t>::eps();
+  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
 
   const Vector3<ccd_real_t> p_OA(simplex->ps[simplex->last].v.v);
   const Vector3<ccd_real_t> p_OB(simplex->ps[0].v.v);
@@ -922,7 +922,7 @@ static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
   using std::abs;
   using std::max;
 
-  const ccd_real_t eps = constants<ccd_real_t>::eps();
+  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
   // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd
   // is actually float based.
   for (int i = 0; i < 3; ++i) {
@@ -960,34 +960,12 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
   ccdVec3Normalize(&AB);
   ccdVec3Normalize(&AC);
   ccdVec3Cross(&n, &AB, &AC);
-  const ccd_real_t eps = constants<ccd_real_t>::eps();
+  constexpr ccd_real_t eps = constants<ccd_real_t>::eps();
   // Second co-linearity condition.
   if (ccdVec3Len2(&n) < eps * eps) return true;
   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,14 @@ static int __ccdGJK(const void *obj1, const void *obj2,
  */
 static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
   assert(polytope->nearest_type == CCD_PT_EDGE);
+
+  // We define epsilon to include an additional bit of noise. The goal is to
+  // pick the smallest epsilon possible. This factor of two proved necessary
+  // due to unit test behavior on the mac. In the future, as we collect
+  // more evidence, it may be necessary to increase to more bits. But the need
+  // should always be demonstrable and not purely theoretical.
+  constexpr ccd_real_t kEps = 2 * 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 +1687,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 +1703,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 +1775,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 +1787,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;

include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_box-inl.h

@@ -135,7 +135,7 @@ FCL_EXPORT bool sphereBoxIntersect(const Sphere<S>& sphere,
     // Furthermore, in finding the *near* face, a better candidate must be more
     // than this epsilon closer to the sphere center (see the test in the
     // else branch).
-    auto eps = 16 * constants<S>::eps();
+    constexpr auto eps = 16 * constants<S>::eps();
     if (N_is_not_C && squared_distance > eps * eps) {
       // The center is on the outside. Normal direction is from C to N (computed
       // above) and penetration depth is r - |p_BN - p_BC|. The contact position

include/fcl/narrowphase/detail/primitive_shape_algorithm/sphere_cylinder-inl.h

@@ -155,7 +155,7 @@ FCL_EXPORT bool sphereCylinderIntersect(
     // Furthermore, in finding the *near* face, a better candidate must be more
     // than this epsilon closer to the sphere center (see the test in the
     // else branch).
-    const auto eps = 16 * constants<S>::eps();
+    constexpr auto eps = 16 * constants<S>::eps();
     if (S_is_outside && p_SN_squared_dist > eps * eps) {
       // The sphere center is *measurably outside* the cylinder. There are three
       // possibilities: nearest point lies on the cap face, cap edge, or barrel.

test/test_fcl_signed_distance.cpp

@@ -325,12 +325,22 @@ void test_distance_box_box_helper(const Vector3<S>& box1_size,
   // p_B1P1 is the position of the witness point P1 on box 1, measured
   // and expressed in the box 1 frame B1.
   const Vector3<S> p_B1P1 = X_WB1.inverse() * result.nearest_points[0];
-  const double tol = 10 * std::numeric_limits<S>::epsilon();
-  EXPECT_TRUE((p_B1P1.array().abs() <= (box1_size / 2).array() + tol).all());
+  constexpr double tol = 10 * constants<S>::eps();
+  const double tol_1 = tol * std::max(S(1), (box1_size / 2).maxCoeff());
+  EXPECT_TRUE(
+      (p_B1P1.array().abs() <= (box1_size / 2).array() + tol_1).all())
+      << "\n  p_B1P1: " << p_B1P1.transpose()
+      << "\n  box1_size / 2: " << (box1_size / 2).transpose()
+      << "\n  tol: " << tol_1;
   // p_B2P2 is the position of the witness point P2 on box 2, measured
   // and expressed in the box 2 frame B2.
+  const double tol_2 = tol * std::max(S(1), (box2_size / 2).maxCoeff());
   const Vector3<S> p_B2P2 = X_WB2.inverse() * result.nearest_points[1];
-  EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all());
+  EXPECT_TRUE(
+      (p_B2P2.array().abs() <= (box2_size / 2).array() + tol_2).all())
+      << "\n  p_B2P2: " << p_B2P2.transpose()
+      << "\n  box2_size / 2: " << (box2_size / 2).transpose()
+      << "\n  tol: " << tol_2;
 
   // An expected distance has been provided; let's test that the value is as
   // expected.
@@ -345,6 +355,7 @@ void test_distance_box_box_helper(const Vector3<S>& box1_size,
 // reported in https://github.com/flexible-collision-library/fcl/issues/388
 template <typename S>
 void test_distance_box_box_regression1() {
+  SCOPED_TRACE("test_distance_box_box_regression1");
   const Vector3<S> box1_size(0.03, 0.12, 0.1);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
@@ -370,6 +381,7 @@ void test_distance_box_box_regression1() {
 // reported in https://github.com/flexible-collision-library/fcl/issues/395
 template <typename S>
 void test_distance_box_box_regression2() {
+  SCOPED_TRACE("test_distance_box_box_regression2");
   const Vector3<S> box1_size(0.46, 0.48, 0.01);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   X_WB1.matrix() <<  1,0,0, -0.72099999999999997424,
@@ -393,6 +405,7 @@ void test_distance_box_box_regression2() {
 // reported in https://github.com/flexible-collision-library/fcl/issues/415
 template <typename S>
 void test_distance_box_box_regression3() {
+  SCOPED_TRACE("test_distance_box_box_regression3");
   const Vector3<S> box1_size(0.49, 0.05, 0.21);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   // clang-format off
@@ -416,6 +429,7 @@ void test_distance_box_box_regression3() {
 // reported in https://github.com/flexible-collision-library/fcl/issues/398
 template <typename S>
 void test_distance_box_box_regression4() {
+  SCOPED_TRACE("test_distance_box_box_regression4");
   const Vector3<S> box1_size(0.614, 3, 0.37);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   X_WB1.translation() << -0.675, 0, 0.9115;
@@ -429,6 +443,7 @@ void test_distance_box_box_regression4() {
 // reported in https://github.com/flexible-collision-library/fcl/issues/428
 template <typename S>
 void test_distance_box_box_regression5() {
+  SCOPED_TRACE("test_distance_box_box_regression5");
   const Vector3<S> box1_size(0.2, 0.33, 0.1);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   X_WB1.translation() << -0.071000000000000035305, -0.77200000000000001954, 0.79999999999999993339;
@@ -440,6 +455,7 @@ void test_distance_box_box_regression5() {
 
 template <typename S>
 void test_distance_box_box_regression6() {
+  SCOPED_TRACE("test_distance_box_box_regression6");
   const Vector3<S> box1_size(0.31650000000000000355, 0.22759999999999999676, 0.1768000000000000127);
   Transform3<S> X_WB1 = Transform3<S>::Identity();
   // clang-format off
@@ -460,11 +476,64 @@ void test_distance_box_box_regression6() {
   test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
 }
 
+// Issue #493 outlines a number of scenarios that caused signed distance
+// failure. They consisted of two identical, stacked boxes. The boxes are
+// slightly tilted. The boxes were essentially touching but were separated by
+// infinitesimally small distances. The issue outlines three different examples.
+// Rather than reproducing each of them verbatim, this test attempts to
+// generalize those cases by testing the stacked scenario across various box
+// sizes and separation amounts (ranging from slightly penetrating to slightly
+// separated). These should essentially cover the variations described in the
+// issue.
+template <typename S>
+void test_distance_box_box_regression_tilted_kissing_contact() {
+  SCOPED_TRACE("test_distance_box_box_regression_tilted_kissing_contact");
+  // The boxes are posed relative to each other in a common frame F (such that
+  // it is easy to reason about their separation). The stack is rotated around
+  // box A's origin and translated into the world frame.
+  Matrix3<S> R_WF;
+  R_WF <<
+       0.94096063217417758029, 0.29296840037289501035, 0.16959541586174811667,
+      -0.23569836841299879326, 0.92661523595848427348, -0.29296840037289506586,
+      -0.2429801799032638987, 0.23569836841299884878, 0.94096063217417758029;
+
+  for (const S dim : {S(0.01), S(0.25), S(0.5), S(10), S(1000)}) {
+    const Vector3<S> box_size(dim, dim, dim);
+
+    const Vector3<S> p_WA(0, 0, 5 * dim);
+    Transform3<S> X_WA;
+    X_WA.linear() = R_WF;
+    X_WA.translation() = p_WA;
+    Transform3<S> X_WB;
+    X_WB.linear() = R_WF;
+
+    // Both boxes always have the same orientation and the *stack* is always
+    // located at p_WA. Only the translational component of X_WB changes with
+    // varying separation distance.
+
+    // By design, the distances are all near epsilon. We'll scale them up for
+    // larger boxes to make sure the distance doesn't simply disappear in
+    // the rounding noise.
+    for (const S distance : {S(-1e-15), S(-2.5e-16), S(-1e-16), S(0), S(1e-16),
+                             S(2.5e-16), S(1e-15)}) {
+      const S scaled_distance = distance * std::max(S(1), dim);
+      const Vector3<S> p_AB_F = Vector3<S>(0, dim + scaled_distance, 0);
+
+      X_WB.translation() = p_WA + R_WF * p_AB_F;
+      SCOPED_TRACE("dim: " + std::to_string(dim) +
+                   ", distance: " + std::to_string(distance));
+      test_distance_box_box_helper(box_size, X_WA, box_size, X_WB,
+                                   &scaled_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
 template <typename S>
 void test_distance_sphere_box_regression1() {
+  SCOPED_TRACE("test_distance_sphere_box_regression1");
   using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
   const S sphere_radius = 0.06;
   CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
@@ -533,6 +602,7 @@ 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_regression_tilted_kissing_contact<double>();
   test_distance_sphere_box_regression1<double>();
 }