fcl::distance() erroneous penetration depth query when using solvers different than INDEP

[j-rivero]

The issue was originally found by @avalenzu, I just reporting it here so we can track it and help others that find this problem. The original report can be read at RobotLocomotion/drake#7417 (comment), I reproduce part of it here:

Does appear to be a regression due to cd2c6a1. Prior to that commit, the fcl::distance() function would perform a penetration depth query in order to compute a signed distance for intersecting geometries. After that commit (and the subsequent fixes in 5230025) it performs the penetration depth query only when using the INDEP solver. When using the LIBCCD solver it does ... something else. That "something else" returns the erroneous values.

[Levi-Armstrong]

I am not sure if this related but I have been recently upgrading MoveIt's FCL implementation to take advantage of the signed distance but when checking two object types (OT_BVH) it is not returning a signed distance.

[BryceStevenWilley]

@Levi-Armstrong I believe what you're talking about is #221, and seems to only deal with a few specific if statements unrelated to this issue.

[BryceStevenWilley]

As a starting point for anyone who wants to investigate what the "something else" is doing, I've tracked the distance calculation to here, which is based on the nearest edge EPA calculation.

[hongkai-dai]

The EPA implementation using LIBCCD solver is almost completely rewritten, after #305 #296 #290. They are still using LIBCCD data structures, but the implementation is updated, with unit tests added. I believe we can close this issue.

There are still some TODOs left for the EPA implementation. For example, when some simplex/polytope is degenerate, the algorithm might return wrong result. I observer one such failure when two boxes are touching, as reported in the unit test https://github.com/flexible-collision-library/fcl/blob/master/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp#L315~L322. I will address this issue later.