Reworking gjk_libccd doSimplex2()

[SeanCurtis-TRI]

1. Add a ton of documentation about this portion of the whole algorithm.
2. Rephrase the test with a greater understanding and simpler results.
3. Add unit tests

• the old code failed in the following ways:
  • Didn't recognize when A was the origin.
  • Much larger tolerance for determining if A was co-linear with B.

4. Incidentally add some todos to doSimplex3() while examining
   relationships.

Related to #366

The vast majority of the changes are in tests and documentation.

Category            added  modified  removed  
----------------------------------------------
code                190    10        12       
comments            198    1         11       
blank               52     0         0        
----------------------------------------------
TOTAL               440    11        23  

---

This change is 

[SeanCurtis-TRI]

+@DamrongGuoy and +@hongkai-dai for review, please.

Reviewable status: 0 of 3 files reviewed, all discussions resolved (waiting on @DamrongGuoy and @hongkai-dai)

[hongkai-dai]

Reviewed 3 of 3 files at r1, 1 of 1 files at r2.
Reviewable status: all files reviewed, 5 unresolved discussions (waiting on @DamrongGuoy and @SeanCurtis-TRI)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 71 at r1 (raw file):

                                             const ccd_vec3_t& B,
                                             bool validate = true) {
    Vector3Ccd p_OA(A.v);

const Vector3Ccd, same below.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 94 at r1 (raw file):

  // Point A is defined by a linear combination of the direction to B (phat_OB)
  // and its normal (norm_OB) as: A = u·phat_OB + v·norm_OB.
  void ConfigureDeathTest(double u, double v, double dist_OB) {

This is predicated on u being positive. Otherwise A is in region 1. Add an assertion that u > 0?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 170 at r1 (raw file):

//                              // Must be strictly < 0, because we exclude
//                              // co-linearity cases in this test.
//   dir · (p_OA × p_AB) = 0    // Lies on the plane defined b A, B, and O.

defined by.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

}

// Tests the case where the origin does *not* lie on the simplex. The return

Should we also check that dir = -v, where v is the closest point on AB to the origin? Currently we checked that dir is colinear with -v, but not that they are equal.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 177 at r2 (raw file):

    // to recognize a valid direction to machine precision.
    const ccd_real_t eps = std::max({1., p_OA.norm(), p_OB.norm()}) * kEps;
    const Vector3Ccd phat_AB = (p_OB - p_OB).normalized();

p_OB - p_OA.

[DamrongGuoy]

Checkpoint. I have just read the comment before doSimplex2. It is very useful.

Reviewable status: all files reviewed, 7 unresolved discussions (waiting on @SeanCurtis-TRI)

---

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

     vertex for that direction. It could lie on the boundary of regions 2 & 3
     and still be a valid support vertex.
 The point cannot lie in region 2 (or on the boundary between 2 & 3).

Nit, "The point A cannot..." or "A cannot..."

---

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

   - We confirm that O is not strictly beyond A in the direction p_BO. For all
     A in region 2, O lies beyond A (when projected onto the p_BO vector).
 The point _must_ lie in region 1 (including the boundary between regions 1 &

Nit, same as above.

[sherm1]

A few drive-by comments.

Reviewable status: all files reviewed, 13 unresolved discussions (waiting on @SeanCurtis-TRI)

---

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

      was *not* the origin.
   2. We define a support direction as p_BO = -p_OB and use that to get the
      Minkowski support vector A.

minor: vector A -> point A ? (vector could be p_OA?)

---

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

   2. We define a support direction as p_BO = -p_OB and use that to get the
      Minkowski support vector A.
   3. We confirm that O is not strictly beyond A in the direction p_BO.

BTW consider adding "... beyond A (indicating separation)"

---

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

   - determine if the origin lies within the simplex (i.e. lies on the line
     segment) and reports if this is the case, otherwise

BTW consider adding "...on the line segment , indicating penetration"

---

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

     vertex for that direction. It could lie on the boundary of regions 2 & 3
     and still be a valid support vertex.
 The point cannot lie in region 2 (or on the boundary between 2 & 3).

minor: need a blank line or doxygen will combine this with the previous bulleted paragraph.

---

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

   - We confirm that O is not strictly beyond A in the direction p_BO. For all
     A in region 2, O lies beyond A (when projected onto the p_BO vector).
 The point _must_ lie in region 1 (including the boundary between regions 1 &

minor: blank line needed

---

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

     A in region 2, O lies beyond A (when projected onto the p_BO vector).
 The point _must_ lie in region 1 (including the boundary between regions 1 &
 2) by the process of elimination.

BTW the "process of elimination" is usually written without "the". Not sure why!

---

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

 a 3-simplex via the same construction process. Therefore, as long as
 doSimplex2() is only called from doSimplex() its region 1 assumption _should_
 remain valid.

BTW Wow! Awesome comment. I understand this now!

---

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

  // Confirm that A is in region 1.
  assert(p_OA.normalized().dot(p_OB.normalized()) <= 0);

minor: the ".normalize()" here doesn't help, and in fact can cause trouble (NaN) if one of the vectors is zero. The reason is that this is just (p_OA⋅p_OB)/d where d=‖p_OA‖⋅‖p_OB‖ ≥ 0. The sign can't change by dividing by d.

---

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

  // |A × B| = |A||B|sin(θ). If they are co-linear then theta will be zero -->
  // sin(θ) = 0. So, |A × B| = 0. Equivalently, |A × B|² = 0 and since
  // |A| = |B| = 1 we get |A × B|² = 0 is our test or, numerically,

BTW consider "and since we will make |A| = |B| = 1 below"

---

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

  const Vector3<ccd_real_t> phat_AB = (p_OB - p_OA).normalized();
  const Vector3<ccd_real_t> norm = p_OB.normalized().cross(phat_AB);
  if (norm.squaredNorm() < eps * eps) return 1;

minor: confusing to call this vector norm here with the other meaning of norm in the same expression. Consider cross or perp instead?

---

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

  // magnitude is large enough (> ε) to provide a meaningful direction.
  const Vector3<ccd_real_t> new_dir = norm.cross(phat_AB);
  ccdVec3Set(dir, new_dir(0), new_dir(1), new_dir(2));

If ‖norm‖ ≈ ε and phat_AB is also very short then their cross product could be a very small vector << ε. Is that OK? I guess this gets normalized somewhere later before being used as a support direction?

[SeanCurtis-TRI]

Comments addressed and a stab at correcting various CI issues. PTAL.

Reviewable status: 1 of 3 files reviewed, 6 unresolved discussions (waiting on @hongkai-dai and @sherm1)

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: vector A -> point A ? (vector could be p_OA?)

done I like "point" more than the intended "vertex". I'm going with that.

---

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

Previously, sherm1 (Michael Sherman) wrote…

BTW consider adding "... beyond A (indicating separation)"

Done

---

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

Previously, sherm1 (Michael Sherman) wrote…

BTW consider adding "...on the line segment , indicating penetration"

Done

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

Nit, "The point A cannot..." or "A cannot..."

Done

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: need a blank line or doxygen will combine this with the previous bulleted paragraph.

Done

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

Nit, same as above.

Done

---

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

Previously, sherm1 (Michael Sherman) wrote…

BTW the "process of elimination" is usually written without "the". Not sure why!

Done Thanks :)

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: the ".normalize()" here doesn't help, and in fact can cause trouble (NaN) if one of the vectors is zero. The reason is that this is just (p_OA⋅p_OB)/d where d=‖p_OA‖⋅‖p_OB‖ ≥ 0. The sign can't change by dividing by d.

Done Your prediction of Nan burned me. I reformulated the test based on CI failures (A == O was one of the failures. :D)

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: confusing to call this vector norm here with the other meaning of norm in the same expression. Consider cross or perp instead?

Done I decided to go with a third option - remove the otherwise pointless abbreviation. I feel "plane_normal" captures the semantics and won't be confused with "the norm".

---

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

Previously, sherm1 (Michael Sherman) wrote…

If ‖norm‖ ≈ ε and phat_AB is also very short then their cross product could be a very small vector << ε. Is that OK? I guess this gets normalized somewhere later before being used as a support direction?

BTW I originally normalized norm (now normal). But all of my tests passed without it (and I really wanted to get rid of the third normalization). Hence the comment about that the magnitude of norm had already been shown to be "large enough".

However, I can't convincingly argue that it can't cause problems. So, I've extended the comment to open the door for future developers to put it back.

The resulting cross product need only provide an ordering of minkowski vertices. So, as long as it's larger than epsilon, we're good.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 71 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

const Vector3Ccd, same below.

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 94 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

This is predicated on u being positive. Otherwise A is in region 1. Add an assertion that u > 0?

Not in this case -- this is me attempting to create A not in region 1 -- that's why it's used to configure the death test. And, even then, I'm loathe to put in a constraint that u < 0, because I'm testing cases where u > -ε still is considered region 1. In this case, I'll rely on the fact that it is called in the death tests as the proper affirmation.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 170 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

defined by.

Done.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Should we also check that dir = -v, where v is the closest point on AB to the origin? Currently we checked that dir is colinear with -v, but not that they are equal.

That test is encapsulated within the other three. If it a) lies on the plane formed by OAB, is perpendicular to AB, and points towards O, then dir = -v (without computing v).

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 177 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

p_OB - p_OA.

Done

[sherm1]

Reviewed 1 of 2 files at r3.
Reviewable status: 2 of 3 files reviewed, 5 unresolved discussions (waiting on @hongkai-dai)

[hongkai-dai]

Reviewable status: 2 of 3 files reviewed, 1 unresolved discussion (waiting on @hongkai-dai and @SeanCurtis-TRI)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

That test is encapsulated within the other three. If it a) lies on the plane formed by OAB, is perpendicular to AB, and points towards O, then dir = -v (without computing v).

