Distance computed in EPA incorrectly uses an un-normalized vector

[hongkai-dai]

In EPA algorithm, as outlined in section 2.5 of "Real-time collision detection with implicit objects" by Leif Olvang, in each iteration, we do the following

1. Find the closest point on the polytope, denote the point as vᵢ
2. Compute the support of the Minkowski sum A⊖B along the direction as wᵢ = S(vᵢ).
3. Compute the upper bound of the distance as μᵢ ← min(μᵢ, wᵢᵀvᵢ/|vᵢ|)
4. If the difference between the upper bound μᵢ and the lower bound |vᵢ| is sufficiently small, terminates the algorithm.

Notice that in step 3, the vector vᵢ needs to be normalized before multiplying with wᵢ. On the other hand, in FCL's implementation, it uses the unnormalized vector vᵢ (namely variable el->witness)

There are other issues in this piece of code that confuses me.

1. In https://github.com/flexible-collision-library/fcl/blob/master/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L882~L883, the difference between dist and el->dist is less than EPA tolerance, why the code returns -1? As far as I understand, we return 0 on success, and -1 otherwise. This seems to me a successful iteration.
2. https://github.com/flexible-collision-library/fcl/blob/master/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L871~L873, the code says that if the nearest point on the polytope the origin, then it is a touch contact. I do not think this is right. It is a touch contact if origin is the nearest point on the Minkowski sum, but it says nothing if origin is the nearest point on the polytope, we just need to continue expanding the polytope until the difference between the distance upperbound μᵢ and lowerbound |vᵢ| is sufficiently small.

@SeanCurtis-TRI @sherm1

[amcastro-tri]

Was this issue resolved with #305 @hongkai-dai?

[hongkai-dai]

Resolved by #305