extractClosestPoints() from simplex made more robust to degenerate simplex

[SeanCurtis-TRI]

Issue #293 shows a case where the result of the GJK between two separated object is a degenerate simplex. Specifically, a 3-simplex consisting of co-linear vertices. This leads to zero dividing zero which causes NaN to propagate everywhere.

This PR has two curated commits:

1. Added a test to reproduce the scenario described in Nan value in the nearest points when using distance() #293.
2. Correction to the issue. The correction includes:

• Add degeneracy tests to 3-simplex and 2-simplex extraction algorithms and degradation mechanism (i.e., a degenerate triangle is processed as a line segment and a degenerate line segment is processed as a point).
• Add unit tests on extractClosestPoints() and all of the supporting methods
• Updated documentation on the algorithms in place.

Resolves #293
replaces #295

---

This change is 

[SeanCurtis-TRI]

@tongtybj Please take a look at this solution. Sorry for the lag in getting this PR in. Robustly solving the problem took some more rigor.

[SeanCurtis-TRI]

+@sherm1

---

Review status: 0 of 4 files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[SeanCurtis-TRI]

Hold off on review; I've broken CI.

---

Review status: 0 of 4 files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

Comments from Reviewable

[tongtybj]

@SeanCurtis-TRI

Thank you for your kind commit. I carefully read your improvement.
I also tested with other code, it works well without any errors.

LGTM.

[SeanCurtis-TRI]