I disagree, dir can be k * -v, where k is a scalar, and still satisfies that dir lies on the plane OAB, being perpendicular to AB, and also points towards O.

[SeanCurtis-TRI]

Reviewable status: 2 of 3 files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I disagree, dir can be k * -v, where k is a scalar, and still satisfies that dir lies on the plane OAB, being perpendicular to AB, and also points towards O.

Ah...I see the point you're making. I agree, my test doesn't affirm anything about the magnitude of dir.

In this case, I would argue the magnitude of dir is irrelevant. And, in fact, if I were to put my normalization back in, in almost every case, dir = k * -v = 1 would be the solution. I don't believe any of the code relies on the magnitude of dir implicitly (please correct me if I'm wrong) and, therefore, as long as it has sufficient magnitude to be operable, and the right direction, the actual magnitude doesn't matter.

[hongkai-dai]

Reviewable status: 2 of 3 files reviewed, 1 unresolved discussion (waiting on @hongkai-dai and @SeanCurtis-TRI)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Ah...I see the point you're making. I agree, my test doesn't affirm anything about the magnitude of dir.

In this case, I would argue the magnitude of dir is irrelevant. And, in fact, if I were to put my normalization back in, in almost every case, dir = k * -v = 1 would be the solution. I don't believe any of the code relies on the magnitude of dir implicitly (please correct me if I'm wrong) and, therefore, as long as it has sufficient magnitude to be operable, and the right direction, the actual magnitude doesn't matter.

