Add a guard to faceNormalPointingOutward to detect degeneracy

This adds the `triangle_area_is_zero` test to `faceNormalPointingOutward`.
This is a prerequisite to the `isOutsidePolytopeFace` test. The test is
rendered meaningless if the reported normal is defined by numerical noise.
If a scenario is failing due to the `isOutsidePolytopFace` assertion
failing, this will detect if its due to face degeneracies.

Author: SeanCurtis-TRI

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

@@ -765,6 +765,64 @@ static int simplexToPolytope4(const void *obj1, const void *obj2,
     return 0;
 }
 
+/** Reports true if p and q are coincident. */
+static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
+  // This uses a scale-dependent basis for determining coincidence. It examines
+  // each axis independently, and only, if all three axes are sufficiently
+  // close (relative to its own scale), are the two points considered
+  // coincident.
+  //
+  // For dimension i, two values are considered the same if:
+  //   |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |pᵢ|)
+  // And the points are coincident if the previous condition for all
+  // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions).
+  using std::abs;
+  using std::max;
+
+  const 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) {
+    const ccd_real_t scale =
+        max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
+    const ccd_real_t delta = abs(p.v[i] - q.v[i]);
+    if (delta > scale) return false;
+  }
+  return true;
+}
+
+/** Determines if the the triangle defined by the three vertices has zero area.
+ Area can be zero for one of two reasons:
+   - the triangle is so small that the vertices are functionally coincident, or
+   - the vertices are co-linear.
+ Both conditions are computed with respect to machine precision.
+ @returns true if the area is zero.  */
+static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
+                                  const ccd_vec3_t& c) {
+  // First coincidence condition. This doesn't *explicitly* test for b and c
+  // being coincident. That will be captured in the subsequent co-linearity
+  // test. If b and c *were* coincident, it would be cheaper to perform the
+  // coincidence test than the co-linearity test.
+  // However, the expectation is that typically the triangle will not have zero
+  // area. In that case, we want to minimize the number of tests performed on
+  // the average, so we prefer to eliminate one coincidence test.
+  if (are_coincident(a, b) || are_coincident(a, c)) return true;
+
+  // We're going to compute the *sine* of the angle θ between edges (given that
+  // the vertices are *not* coincident). If the sin(θ) < ε, the edges are
+  // co-linear.
+  ccd_vec3_t AB, AC, n;
+  ccdVec3Sub2(&AB, &b, &a);
+  ccdVec3Sub2(&AC, &c, &a);
+  ccdVec3Normalize(&AB);
+  ccdVec3Normalize(&AC);
+  ccdVec3Cross(&n, &AB, &AC);
+  const ccd_real_t eps = constants<ccd_real_t>::eps();
+  // Second co-linearity condition.
+  if (ccdVec3Len2(&n) < eps * eps) return true;
+  return false;
+}
+
 /**
  * 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
@@ -778,6 +836,17 @@ static int simplexToPolytope4(const void *obj1, const void *obj2,
  */
 static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
                                             const ccd_pt_face_t* face) {
+  // This doesn't necessarily define a triangle; I don't know that the third
+  // vertex added here is unique from the other two.
+#ifndef NDEBUG
+  // quick test for degeneracy
+  const ccd_vec3_t& a = face->edge[0]->vertex[1]->v.v;
+  const ccd_vec3_t& b = face->edge[0]->vertex[0]->v.v;
+  const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v;
+  const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ?
+                        face->edge[1]->vertex[1]->v.v : test_v;
+  assert(!triangle_area_is_zero(a, b, c));
+#endif
   // We find two edges of the triangle as e1 and e2, and the normal vector
   // of the face is e1.cross(e2).
   ccd_vec3_t e1, e2;
@@ -1363,65 +1432,6 @@ static int __ccdEPA(const void *obj1, const void *obj2,
     return 0;
 }
 
-
-/** Reports true if p and q are coincident. */
-static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
-  // This uses a scale-dependent basis for determining coincidence. It examines
-  // each axis independently, and only, if all three axes are sufficiently
-  // close (relative to its own scale), are the two points considered
-  // coincident.
-  //
-  // For dimension i, two values are considered the same if:
-  //   |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |pᵢ|)
-  // And the points are coincident if the previous condition for all
-  // `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions).
-  using std::abs;
-  using std::max;
-  
-  const 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) {
-    const ccd_real_t scale =
-        max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
-    const ccd_real_t delta = abs(p.v[i] - q.v[i]);
-    if (delta > scale) return false;
-  }
-  return true;
-}
-
-/** Determines if the the triangle defined by the three vertices has zero area.
- Area can be zero for one of two reasons:
-   - the triangle is so small that the vertices are functionally coincident, or
-   - the vertices are co-linear.
- Both conditions are computed with respect to machine precision.
- @returns true if the area is zero.  */
-static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
-                                  const ccd_vec3_t& c) {
-  // First coincidence condition. This doesn't *explicitly* test for b and c
-  // being coincident. That will be captured in the subsequent co-linearity
-  // test. If b and c *were* coincident, it would be cheaper to perform the
-  // coincidence test than the co-linearity test.
-  // However, the expectation is that typically the triangle will not have zero
-  // area. In that case, we want to minimize the number of tests performed on
-  // the average, so we prefer to eliminate one coincidence test.
-  if (are_coincident(a, b) || are_coincident(a, c)) return true;
-
-  // We're going to compute the *sine* of the angle θ between edges (given that
-  // the vertices are *not* coincident). If the sin(θ) < ε, the edges are
-  // co-linear.
-  ccd_vec3_t AB, AC, n;
-  ccdVec3Sub2(&AB, &b, &a);
-  ccdVec3Sub2(&AC, &c, &a);
-  ccdVec3Normalize(&AB);
-  ccdVec3Normalize(&AC);
-  ccdVec3Cross(&n, &AB, &AC);
-  const ccd_real_t eps = constants<ccd_real_t>::eps();
-  // Second co-linearity condition.
-  if (ccdVec3Len2(&n) < eps * eps) return true;
-  return false;
-}
-
 /** Given a single support point, `q`, extract the point `p1` and `p2`, the
  points on object 1 and 2, respectively, in the support data of `q`.  */
 static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1,

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp

