Resolves the error in EPA when the query point is colinear with an ed…

…ge (#417)

When the query point is colinear with an edge, treat this edge as an internal edge, and continue to expand the boundary.

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

@@ -1160,7 +1160,6 @@ static bool ComputeVisiblePatchRecursiveSanityCheck(
   return true;
 }
 #endif
-
 /**
  * This function contains the implementation detail of ComputeVisiblePatch()
  * function. It should not be called by any function other than
@@ -1187,19 +1186,40 @@ static void ComputeVisiblePatchRecursive(
     if (!isOutsidePolytopeFace(&polytope, g, &query_point)) {
       // Cannot see the neighbouring face from the new vertex.
 
-      // TODO([email protected]): when the new vertex is colinear with a
-      // border edge, we should remove the degenerate triangle. We could remove
-      // the middle vertex on that line from the polytope, and then reconnect
-      // the polytope.
-      if (triangle_area_is_zero(query_point, f.edge[edge_index]->vertex[0]->v.v,
-                                f.edge[edge_index]->vertex[1]->v.v)) {
-        FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
-            "The new vertex is colinear with an existing edge. The added "
-            "triangle would be degenerate.");
+      if (!triangle_area_is_zero(query_point,
+                                 f.edge[edge_index]->vertex[0]->v.v,
+                                 f.edge[edge_index]->vertex[1]->v.v)) {
+        // If query point is outside of the face g, and the triangle
+        // (query_point, v[0], v[1]) has non-zero area, then the edge
+        // f.edge[edge_index] is a border edge, and we will connect the query
+        // point with this border edge to form a triangle. Otherwise, this edge
+        // is not a border edge.
+        border_edges->insert(f.edge[edge_index]);
+        return;
       }
-      border_edges->insert(f.edge[edge_index]);
-      return;
     }
+    // We regard the edge f.edge[edge_index] not as an internal edge (not a
+    // border edge), if it satisfies one of the following two conditions
+    // 1. The face g is visible to the query point.
+    // 2. The triangle formed by the edge and the query point has zero area.
+    // The first condition is obvious. Here we explain the second condition:
+    // For this triangle to have no area, the query point must be co-linear with
+    // a candidate border edge. That means it is simultaneously co-planar with
+    // the two faces adjacent to that edge. But to be in this branch, one face
+    // was considered to be visible and the other to not be visible -- an
+    // inconsistent classification.
+
+    // The solution is to unify that classification. We can consider both
+    // faces as being visible or not. If we were to consider them not
+    // visible, we would shrink the size of the visible patch (making this
+    // slightly faster), but would risk introducing co-planar faces into the
+    // polytope. We choose to consider both faces as being visible. At the
+    // cost of a patch boundary with more edges, we guarantee that we don't
+    // add co-planar faces.
+
+    // It may be that co-planar faces are permissible and a smaller
+    // patch is preferred. This is still an open problem.For now, we go with
+    // the "safe" choice.
     visible_faces->insert(g);
     internal_edges->insert(f.edge[edge_index]);
     for (int i = 0; i < 3; ++i) {

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

@@ -690,6 +690,58 @@ GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatch_2FacesVisible) {
                            {0, 1}, {0});
 }
 
+/*
+ * Given an equilateral tetrahedron, create a query point that is co-linear with
+ * edge 0 as q = v₀ + ρ(v₀ - v₁), confirms that the correct tetrahedra faces are
+ * included in the visible patch. Point q is co-linear with edge 0 which is
+ * adjacent to faces f0 and f1. Face f3 is trivially visible from q.
+ *
+ * If the query point is co-linear with a tet edge, then both adjacent faces
+ * should be visible. The behavior is sensitive to numerical accuracy issues and
+ * we expose rho (ρ) as a parameter so that different scenarios can easily be
+ * authored which exercise different code paths to determine visibility. (In the
+ * code, "visibility" may be determined by multiple tests.)
+ */
+void CheckComputeVisiblePatchColinearNewVertex(EquilateralTetrahedron& tet,
+                                               double rho) {
+  // A new vertex is colinear with an edge e[0] of the tetrahedron. The border
+  // edges should be e[1], e[4], e[5]. The visible faces should be f[0], f[1],
+  // f[3], and the internal edges should be e[0], e[2], e[3].
+  // For the numbering of the edges/vertices/faces in the equilateral
+  // tetrahedron, please refer to the documentation of EquilateralTetrahedron.
+  ccd_vec3_t query_point;
+  for (int i = 0; i < 3; ++i) {
+    query_point.v[i] = (1 + rho) * tet.v(0).v.v.v[i] - rho * tet.v(1).v.v.v[i];
+  }
+  std::unordered_set<ccd_pt_edge_t*> border_edges;
+  std::unordered_set<ccd_pt_face_t*> visible_faces;
+  std::unordered_set<ccd_pt_edge_t*> internal_edges;
+  libccd_extension::ComputeVisiblePatch(tet.polytope(), tet.f(3), query_point,
+                                        &border_edges, &visible_faces,
+                                        &internal_edges);
+
+  EXPECT_EQ(border_edges.size(), 3u);
+  EXPECT_EQ(border_edges, std::unordered_set<ccd_pt_edge_t*>(
+                              {&(tet.e(1)), &(tet.e(4)), &(tet.e(5))}));
+  EXPECT_EQ(visible_faces.size(), 3u);
+  EXPECT_EQ(visible_faces, std::unordered_set<ccd_pt_face_t*>(
+                               {&(tet.f(0)), &(tet.f(1)), &(tet.f(3))}));
+  EXPECT_EQ(internal_edges.size(), 3u);
+  EXPECT_EQ(internal_edges, std::unordered_set<ccd_pt_edge_t*>(
+                                {&(tet.e(0)), &(tet.e(2)), &(tet.e(3))}));
+}
+
+GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchColinearNewVertex) {
+  // Case 1: Visibility of faces f0 and f1 is not immediately apparent --
+  // requires recognition that q, v0, and v1 are colinear.
+  EquilateralTetrahedron tet1(-0.05, -0.13, 0.12);
+  CheckComputeVisiblePatchColinearNewVertex(tet1, 1.9);
+  // Case 2: Visibility of faces f0 and f1 are independently confirmed --
+  // colinearity doesn't matter.
+  EquilateralTetrahedron tet2(0.1, 0.2, 0.3);
+  CheckComputeVisiblePatchColinearNewVertex(tet2, 0.3);
+}
+
 // Tests that the sanity check causes `ComputeVisiblePatch()` to throw in
 // debug builds.
 GTEST_TEST(FCL_GJK_EPA, ComputeVisiblePatchSanityCheck) {

test/test_fcl_signed_distance.cpp

@@ -335,7 +335,7 @@ void test_distance_box_box_helper(const Vector3<S>& box1_size,
 // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
 // reported in https://github.com/flexible-collision-library/fcl/issues/388
 template <typename S>
-void test_distance_box_box1() {
+void 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,
@@ -360,7 +360,7 @@ void test_distance_box_box1() {
 // unexpected `triangle_size_is_zero` error. This error was
 // reported in https://github.com/flexible-collision-library/fcl/issues/395
 template <typename S>
-void test_distance_box_box2() {
+void 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,
@@ -379,40 +379,63 @@ void test_distance_box_box2() {
   test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
 }
 
+// This is a *specific* case that has cropped up in the wild that reaches the
+// unexpected `query point colinear with the edge` error. This error was
+// reported in https://github.com/flexible-collision-library/fcl/issues/415
+template <typename S>
+void 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
+  X_WB1.matrix() << 4.8966386501092529215e-12, -1,0,-0.43999999999999994671,
+                       1, 4.8966386501092529215e-12,0,-0.61499999999858001587,
+                       0,0,1,0.35499999999999998224,
+                       0,0,0,1;
+  // clang-format on
+  const Vector3<S> box2_size(0.035, 0.12, 0.03);
+  Transform3<S> X_WB2 = Transform3<S>::Identity();
+  // clang-format off
+  X_WB2.matrix() << 0.83512153565236335595,    -0.55006546945762568868, -9.4542360608233572896e-16,    -0.40653441507331000704,
+   0.55006546945762568868,     0.83512153565236313391,  1.1787444236552387666e-15,    -0.69166166923735727945,
+1.2902271444330665572e-16, -1.4878153530113264589e-15,                          1,     0.43057093858718892276,
+                        0,                          0,                          0,                          1;
+  // clang-format on
+  test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2);
+
+}
+
 //==============================================================================
 
-GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd)
-{
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_ccd) {
   test_distance_spheresphere<double>(GST_LIBCCD);
 }
 
-GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep)
-{
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_sphere_indep) {
   test_distance_spheresphere<double>(GST_INDEP);
 }
 
-GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd)
-{
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_ccd) {
   test_distance_spherecapsule<double>(GST_LIBCCD);
 }
 
-GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep)
-{
+GTEST_TEST(FCL_NEGATIVE_DISTANCE, sphere_capsule_indep) {
   test_distance_spherecapsule<double>(GST_INDEP);
 }
 
-GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd)
-{
+GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_sphere1_ccd) {
   test_distance_cylinder_sphere1<double>();
 }
 
 GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) {
   test_distance_cylinder_box<double>();
 }
 
-GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) {
-  test_distance_box_box1<double>();
-  test_distance_box_box2<double>();
+GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) {
+  // A collection of scnarios observed in practice that have created error
+  // conditions in previous commits of the code. Each test is a unique instance.
+  test_distance_box_box_regression1<double>();
+  test_distance_box_box_regression2<double>();
+  test_distance_box_box_regression3<double>();
 }
 
 //==============================================================================