I think we use dir later. For example, in __ccdGJK, it determines if |dir| = 0, and if it does, then means that -v = 0, and hence the objects are intersecting.

fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

Lines 1396 to 1398 in 8e929da

	if (ccdIsZero(ccdVec3Len2(&dir))){
	return -1; // intersection not found
	}

[SeanCurtis-TRI]

Reviewable status: 2 of 3 files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I think we use dir later. For example, in __ccdGJK, it determines if |dir| = 0, and if it does, then means that -v = 0, and hence the objects are intersecting.

fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

Lines 1396 to 1398 in 8e929da

	if (ccdIsZero(ccdVec3Len2(&dir))){
	return -1; // intersection not found
	}

However, that's a line of code we all agree is garbage. If the origin is really that close to the simplex, the doSimplex*() should have caught it and returned 1. So, the fact that it does that test at all is a design error, and the fact that it decides that the distance to the origin being zero means no intersection is found, is a logic bug.

But this does raise the bigger question which is: "Should the magnitude of dir actually matter?" And to determine that, I'll have to look at all the doSimplex*() methods. I assume the answer should be "no", because it's hard to believe that the line you cited is any smarter about recognizing "zero distance" than the simplex operations themselves.

[DamrongGuoy]

Checkpoint. I've finished the gjk_libccd-inl.h. Next I'll review the unit test.