@@ -293,6 +293,138 @@ GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) {
   CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside);
 }
 
+#ifndef NDEBUG
+
+/** A degenerate tetrahedron due to vertices considered to be coincident.
+ It is, strictly speaking a valid tetrahedron, but the points are so close that
+ the calculations on edge lengths cannot be trusted.
+
+ More particularly, one face is *very* small but the other three faces are quite
+ long with horrible aspect ratio.
+
+ Vertices v0, v1, and v2 are all close to each other, v3 is distant.
+ Edges e0, e1, and e2 connect vertices (v0, v1, and v2) and, as such, have very
+ short length. Edges e3, e4, and e5 connect to the distance vertex and have
+ long length.
+
+ Face 0 is the small face. Faces 1-3 all include one edge of the small face.
+
+ All faces should be considered degenerate due to coincident points. */
+class CoincidentTetrahedron : public Polytope {
+ public:
+  CoincidentTetrahedron() : Polytope() {
+    const ccd_real_t delta = constants<ccd_real_t>::eps() / 4;
+    v().resize(4);
+    e().resize(6);
+    f().resize(4);
+    auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y,
+                                       ccd_real_t z) {
+      return ccdPtAddVertexCoords(&this->polytope(), x, y, z);
+    };
+    v()[0] = AddTetrahedronVertex(0.5, delta, delta);
+    v()[1] = AddTetrahedronVertex(0.5, -delta, delta);
+    v()[2] = AddTetrahedronVertex(0.5, -delta, -delta);
+    v()[3] = AddTetrahedronVertex(-0.5, 0, 0);
+    e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
+    e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
+    e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
+    e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
+    e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
+    e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
+    f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
+    f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
+    f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
+    f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
+  }
+};
+
+// Tests against a polytope with a face where all the points are too close to
+// distinguish.
+GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident0) {
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+  CoincidentTetrahedron p;
+
+  // The test point doesn't matter; it'll never get that far.
+  ccd_vec3_t pt{{10, 10, 10}};
+  ASSERT_DEATH(
+      libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), &pt),
+      ".*Assertion `!triangle_area_is_zero.*");
+}
+
+// Tests against a polytope with a face where *two* points are too close to
+// distinguish.
+GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident1) {
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+  CoincidentTetrahedron p;
+
+  // The test point doesn't matter; it'll never get that far.
+  ccd_vec3_t pt{{10, 10, 10}};
+  ASSERT_DEATH(
+      libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt),
+      ".*Assertion `!triangle_area_is_zero.*");
+}
+
+/** A degenerate tetrahedron due to vertices considered to be colinear.
+ It is, strictly speaking a valid tetrahedron, but the vertices are so close to
+ being colinear, that the area can't meaningfully be computed.
+
+ More particularly, one face is *very* large but the fourth vertex lies just
+ slightly off that plane *over* one of the edges. The face that is incident to
+ that edge and vertex will have colinear edges.
+
+ Vertices v0, v1, and v2 are form the large triangle. v3 is the slightly
+ off-plane vertex. Edges e0, e1, and e2 connect vertices (v0, v1, and v2). v3
+ projects onto edge e0. Edges e3 and e4 connect v0 and v1 to v3, respectively.
+ Edges e3 and e4 are colinear. Edge e5 is the remaining, uninteresting edge.
+ Face 0 is the large triangle.
+ Face 1 is the bad face.  Faces 2 and 3 are irrelevant. */
+class ColinearTetrahedron : public Polytope {
+ public:
+  ColinearTetrahedron() : Polytope() {
+    const ccd_real_t delta = constants<ccd_real_t>::eps() / 100;
+    v().resize(4);
+    e().resize(6);
+    f().resize(4);
+    auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y,
+                                       ccd_real_t z) {
+      return ccdPtAddVertexCoords(&this->polytope(), x, y, z);
+    };
+    v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), -1);
+    v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), -1);
+    v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), -1);
+    // This point should lie *slightly* above the edge connecting v0 and v1.
+    v()[3] = AddTetrahedronVertex(0, -0.5 / std::sqrt(3), -1 + delta);
+
+    e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
+    e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
+    e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
+    e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
+    e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
+    e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
+    f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
+    f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
+    f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
+    f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
+  }
+};
+
+// Tests against a polytope with a face where two edges are colinear.
+GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Colinear) {
+  ::testing::FLAGS_gtest_death_test_style = "threadsafe";
+  ColinearTetrahedron p;
+
+  // This test point should pass w.r.t. the big face.
+  ccd_vec3_t pt{{0, 0, -10}};
+  EXPECT_TRUE(libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0),
+                                                      &pt));
+  // Face 1, however, is definitely colinear.
+  ASSERT_DEATH(
+      libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt),
+      ".*Assertion `!triangle_area_is_zero.*");
+}
+#endif
+
+
 // Construct a polytope with the following shape, namely an equilateral triangle
 // on the top, and an equilateral triangle of the same size, but rotate by 60
 // degrees on the bottom. We will then connect the vertices of the equilateral