nextSupport function does not compute the right answer

[hongkai-dai]

I believe there are problems in nextSupport function. What this function does is a part of the EPA algorithm. The EPA algorithm can be summarized as follows

1. Given two convex objects A and B, and we know that Minkowski difference A⊖B contains the origin, we want to find the smallest distance from the boundary of A⊖B to the origin. We start with an upper bound of the distance μ₀ = +∞, and a polytope whose vertices are on the boundary of A⊖B. This polytope also contains the origin.
2. At iteration i, find the point on the boundary of the polytope that is closest to the origin, denote this point as vᵢ.
3. Find the support point of the Minkowski difference A⊖B, along the direction of vᵢ. We denote this support point as wᵢ.
4. Update the distance upper bound μᵢ₊₁ = min(μᵢ, wᵢᵀvᵢ/|vᵢ|), and the lower bound as |vᵢ|.
5. Terminate until μᵢ - |vᵢ| ≤ ε. Then we are guaranteed that the distance we found is within ε to the true distance.

Given this is what EPA algorithm does, there are a few issues that confuse me in libccd's nextSupport function

1. The "touch contact" is detected in https://github.com/danfis/libccd/blob/master/src/ccd.c#L964~L966, that if el (which is the point on the boundary of the polytope that is nearest to the origin) is the origin. I do not think this is the right assertion. The condition for touch contact should be that the origin is on the boundary of the Minkowski difference A⊖B, not on the boundary of the polytope. The polytope is inside the Minkowski difference. When the origin is on the boundary of the polytope, it only means that the object A and B is in contact (or penetrating). We should continue to expand the polytope, until the difference between the distance upper bound and the lower bound is sufficiently small, instead of returning -1 here.
2. The dist is computed in https://github.com/danfis/libccd/blob/master/src/ccd.c#L973 as

   dist = ccdVec3Dot(&out->v, &el->witness);

   where out->v is wᵢ in the documentation above, and el->witness is vᵢ. I do not think the computation of dist is right. To compute the distance, I think el->witness should be normalized.

3. The termination condition in https://github.com/danfis/libccd/blob/master/src/ccd.c#L975 and https://github.com/danfis/libccd/blob/master/src/ccd.c#L992 have different meanings. In https://github.com/danfis/libccd/blob/master/src/ccd.c#L975, el->dist is the squared distance of the witness, while dist should be a non-squared distance. And even if we use squared distance for both dist and el->dist, I still do not think it is a good termination condition, since μᵢ² - |vᵢ|² ≤ ε says nothing about the difference between the upper bound μᵢ and the lower bound |vᵢ| (because both μᵢ and |vᵢ| can be arbitrarily large or arbitrarily small, thus making the difference μᵢ - |vᵢ| arbitrary.). On the other hand, the termination condition in https://github.com/danfis/libccd/blob/master/src/ccd.c#L992 is reasonable, as it means (μᵢ - |vᵢ|)² ≤ ε

[sherm1]

@hongkai-dai: For point 2 above, the calculated dist ought to be a squared distance like all the other Dist2 calculations there (but of course the variable name shouldn't have been dist if it isn't a distance!). So it can still be calculated without a square root: d²=(v⋅w)²/(w⋅w).

[sherm1]

Oh, I take it back. Combined with point 3 I see you need the actual distance there (and sqrt(el->dist)) to compute a meaningful termination at 975.

[sherm1]

This also makes me wonder whether epa_tolerance is actually a distance squared, which is how it is being used. Looks like it is set to .0001m² by default which means the length tolerance is .01m. Is that intentional @danfis? That seems like a very loose tolerance.

[hongkai-dai]

@sherm1 for point 2, if we want to compute the squared distance, it should be d²=(v⋅w)²/(w⋅w), that is still not what the code dist = ccdVec3Dot(&out->v, &el->witness); does (it computes v⋅w), so the code is still wrong.