Reviewable status: 2 of 3 files reviewed, 2 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

---

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

 a 3-simplex via the same construction process. Therefore, as long as
 doSimplex2() is only called from doSimplex() its region 1 assumption _should_
 remain valid.

BTW, can we put a note on how we use the epsilon? I see two places: verifying that A is in region 1, and checking co-linearity of A, O, B. It looks like the first application uses ε as a squared distance, and the second application uses ε as sin(θ).

---

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

  //  - p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
  //  - p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //  - (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)

Nit, at first glance, I thought the beginning "-" means negative sign. Now I see they are indentation. Should we remove "-", so it would look like this?

  //    p_AB ⋅ phat_OB >= |p_OB| * (1 - ε)
  //    p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
  //    p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //    (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)

---

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

  //  - p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
  //  - p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //  - (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)

BTW, is it clear why (1 - ε) as opposed to (1 + ε) ? Perhaps it's something the main algorithm prefers?

---

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

  //  - p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
  //  - p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //  - (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)

BTW, does this simplification help?

//            (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε) 
//  p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) 
//  p_OB ⋅ p_OB - p_OA ⋅ p_OB >= p_OB ⋅ p_OB * (1 - ε) 
//                        - p_OA ⋅ p_OB >= p_OB ⋅ p_OB * (- ε)
//                          p_OA ⋅ p_OB <= |p_OB|² * ε

In other words, the assert could be:

assert(p_OB.dot(p_OA) <= p_OB.squareNorm() * eps &&
            "A is not in region 1");

---

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

  // versa); the imprecision would be introduced in the cross-product operation.
  const Vector3<ccd_real_t> phat_AB = (p_OB - p_OA).normalized();
  const Vector3<ccd_real_t> plane_normal = p_OB.normalized().cross(phat_AB);

BTW, since we take a cross product of two unit vectors, should we comment about its length, perhaps like this?

// The length of plane_normal is at most 1.0 by construction.

---

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

//   0:
//  -1: If the 3-simplex is degenerate. How is this intepreted?
static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)

BTW, could we return an enum class instead of an int? Perhaps like this?

enum class LocationOfOrigin {kInside, kOutside, kAmbiguous};

or any names as you see fit. The number 0, 1, -1 doesn't say much to me.

If we decide to do it for doSimplex3, we should do it for doSimplex2 and doSimplex4 too.

[DamrongGuoy]

with a few questions.

Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 4 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

