Reworking gjk_libccd doSimplex2()

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.

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

@@ -226,48 +226,122 @@ _ccd_inline void tripleCross(const ccd_vec3_t *a, const ccd_vec3_t *b,
  ccdVec3Cross(d, &e, c);
 }
 
-static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir)
-{
- const ccd_support_t *A, *B;
- ccd_vec3_t AB, AO, tmp;
- ccd_real_t dot;
-
- // get last added as A
- A = ccdSimplexLast(simplex);
- // get the other point
- B = ccdSimplexPoint(simplex, 0);
- // compute AB oriented segment
- ccdVec3Sub2(&AB, &B->v, &A->v);
- // compute AO vector
- ccdVec3Copy(&AO, &A->v);
- ccdVec3Scale(&AO, -CCD_ONE);
+/* This is *not* an implementation of the general function: what's the nearest
+ point on the line segment AB to the origin O? It is not intended to be.
+ This is a limited, special case which exploits the known (or at least
+ expected) construction history of AB. The history is as follows:
+
+ 1. We originally started with the Minkowski support vertex B (p_OB), which
+ 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.
+ 3. We confirm that O is not strictly beyond A in the direction p_BO.
+ 4. Then, and only then, do we invoke this method.
+
+ The method will do one of two things:
+
+ - determine if the origin lies within the simplex (i.e. lies on the line
+ segment) and reports if this is the case, otherwise
+ - it computes a new support direction: a vector pointing to the origin from
+ the nearest point on the segment AB.
+
+ The algorithm exploits the construction history as outlined below. Without
+ loss of generality, we place B some non-zero distance away from the origin
+ along the î direction (all other orientations can be rigidly transformed to
+ this canonical example). The diagram below shows the origin O and the vertex
+ B. It also shows three regions: 1, 2, and 3.
+
+ ĵ
+ 1 ⯅ 2 3
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ ───────────O──────────B────⯈ î
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+ │░░░░░░░░░░┆▒▒▒▒▒
+
+ The point A cannot lie in region 3.
+ - B is a support vertex of the Minkowski difference. p_BO defines the
+ support vector that produces the support vertex A. Vertex A must lie at
+ least as far in the p_BO as B otherwise A is not actually a valid support
+ 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).
+ - 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 &
+ 2) by the process of elimination.
+
+ The implication of this is that the O must project onto the _interior_ of the
+ line segment AB (with one notable exception). If A = O, then the projection of
+ O is at the end point and, is in fact, itself.
+
+ Therefore, this function can only have two possible outcomes:
+
+ 1. In the case where p_BA = k⋅p_BO (i.e., they are co-linear), we know the
+ origin lies "in" the simplex. If A = O, it lies on the simplex's surface
+ and the objects are touching, otherwise, the objects are penetrating.
+ Either way, we can report that they are definitely *not* separated.
+ 2. Otherwise, the simplex remains unchanged, and the new support direction
+ is perpendicular to the line segment AB, pointing to O from the nearest
+ point on the segment to O.
+
+ Return value indicates concrete knowledge that the origin lies "in" the
+ 2-simplex (encoded as a 1), or indication that computation should continue (0).
+
+ @note: doSimplex2 should _only_ be called with the construction history
+ outlined above: promotion of a 1-simplex. doSimplex2() is only invoked by
+ doSimplex(). This follows the computation of A and the promotion of the
+ simplex. Therefore, the history is always valid; even though doSimplex3() can
+ demote itself to a 2-simplex, that 2-simplex immediately gets promoted back to
+ 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.
+*/
+static int doSimplex2(ccd_simplex_t *simplex, ccd_vec3_t *dir) {
+ const ccd_real_t eps = constants<ccd_real_t>::eps();
 
- // dot product AB . AO
- dot = ccdVec3Dot(&AB, &AO);
+ const Vector3<ccd_real_t> p_OA(simplex->ps[simplex->last].v.v);
+ const Vector3<ccd_real_t> p_OB(simplex->ps[0].v.v);
 
- // check if origin doesn't lie on AB segment
- ccdVec3Cross(&tmp, &AB, &AO);
- if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
- return 1;
- }
+ // Confirm that A is in region 1.
+ assert(p_OA.normalized().dot(p_OB.normalized()) <= 0);
 
- // check if origin is in area where AB segment is
- if (ccdIsZero(dot) || dot < CCD_ZERO){
- // origin is in outside are of A
- ccdSimplexSet(simplex, 0, A);
- ccdSimplexSetSize(simplex, 1);
- ccdVec3Copy(dir, &AO);
- }else{
- // origin is in area where AB segment is
-
- // keep simplex untouched and set direction to
- // AB x AO x AB
- tripleCross(&AB, &AO, &AB, dir);
- }
+ // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in"
+ // the simplex.
 
+ // |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,
+ // |A × B|² < ε².
+ //
+ // We normalize the vectors p_AB and p_OB so that the evaluation of the angle
+ // between them is *not* sensitive to the length of those vectors. Otherwise,
+ // disparity in length would lead to noise, or large vectors would make the
+ // epsilon meaningless. It's not enough to simply scale epsilon by |p_AB| and
+ // |p_OB| because it doesn't address the issue of |p_OB| << |p_AB| (or vice
+ // 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> norm = p_OB.normalized().cross(phat_AB);
+ if (norm.squaredNorm() < eps * eps) return 1;
+
+ // Otherwise the direction lies perpendicular to the segment, pointing towards
+ // O. Note: we don't normalize norm because we've already decided that it's
+ // 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));
  return 0;
 }
 
+// TODO(SeanCurtis-TRI): Define the return value:
+// 1: (like doSimplex2) --> origin is "in" the simplex.
+// 0:
+// -1: If the 3-simplex is degenerate. How is this intepreted?
 static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
 {
  const ccd_support_t *A, *B, *C;
@@ -281,14 +355,21 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
  C = ccdSimplexPoint(simplex, 0);
 
  // check touching contact
+ // TODO(SeanCurtis-TRI): This is dist2 (i.e., dist_squared) and should be
+ // tested to be zero within a tolerance of ε² (and not just ε).
  dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
  if (ccdIsZero(dist)){
  return 1;
  }
 
  // check if triangle is really triangle (has area > 0)
- // if not simplex can't be expanded and thus no itersection is found
+ // if not simplex can't be expanded and thus no intersection is found
+ // TODO(SeanCurtis-TRI): Coincident points is sufficient but not necessary
+ // for a zero-area triangle. What about co-linearity? Can we guarantee that
+ // co-linearity can't happen? See the `triangle_area_is_zero()` method in
+ // this same file.
  if (ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v)){
+ // TODO(SeanCurtis-TRI): Why do we simply quit if the simplex is degenerate?
  return -1;
  }
 

test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt

@@ -1,6 +1,7 @@
 set(tests
  test_gjk_libccd-inl_epa.cpp
  test_gjk_libccd-inl_extractClosestPoints.cpp
+ test_gjk_libccd-inl_gjk_doSimplex2.cpp
  test_gjk_libccd-inl_signed_distance.cpp
 )