Fix epa box support (#397)

Make sure that the support point of a box is always one of the box vertices, not the middle of an edge or a face. This helps to reduce the "degenerate triangle issue" reported in #395

Author: hongkai-dai

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

@@ -1186,6 +1186,17 @@ static void ComputeVisiblePatchRecursive(
     // g is not a visible face
     if (!isOutsidePolytopeFace(&polytope, g, &query_point)) {
       // Cannot see the neighbouring face from the new vertex.
+
+      // TODO(hongkai.dai@tri.global): 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.");
+      }
       border_edges->insert(f.edge[edge_index]);
       return;
     }
@@ -2241,13 +2252,21 @@ static void convexToGJK(const Convex<S>& s, const Transform3<S>& tf,
 static inline void supportBox(const void* obj, const ccd_vec3_t* dir_,
                               ccd_vec3_t* v)
 {
+  // Use a customized sign function, so that the support of the box always
+  // appears in one of the box vertices.
+  // Picking support vertices on the interior of faces/edges can lead to
+  // degenerate triangles in the EPA algorithm and are no more correct than just
+  // picking box corners.
+  auto sign = [](ccd_real_t x) -> ccd_real_t {
+    return x >= 0 ? ccd_real_t(1.0) : ccd_real_t(-1.0);
+  };
   const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
   ccd_vec3_t dir;
   ccdVec3Copy(&dir, dir_);
   ccdQuatRotVec(&dir, &o->rot_inv);
-  ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
-             ccdSign(ccdVec3Y(&dir)) * o->dim[1],
-             ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
+  ccdVec3Set(v, sign(ccdVec3X(&dir)) * o->dim[0],
+             sign(ccdVec3Y(&dir)) * o->dim[1],
+             sign(ccdVec3Z(&dir)) * o->dim[2]);
   ccdQuatRotVec(v, &o->rot);
   ccdVec3Add(v, &o->pos);
 }

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

@@ -1289,7 +1289,7 @@ void SetUpBoxToBox(const Transform3<S>& X_WF, void** o1, void** o2, ccd_t* ccd,
   fcl::Box<S> box1(box1_size);
   fcl::Box<S> box2(box2_size);
   *o1 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB1);
-  *o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB2);
+  *o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box2, X_WB2);
 
   // Set up ccd solver.
   CCD_INIT(ccd);
@@ -1329,20 +1329,20 @@ void TestSimplexToPolytope3InGivenFrame(const Transform3<S>& X_WF) {
   // We find three points Pa1, Pb1, Pc1 on box 1, and three points Pa2, Pb2, Pc2
   // on box 2, such that the 2-simplex with vertices (Pa1 - Pa2, Pb1 - Pb2,
   // Pc1 - Pc2) contains the origin.
-  const Vector3<S> p_FPa1(-1, -1, -1);
-  const Vector3<S> p_FPa2(-0.1, 0.5, -1);
+  const Vector3<S> p_FPa1(-1, -1, 0.1);
+  const Vector3<S> p_FPa2(-0.1, 0.5, 0.1);
   pts[0].v = ToCcdVec3<S>(p_FPa1 - p_FPa2);
   pts[0].v1 = ToCcdVec3<S>(p_FPa1);
   pts[0].v2 = ToCcdVec3<S>(p_FPa2);
 
-  const Vector3<S> p_FPb1(-1, 1, -1);
-  const Vector3<S> p_FPb2(-0.1, 0.5, -1);
+  const Vector3<S> p_FPb1(-1, 1, 0.1);
+  const Vector3<S> p_FPb2(-0.1, 0.5, 0.1);
   pts[1].v = ToCcdVec3<S>(p_FPb1 - p_FPb2);
   pts[1].v1 = ToCcdVec3<S>(p_FPb1);
   pts[1].v2 = ToCcdVec3<S>(p_FPb2);
 
-  const Vector3<S> p_FPc1(1, 1, -1);
-  const Vector3<S> p_FPc2(-0.1, 0.5, -1);
+  const Vector3<S> p_FPc1(1, 1, 0.1);
+  const Vector3<S> p_FPc2(-0.1, 0.5, 0.1);
   pts[2].v = ToCcdVec3<S>(p_FPc1 - p_FPc2);
   pts[2].v1 = ToCcdVec3<S>(p_FPc1);
   pts[2].v2 = ToCcdVec3<S>(p_FPc2);

test/test_fcl_capsule_box_1.cpp

@@ -117,7 +117,7 @@ void test_distance_capsule_box(fcl::GJKSolverType solver_type, S solver_toleranc
 
 GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
 {
-  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-6, 1e-4);
+  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-8, 4e-4);
 }
 
 GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_indep)