a discussion (no related file):
BTW, in the future, we might want to do value-parameterized tests (https://github.com/google/googletest/blob/master/googletest/docs/advanced.md#value-parameterized-tests).
It is more involved to set up, but it's good for data-driven testing. I'll be happy to work on it if we create a follow-up issue in GitHub.

---

---

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

  //  - p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //  - (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)
  assert(p_OB.dot(p_OB - p_OA) >= p_OB.squaredNorm() * (1 - eps) &&

BTW, I'm not sure why Reviewable lost my comment about this line (GitHub still has it), so I repeat it briefly here. It should be the same as:

assert(p_OB.dot(p_OA) <= p_OB.squaredNorm() * eps &&

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 202 at r4 (raw file):

    if (std::abs(dir.dot(p_OA.normalized().cross(phat_AB))) > eps) {
      return ::testing::AssertionFailure()
             << "Direction does not lie on the triangle formed by OAB:"

Nit, "on the plane of the triangle OAB" ?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 223 at r4 (raw file):

  EXPECT_EQ(norm_OB.dot(phat_OB), 0);
  const ccd_real_t dist_OB = 3;
  const Vector3Ccd p_OB = phat_OB * dist_OB;

Nit, should we refactor this block of code? I think I see it the third time now.

[SeanCurtis-TRI]

Good comments and conversations. PTAL

Reviewable status: 1 of 3 files reviewed, 1 unresolved discussion (waiting on @DamrongGuoy, @hongkai-dai, and @sherm1)

a discussion (no related file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, in the future, we might want to do value-parameterized tests (https://github.com/google/googletest/blob/master/googletest/docs/advanced.md#value-parameterized-tests).
It is more involved to set up, but it's good for data-driven testing. I'll be happy to work on it if we create a follow-up issue in GitHub.

Ok Agreed.

---

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, can we put a note on how we use the epsilon? I see two places: verifying that A is in region 1, and checking co-linearity of A, O, B. It looks like the first application uses ε as a squared distance, and the second application uses ε as sin(θ).

Done I've added a comment at the declaration of eps, ptal.

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

Nit, at first glance, I thought the beginning "-" means negative sign. Now I see they are indentation. Should we remove "-", so it would look like this?

  //    p_AB ⋅ phat_OB >= |p_OB| * (1 - ε)
  //    p_AB ⋅ phat_OB ⋅ |p_OB| >= |p_OB|⋅|p_OB| * (1 - ε)
  //    p_AB ⋅ p_OB >= |p_OB|² * (1 - ε)
  //    (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε)

Done Agreed

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, is it clear why (1 - ε) as opposed to (1 + ε) ? Perhaps it's something the main algorithm prefers?

Done I've extended the documentation

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, does this simplification help?

//            (p_OB - p_OA) ⋅ p_OB >= |p_OB|² * (1 - ε) 
//  p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε) 
//  p_OB ⋅ p_OB - p_OA ⋅ p_OB >= p_OB ⋅ p_OB * (1 - ε) 
//                        - p_OA ⋅ p_OB >= p_OB ⋅ p_OB * (- ε)
//                          p_OA ⋅ p_OB <= |p_OB|² * ε

In other words, the assert could be:

assert(p_OB.dot(p_OA) <= p_OB.squareNorm() * eps &&
            "A is not in region 1");

Done I like this. And it still works if A = O.

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, I'm not sure why Reviewable lost my comment about this line (GitHub still has it), so I repeat it briefly here. It should be the same as:

assert(p_OB.dot(p_OA) <= p_OB.squaredNorm() * eps &&

OK It didn't "lose" it; I bet that you hit publish. And when you BTW the comment and publish, it disappears off your radar until I respond.

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, since we take a cross product of two unit vectors, should we comment about its length, perhaps like this?

// The length of plane_normal is at most 1.0 by construction.

OK This goes away now that it's size is unbounded.

---

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

Previously, DamrongGuoy (Damrong Guoy) wrote…

BTW, could we return an enum class instead of an int? Perhaps like this?

enum class LocationOfOrigin {kInside, kOutside, kAmbiguous};

or any names as you see fit. The number 0, 1, -1 doesn't say much to me.

If we decide to do it for doSimplex3, we should do it for doSimplex2 and doSimplex4 too.

OK I agree. The note was just about understanding the semantics, not designing interface.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 163 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

However, that's a line of code we all agree is garbage. If the origin is really that close to the simplex, the doSimplex*() should have caught it and returned 1. So, the fact that it does that test at all is a design error, and the fact that it decides that the distance to the origin being zero means no intersection is found, is a logic bug.

But this does raise the bigger question which is: "Should the magnitude of dir actually matter?" And to determine that, I'll have to look at all the doSimplex*() methods. I assume the answer should be "no", because it's hard to believe that the line you cited is any smarter about recognizing "zero distance" than the simplex operations themselves.

Per f2f - I've updated the test and the documentation. PTAL

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 202 at r4 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Nit, "on the plane of the triangle OAB" ?

Done

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_gjk_doSimplex2.cpp, line 223 at r4 (raw file):

Previously, DamrongGuoy (Damrong Guoy) wrote…

Nit, should we refactor this block of code? I think I see it the third time now.

Done

[hongkai-dai]

Reviewable status: 1 of 3 files reviewed, all discussions resolved (waiting on @DamrongGuoy and @sherm1)

[DamrongGuoy]

Reviewable status: 1 of 3 files reviewed, all discussions resolved (waiting on @DamrongGuoy and @sherm1)

---

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done I've added a comment at the declaration of eps, ptal.

Looks good. Thanks.

[DamrongGuoy]

Reviewed 1 of 2 files at r5.
Reviewable status: 2 of 3 files reviewed, all discussions resolved (waiting on @sherm1)

[SeanCurtis-TRI]

Reviewed 1 of 2 files at r5.
Reviewable status: complete! all files reviewed, all discussions resolved