Reworking gjk_libccd doSimplex2()

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

@@ -40,8 +40,8 @@
 
 #include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd.h"
 
-#include <unordered_set>
 #include <unordered_map>
+#include <unordered_set>
 
 #include "fcl/common/unused.h"
 #include "fcl/common/warning.h"
@@ -226,48 +226,152 @@ _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);
-
- // dot product AB . AO
- dot = ccdVec3Dot(&AB, &AO);
+/* 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 point 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 point A.
+ 3. We confirm that O is not strictly beyond A in the direction p_BO
+ (confirming separation).
+ 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, confirming non-separation) 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 direction is
+ guaranteed; the only guarantee about the magnitude is that it is
+ numerically viable (i.e. greater than epsilon).
+
+ 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 point
+ 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 point of the Minkowski difference. p_BO defines the
+ support vector that produces the support point A. Vertex A must lie at
+ least as far in the p_BO as B otherwise A is not actually a valid support
+ point for that direction. It could lie on the boundary of regions 2 & 3
+ and still be a valid support point.
+
+ The point A 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 A _must_ lie in region 1 (including the boundary between regions 1 &
+ 2) by 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. p_BA ≠ k⋅p_BO, we define the new support direction as 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) {
+ // Used to define numerical thresholds near zero; typically scaled to the size
+ // of the quantities being tested.
+ const ccd_real_t eps = constants<ccd_real_t>::eps();
 
- // check if origin doesn't lie on AB segment
- ccdVec3Cross(&tmp, &AB, &AO);
- if (ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO){
+ 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);
+
+ // Confirm that A is in region 1. Given that A may be very near to the origin,
+ // we must avoid normalizing p_OA. So, we use this instead.
+ // let A' be the projection of A onto the line defined by O and B.
+ // |A'B| >= |OB| iff A is in region 1.
+ // Numerically, we can express it as follows (allowing for |A'B| to be ever
+ // so slightly *less* than |OB|):
+ // p_AB ⋅ phat_OB >= |p_OB| - |p_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 - ε)
+ // p_OB ⋅ p_OB - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε)
+ // |p_OB|² - p_OA ⋅ p_OB >= |p_OB|² * (1 - ε)
+ // -p_OA ⋅ p_OB >= -|p_OB|²ε
+ // p_OA ⋅ p_OB <= |p_OB|²ε
+ assert(p_OA.dot(p_OB) <= p_OB.squaredNorm() * eps && "A is not in region 1");
+
+ // Test for co-linearity. Given A is in region 1, co-linearity --> O is "in"
+ // the simplex.
+ // We'll use the angle between two vectors to determine co-linearity: p_AB
+ // and p_OB. If they are co-linear, then the angle between them (θ) is zero.
+ // Similarly, sin(θ) is zero. Ideally, it can be expressed as:
+ // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| = 0
+ // Numerically, we allow θ (and sin(θ)) to be slightly larger than zero
+ // leading to a numerical formulation as:
+ // |p_AB × p_OB| = |p_AB||p_OB||sin(θ)| < |p_AB||p_OB|ε
+ // Finally, to reduce the computational cost, we eliminate the square roots by
+ // evaluating the equivalently discriminating test:
+ // |p_AB × p_OB|² < |p_AB|²|p_OB|²ε²
+ //
+ // In addition to providing a measure of co-linearity, the cross product gives
+ // us the normal to the plane on which points A, B, and O lie (which we will
+ // use later to compute a new support direction, as necessary).
+ const Vector3<ccd_real_t> p_AB = p_OB - p_OA;
+ const Vector3<ccd_real_t> plane_normal = p_OB.cross(p_AB);
+ if (plane_normal.squaredNorm() <
+ p_AB.squaredNorm() * p_OB.squaredNorm() * eps * eps) {
  return 1;
  }
 
- // 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);
- }
-
+ // O is not co-linear with AB, so dist(O, AB) > ε. Define `dir` as the
+ // direction to O from the nearest point on AB.
+ // Note: We use the normalized `plane_normal` (n̂) because we've already
+ // concluded that the origin is farther from AB than ε. We want to make sure
+ // `dir` likewise has a magnitude larger than ε. With normalization, we know
+ // |dir| = |n̂ × AB| = |AB| > dist(O, AB) > ε.
+ // Without normalizing, if |OA| and |OB| were smaller than ³√ε but
+ // sufficiently larger than ε, dist(O, AB) > ε, but |dir| < ε.
+ const Vector3<ccd_real_t> new_dir = plane_normal.normalized().cross(p_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 +385,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
 )