test/test_fcl_signed_distance.cpp

@@ -296,34 +296,18 @@ void test_distance_cylinder_box() {
                                     box_size, X_WB);
 }
 
-// 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/388
 template <typename S>
-void test_distance_box_box1() {
+void test_distance_box_box_helper(const Vector3<S>& box1_size,
+                                  const Transform3<S>& X_WB1,
+                                  const Vector3<S>& box2_size,
+                                  const Transform3<S>& X_WB2) {
   using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometryd>;
-  const Vector3<S> box1_size(0.03, 0.12, 0.1);
   CollisionGeometryPtr_t box1_geo(
       new fcl::Box<S>(box1_size(0), box1_size(1), box1_size(2)));
-  Transform3<S> X_WB1 = Transform3<S>::Identity();
-  X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
-      -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
-      -3.0627939739957803544e-08, 6.4729926918527511769e-08,
-      -0.48500002215636439651, -6.4729927722963847085e-08,
-      -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
-      0, 0, 0, 1;
   fcl::CollisionObject<S> box1(box1_geo, X_WB1);
 
-  const Vector3<S> box2_size(0.025, 0.35, 1.845);
   CollisionGeometryPtr_t box2_geo(
       new fcl::Box<S>(box2_size(0), box2_size(1), box2_size(2)));
-  Transform3<S> X_WB2 = Transform3<S>::Identity();
-  // clang-format off
-  X_WB2.matrix() << 0, -1, 0, 0.8,
-                    1, 0, 0, -0.4575,
-                    0, 0, 1, 1.0225, 
-                    0, 0, 0, 1;
-  // clang-format on
   fcl::CollisionObject<S> box2(box2_geo, X_WB2);
 
   fcl::DistanceRequest<S> request;
@@ -347,6 +331,54 @@ void test_distance_box_box1() {
   EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all());
 }
 
+// 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/388
+template <typename S>
+void test_distance_box_box1() {
+  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,
+      -2.8893865161583314238e-08, 0.63499979627350811029, 0.9999999999999980016,
+      -3.0627939739957803544e-08, 6.4729926918527511769e-08,
+      -0.48500002215636439651, -6.4729927722963847085e-08,
+      -2.8893863029448751323e-08, 0.99999999999999711342, 1.0778146458339641356,
+      0, 0, 0, 1;
+
+  const Vector3<S> box2_size(0.025, 0.35, 1.845);
+  Transform3<S> X_WB2 = Transform3<S>::Identity();
+  // clang-format off
+  X_WB2.matrix() << 0, -1, 0, 0.8,
+                    1, 0, 0, -0.4575,
+                    0, 0, 1, 1.0225, 
+                    0, 0, 0, 1;
+  // clang-format on
+  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 `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() {
+  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,
+                      0,1,0, -0.77200000000000001954,
+                      0,0,1,  0.81000000000000005329,
+                      0,0,0,1;
+
+  const Vector3<S> box2_size(0.049521, 0.146, 0.0725);
+  Transform3<S> X_WB2 = Transform3<S>::Identity();
+  // clang-format off
+  X_WB2.matrix() << 0.10758262492983036718,  -0.6624881850015212903, -0.74130653817877356637, -0.42677133002999478872,
+ 0.22682184885125472595,   -0.709614040775253474,   0.6670830248314786326, -0.76596851247746788882,
+-0.96797615037608542021, -0.23991106241273435495,  0.07392465377049164954,  0.80746731400091054098,
+                      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)
@@ -380,6 +412,7 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, cylinder_box_ccd) {
 
 GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) {
   test_distance_box_box1<double>();
+  test_distance_box_box2<double>();
 }
 
 //==============================================================================