Fixing capsule-capsule distance

1. Correct definition of capsule definition - i.e. *centered* on the
   capsule origin.
2. Handle the case where the capsule center lines intersect.
3. Make more robust tests which exercise all (hopefully) the corners of
   the code.

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h

@@ -114,9 +114,10 @@ S closestPtSegmentSegment(
  // If segments not parallel, compute closest point on L1 to L2 and
  // clamp to segment S1. Else pick arbitrary s (here 0)
  if (denom != 0.0) {
- std::cerr << "denominator equals zero, using 0 as reference" << std::endl;
  s = clamp((b*f - c*e) / denom, (S)0.0, (S)1.0);
- } else s = 0.0;
+ } else {
+ s = 0.0;
+ }
  // Compute point on L2 closest to S1(s) using
  // t = Dot((P1 + D1*s) - P2,D2) / Dot(D2,D2) = (b*s + f) / e
  t = (b*s + f) / e;
@@ -143,41 +144,82 @@ S closestPtSegmentSegment(
 
 //==============================================================================
 template <typename S>
-bool capsuleCapsuleDistance(const Capsule<S>& s1, const Transform3<S>& tf1,
- const Capsule<S>& s2, const Transform3<S>& tf2,
- S* dist, Vector3<S>* p1_res, Vector3<S>* p2_res)
+bool capsuleCapsuleDistance(const Capsule<S>& s1, const Transform3<S>& X_FC1,
+ const Capsule<S>& s2, const Transform3<S>& X_FC2,
+ S* dist, Vector3<S>* p_FW1, Vector3<S>* p_FW2)
 {
-
- Vector3<S> p1(tf1.translation());
- Vector3<S> p2(tf2.translation());
-
- // line segment composes two points. First point is given by the origin, second point is computed by the origin transformed along z.
- // extension along z-axis means transformation with identity matrix and translation vector z pos
- Transform3<S> transformQ1 = tf1 * Translation3<S>(Vector3<S>(0,0,s1.lz));
- Vector3<S> q1 = transformQ1.translation();
-
- Transform3<S> transformQ2 = tf2 * Translation3<S>(Vector3<S>(0,0,s2.lz));
- Vector3<S> q2 = transformQ2.translation();
-
- // s and t correspont to the length of the line segment
+ assert(dist != nullptr);
+ assert(p_FW1 != nullptr);
+ assert(p_FW2 != nullptr);
+
+ // Origin' of the capsules' frames relative to F's origin.
+ Vector3<S> p_FC1o = X_FC1.translation();
+ Vector3<S> p_FC2o = X_FC2.translation();
+
+ // A capsule is defined centered on the origin of its canonical frame C
+ // and with the central line segment aligned with Cz. So, the two end points
+ // of the capsule's center line segment are at `z = lz / 2 * Cz` and
+ // `z = -lz / 2 * Cz`, respectively. Cz_F is simply the third column of the
+ // rotation matrix.
+ auto calc_half_arm = [](const Capsule<S>& c,
+ const Transform3<S>& X_FC) -> Vector3<S> {
+ const S half_length = c.lz / 2;
+ const Vector3<S> Cz_F = X_FC.matrix().template block<3, 1>(0, 2);
+ return Cz_F * half_length;
+ };
+
+ const Vector3<S> half_arm_1_F = calc_half_arm(s1, X_FC1);
+ Vector3<S> p_FC1a = p_FC1o + half_arm_1_F;
+ Vector3<S> p_FC1b = p_FC1o - half_arm_1_F;
+
+ const Vector3<S> half_arm_2_F = calc_half_arm(s2, X_FC2);
+ Vector3<S> p_FC2a = p_FC2o + half_arm_2_F;
+ Vector3<S> p_FC2b = p_FC2o - half_arm_2_F;
+
+ // s and t correspond to the length of each line segment; should be s1.lz and
+ // s2.lz, respectively.
  S s, t;
- Vector3<S> c1, c2;
-
- S result = closestPtSegmentSegment(p1, q1, p2, q2, s, t, c1, c2);
- *dist = sqrt(result)-s1.radius-s2.radius;
-
- // getting directional unit vector
- Vector3<S> distVec = c2 -c1;
- distVec.normalize();
-
- // extend the point to be border of the capsule.
- // Done by following the directional unit vector for the length of the capsule radius
- *p1_res = c1 + distVec*s1.radius;
-
- distVec = c1-c2;
- distVec.normalize();
-
- *p2_res = c2 + distVec*s2.radius;
+ // The points on the line segments closest to each other.
+ Vector3<S> p_FN1, p_FN2;
+ // TODO(SeanCurtis-TRI): This query isn't well tailored for this problem.
+ // By construction, we know the unit-length vectors for the two segments (and
+ // their lengths), but closestPtSegmentSegment() infers the segment direction
+ // from the end point. Furthermore, it returns the values for s and t,
+ // neither of which is required by this function. The API should be
+ // streamlined so there is less waste.
+ S squared_dist = closestPtSegmentSegment(p_FC1a, p_FC1b, p_FC2a, p_FC2b, s, t,
+ p_FN1, p_FN2);
+
+ const S segment_dist = sqrt(squared_dist);
+ *dist = segment_dist - s1.radius - s2.radius;
+ Vector3<S> vhat_C1C2_F;
+ const auto eps = constants<S>::eps_78();
+ // We can only use the vector between the center-line nearest points to find
+ // the witness points if they are sufficiently far apart.
+ if (segment_dist > eps) {
+ vhat_C1C2_F = (p_FN2 - p_FN1) / segment_dist;
+ } else {
+ // The points are too close. The center lines intersect. We have two cases:
+ // 1. They intersect at a single point (non-parallel center lines).
+ // - The center lines span a plane and the witness points should lie
+ // above and below that plane the corresponding radius amount.
+ // 2. They intersect at multiple points (parallel, overlapping center
+ // lies).
+ // - Any direction on the plane perpendicular to the common center line
+ // will suffice. We arbitrarily pick the Cx direction.
+ const Vector3<S>& C1z_F = X_FC1.matrix().template block<3, 1>(0, 2);
+ const Vector3<S>& C2z_F = X_FC2.matrix().template block<3, 1>(0, 2);
+ using std::abs;
+ if (abs(C1z_F.dot(C2z_F)) < 1 - eps) {
+ // Vectors are sufficiently perpendicular to use cross product.
+ vhat_C1C2_F = C1z_F.cross(C2z_F).normalized();
+ } else {
+ // Vectors are parallel, simply use Cx_F as the vector.
+ vhat_C1C2_F = X_FC1.matrix().template block<3, 1>(0, 0);
+ }
+ }
+ *p_FW1 = p_FN1 + vhat_C1C2_F * s1.radius;
+ *p_FW2 = p_FN2 - vhat_C1C2_F * s2.radius;
 
  return true;
 }

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h

@@ -61,14 +61,35 @@ S closestPtSegmentSegment(
  Vector3<S> p1, Vector3<S> q1, Vector3<S> p2, Vector3<S> q2,
  S &s, S &t, Vector3<S> &c1, Vector3<S> &c2);
 
-// Computes closest points C1 and C2 of S1(s)=P1+s*(Q1-P1) and
-// S2(t)=P2+t*(Q2-P2), returning s and t. Function result is squared
-// distance between between S1(s) and S2(t)
+/** Computes the signed distance between two capsules `s1` and `s2` (with
+ given poses `X_FC1` and `X_FC2` of the two capsules in a common frame `F`).
+ Further reports the witness points to that distance (`p_FW1` and `p_FW2`).
+
+ It is guaranteed that `|p_FW1 - p_FW2| = |dist|`.
+
+ There are degenerate cases where there is no _unique_ pair of witness points.
+ If the center lines of the two capsules intersect (either once or multiple
+ times when the are co-linear) then distance will still be reported (a
+ negative value with the maximum magnitude possible between the two capsules)
+ and an arbitrary pair of witness points from the set of valid pairs.
+ @param s1 The first capsule.
+ @param X_FC1 The pose of the first capsule in a common frame `F`.
+ @param s2 The second capsule.
+ @param X_FC2 The pose of the second capsule in a common frame `F`.
+ @param dist The signed distance between the two capsules (negative indicates
+ intersection.
+ @param p_FW1 The first witness point: a point on the surface of the first
+ capsule measured and expressed in the common frame `F`.
+ @param p_FW2 The second witness point: a point on the surface of the second
+ capsule measured and expressed in the common frame `F`.
+ @return `true`.
+ @tparam S The scalar type for computation.
+ */
 template <typename S>
 FCL_EXPORT
-bool capsuleCapsuleDistance(const Capsule<S>& s1, const Transform3<S>& tf1,
- const Capsule<S>& s2, const Transform3<S>& tf2,
- S* dist, Vector3<S>* p1_res, Vector3<S>* p2_res);
+bool capsuleCapsuleDistance(const Capsule<S>& s1, const Transform3<S>& X_FC1,
+ const Capsule<S>& s2, const Transform3<S>& X_FC2,
+ S* dist, Vector3<S>* p_FW1, Vector3<S>* p_FW2);
 
 } // namespace detail
 } // namespace fcl