[GJK-EPA] Another real-world signed distance failure

[SeanCurtis-TRI]

Here is a configuration reported from the real world.

Terminate called after throwing an instance of 'std::logic_error'
  what():  Error with configuration
  Original error message: external/fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:(1701): validateNearestFeatureOfPolytopeBeingEdge(): The origin is outside of the polytope. This should already have been identified as separating.
  Shape 1: Box0.31650000000000000355 0.22759999999999999676  0.1768000000000000127
  X_FS1
    0.44540578475530234748     0.89532881496493399442 -8.8937407685638678971e-09      1.2652949075960071568
   -0.89532881496493377238     0.44540578475530190339 -2.8948680226084145336e-08      1.4551012423210101243
-2.1957263975186326105e-08  2.0856732016652919226e-08     0.99999999999999955591     0.49480006232932938204
                         0                          0                          0                          1
  Shape 2: Box 0.49430000000000001714  0.35460000000000002629 0.075200000000000002953
  X_FS2
    0.44171122913485860728      0.8971572827861190591  -1.622764514865468214e-09      1.1304016226141906376
    -0.8971572827861190591     0.44171122913485860728 -5.1621053952306079594e-09      1.8410802645284281009
-3.9144271413829990148e-09  3.7360349218094348098e-09                          1     0.44400006232932492933
                         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

[SeanCurtis-TRI]

Initial analysis shows this is an issue in the hand-off between GJK and EPA.

• GJK has determined that the origin must lie within the Minkowski difference.
• The GJK witness simplex is converted into a polytope.
• The point on the polytope nearest the origin is computed. The near feature is determined to be an edge.
• The initial check on edge nearest feature fails!