There was a mac CI bug due to the known issue of linking against a single-precision libccd on mac (#291). This most recent modification should address that issue.

---

Review status: 2 of 4 files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[SeanCurtis-TRI]

@sherm1 CI is all clear. Can you merge for me.

---

Review status: 1 of 4 files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

Comments from Reviewable

[sherm1]

+@hongkai-dai -@sherm1 (per f2f)

---

Review status: 1 of 4 files reviewed, all discussions resolved (waiting on @tongtybj and @sherm1)

---

Comments from Reviewable

[hongkai-dai]

Reviewed 2 of 3 files at r3.
Review status: 3 of 4 files reviewed, 11 unresolved discussions (waiting on @tongtybj and @SeanCurtis-TRI)

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1017 at r3 (raw file):

  //
  // For dimension i, two values are considered the same if:
  //   |pᵢ - qⱼ| <= ε·max(1, |pᵢ|, |pⱼ|)

Should it be qᵢ instead of qⱼ?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1026 at r3 (raw file):

  // NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd
  // is actually float based.
  ccd_real_t scales[3] = {

It might be worthwhile to put the computation of scales into a for loop, and do an early exit of the for loop, when the condition |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|) fails.

for (int i = 0; i < 3; ++i) {
  const ccd_real_t scale = max({ccd_real_t{1.0}, abs(p.v[i]), abs(q.v[i])}) * eps
  const ccd_real_t delta = abs(p.v[i] - q.v[i]);
  if (delta > scale) return false;
}
return true;

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1047 at r3 (raw file):

                                  const ccd_vec3_t& c) {
  // First coincidence condition.
  if (are_coincident(a, b) || are_coincident(a, c)) return true;

Maybe also check are_coincident(b, c) here? If B and C coincide, then it will fail the second test, so it is correct not to check if B and C coincide. But it looks to me checking if B and C coincide is cheaper than checking colinearlity?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1219 at r3 (raw file):

    //  β = |r_Ap × r_AC| / |r_AB × r_AC|
    //  β = |r_Ap × r_AC| / |n|                  n ≙ r_AB × r_AC
    //  β = n̂·(r_Ap × r_AC) / n̂·n                This step arises from the fact

What is n̂? It is better to say that n̂ is the normalized n here.

---

test/test_fcl_distance.cpp, line 313 at r3 (raw file):

template <typename S>
void NearestPointFromDegenerateSimplex() {
  // Tests an historical bug. In certain configurations, the distance query

s/an/a

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 216 at r3 (raw file):

  // The test wants to provide proof as to the boundary of the tolerance
  // envelope. Therefore, it will construct two triangles. One whose smallest
  // angle is θₑ - δ and the other triangle's smallest angle is θₑ - δ, for

One should be θₑ + δ

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 229 at r3 (raw file):

  //
  // We'll do it by construction:
  //   1. Pick a vector perpendicular to [1, 1, 1] (e.g., v = [1, 1, -2]).

Pick a vector v

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 231 at r3 (raw file):

  //   1. Pick a vector perpendicular to [1, 1, 1] (e.g., v = [1, 1, -2]).
  //   2. C' = B + v * |B|·tan(θ). This produces a point C' that:
  //     a) forms a triangle where ∠C`AB < θ, but

I am not sure if this is a correct statement. Whether ∠CAB is less than θ o not depends on the magnitude of v. If v ha unit length, it looks to me ∠CAB = θ; if |v| < 1, then ∠CAB < θ; otherwise ∠CAB > θ

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 254 at r3 (raw file):

  a << 0, 0, 0;
  b << 1, 1, 1;
  Vector3d v = Vector3d(1, 1, -2).normalized();

const Vector3d

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 305 at r3 (raw file):

    const Vector3d& dummy1{-1, -2, -3};
    const Vector3d& dummy2{-2, -3, -4};
    ccd_vec3_t p0 = eigen_to_ccd(dummy1);

Better to rename p0 with p1, and p1 with p2, to match the documentation below.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 558 at r3 (raw file):

  // Now set up co-linear configuration. Vertex 2 will simply be a linear
  // combination of vertex 0 and vertex1. v2 = v0 + 2 * v1.

I do not think this gives a co-linear configuration. For co-linear configuration, it should be v2 = s * v0 + (1-s) * v1, namely the coefficients of v0 and v1 sum up to 1.

---

Comments from Reviewable

[hongkai-dai]

Reviewed 1 of 3 files at r3.
Review status: all files reviewed, 11 unresolved discussions (waiting on @SeanCurtis-TRI)

---

Comments from Reviewable

[SeanCurtis-TRI]

Thanks @hongkai-dai. Comments addressed. PTAL

---

Review status: 1 of 4 files reviewed, 11 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1017 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Should it be qᵢ instead of qⱼ?

Done

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1026 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

It might be worthwhile to put the computation of scales into a for loop, and do an early exit of the for loop, when the condition |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|) fails.

for (int i = 0; i < 3; ++i) {
  const ccd_real_t scale = max({ccd_real_t{1.0}, abs(p.v[i]), abs(q.v[i])}) * eps
  const ccd_real_t delta = abs(p.v[i] - q.v[i]);
  if (delta > scale) return false;
}
return true;

Done

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1047 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Maybe also check are_coincident(b, c) here? If B and C coincide, then it will fail the second test, so it is correct not to check if B and C coincide. But it looks to me checking if B and C coincide is cheaper than checking colinearlity?

I'm going to make a counter argument. It is cheaper when b and c are co-linear. However, if we expect that it's a rare case, we are now paying the extra cost every time.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1219 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

What is n̂? It is better to say that n̂ is the normalized n here.

Added.

---

test/test_fcl_distance.cpp, line 313 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

s/an/a

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 216 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

One should be θₑ + δ

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 229 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Pick a vector v

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 231 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I am not sure if this is a correct statement. Whether ∠CAB is less than θ o not depends on the magnitude of v. If v ha unit length, it looks to me ∠CAB = θ; if |v| < 1, then ∠CAB < θ; otherwise ∠CAB > θ

I believe making it v̂corrects the problem (after all, the code was v̂.)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 254 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

const Vector3d

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 305 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Better to rename p0 with p1, and p1 with p2, to match the documentation below.

Or change the documentation so I can stay zero-indexed. :)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_extractClosestPoints.cpp, line 558 at r3 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I do not think this gives a co-linear configuration. For co-linear configuration, it should be v2 = s * v0 + (1-s) * v1, namely the coefficients of v0 and v1 sum up to 1.

So, I've updated the intent described in the documentation and corrected the math. PTAL.

---

Comments from Reviewable

[hongkai-dai]

---

Reviewed 1 of 4 files at r2, 3 of 3 files at r4.
Review status: 2 of 4 files reviewed, all discussions resolved (waiting on @hongkai-dai)

---

Comments from Reviewable

[SeanCurtis-TRI]

@sherm1 It looks like it's ready for merging. I've double checked the CI failure and it's the known failure.

---

Review status: 2 of 4 files reviewed, all discussions resolved (waiting on @hongkai-dai)

---

Comments from Reviewable

[sherm1]

Wow! Looks great. Awesome tests!

---

Reviewed 1 of 4 files at r2, 1 of 3 files at r4, 2 of 2 files at r5.
Review status: complete! all files reviewed, all discussions resolved

---

Comments from Reviewable