EPA error when the triangle is degenerate

[hongkai-dai]

This error was caught in the wild

C++ exception with description "Error with configuration
  Original error message: external/fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(969): faceNormalPointingOutward(): Cannot compute face normal for a degenerate (zero-area) triangle
  Shape 1: Box 0.46000000000000001998  0.47999999999999998224 0.010000000000000000208
  X_FS1
                      1                       0                       0 -0.72099999999999997424
                      0                       1                       0 -0.77200000000000001954
                      0                       0                       1  0.81000000000000005329
                      0                       0                       0                       1
  Shape 2: Box0.049521000000000002517   0.1459999999999999909 0.072499999999999995004
  X_FS2
 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
  Solver: GjkSolver_libccd
    collision_tolerance:      2.0097183471152322134e-14
    max collision iterations: 500
    distance tolerance:       9.9999999999999995475e-07
    max distance iterations:  1000" thrown in the test body.

[hongkai-dai]

The problem is that while expanding the polytope in EPA algorithm, we had three vertices of the polytope, that are colinear, hence the triangle is degenerate. The next time when when want to connect the new vertex to the edges of this degenerate triangle, we cannot meaningfully determine an outward normal of the triangle.

The co-linearity occurs due to the support function. Imagine that we have a polytope (as the Minkowski difference A ⊖ B). For most cases the vertices are the unique maximizer of argmin nᵀv, and the support function should return the vertices as the support. But when the direction vector n is perpendicular to a face (or an edge), all the vertices of that feature (face/edge) are the maximizer. The support function returns the middle of the face/edge as the support point. So it is not hard to imagine that for the box case, after several iterations of EPA, with different support directions in each iteration, we end up with three vertices on the same face of the box, out of which two being the opposing vertices, and one being the center of the box face. That gives us three colinear vertices, and hence degenerate triangle.

I think one solution is to detect such degeneracy in the EPA code. We could

1. Every time when we connect the new vertex to the existing edges, we detect if the new vertex is colinear with that edge. If it is, and say that the edge is A -- B -- C, with B being the middle vertex on the line. We remove that vertex B from the polytope, together with the edges and faces that connect with B. This would guarantee that we never have any degenerated triangle in the expanded polytope.
2. An alternative is to keep the degenerated triangle in the expanded polytope. We connect the new vertex to the three vertices of the degenerate triangle. We then move to the neighbouring triangles from this degenerate triangle, and then ask if these neighbouring triangles are visible. The argument is that this degenerate triangles has to be visible from the new vertex (if one of its edge is visible, then all the edges must be visible), hence we can always safely connect the new vertex to the degenerate triangle, and move forward to the neighboring ones.

Approach 2 is much easier than approach 1. But I think approach 1 is a cleaner solution. I think it is easier to think of non-degenerate triangles, and keep nondegeneracy as an invariant property during EPA expansion.

cc @sherm1 , @SeanCurtis-TRI and @DamrongGuoy for discussion.

[sherm1]

From my ignorant point of view, keeping the nondegeneracy invariant enforced seems much nicer.