Fixing capsule-capsule distance

CHANGELOG.md

@@ -19,6 +19,8 @@
  simpler, documented constructor:
  [#325](https://github.com/flexible-collision-library/fcl/pull/325),
  [#338](https://github.com/flexible-collision-library/fcl/pull/338)
+ * Fixed interpretation of capsule in primitive Capsule-Capsule distance computation.
+ [#436](https://github.com/flexible-collision-library/fcl/pull/436)
 
 * Broadphase
 

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

@@ -51,10 +51,12 @@ extern template
 double clamp(double n, double min, double max);
 
 //==============================================================================
-extern template
-double closestPtSegmentSegment(
- Vector3d p1, Vector3d q1, Vector3d p2, Vector3d q2,
- double &s, double& t, Vector3d &c1, Vector3d &c2);
+extern template double closestPtSegmentSegment(const Vector3d& p_FP1,
+ const Vector3d& p_FQ1,
+ const Vector3d& p_FP2,
+ const Vector3d& p_FQ2, double* s,
+ double* t, Vector3d* p_FC1,
+ Vector3d* p_FC2);
 
 //==============================================================================
 extern template
@@ -74,110 +76,164 @@ S clamp(S n, S min, S max)
 
 //==============================================================================
 template <typename S>
-S closestPtSegmentSegment(
- Vector3<S> p1, Vector3<S> q1, Vector3<S> p2, Vector3<S> q2,
- S &s, S &t, Vector3<S> &c1, Vector3<S> &c2)
-{
- const S EPSILON = 0.001;
- Vector3<S> d1 = q1 - p1; // Direction vector of segment S1
- Vector3<S> d2 = q2 - p2; // Direction vector of segment S2
- Vector3<S> r = p1 - p2;
- S a = d1.dot(d1); // Squared length of segment S1, always nonnegative
-
- S e = d2.dot(d2); // Squared length of segment S2, always nonnegative
- S f = d2.dot(r);
- // Check if either or both segments degenerate into points
- if (a <= EPSILON && e <= EPSILON) {
- // Both segments degenerate into points
- s = t = 0.0;
- c1 = p1;
- c2 = p2;
- Vector3<S> diff = c1-c2;
- S res = diff.dot(diff);
- return res;
+S closestPtSegmentSegment(const Vector3<S>& p_FP1, const Vector3<S>& p_FQ1,
+ const Vector3<S>& p_FP2, const Vector3<S>& p_FQ2,
+ S* s, S* t, Vector3<S>* p_FC1, Vector3<S>* p_FC2) {
+ // TODO(SeanCurtis-TRI): Document the match underlying this function -- the
+ // variables: a, b, c, e, and f are otherwise overly inscrutable.
+ const auto kEps = constants<S>::eps_78();
+ const auto kEpsSquared = kEps * kEps;
+
+ Vector3<S> p_P1Q1 = p_FQ1 - p_FP1; // Segment 1's displacement vector: D1.
+ Vector3<S> p_P2Q2 = p_FQ2 - p_FP2; // Segment 2's displacement vector: D2.
+ Vector3<S> p_P2P1 = p_FP1 - p_FP2;
+
+ S a = p_P1Q1.dot(p_P1Q1); // Squared length of segment S1, always nonnegative.
+ S e = p_P2Q2.dot(p_P2Q2); // Squared length of segment S2, always nonnegative.
+ S f = p_P2Q2.dot(p_P2P1);
+
+ // Check if either or both segments degenerate into points.
+ if (a <= kEpsSquared && e <= kEpsSquared) {
+ // Both segments degenerate into points.
+ *s = *t = 0.0;
+ *p_FC1 = p_FP1;
+ *p_FC2 = p_FP2;
+ return (*p_FC1 - *p_FC2).squaredNorm();
  }
- if (a <= EPSILON) {
- // First segment degenerates into a point
- s = 0.0;
- t = f / e; // s = 0 => t = (b*s + f) / e = f / e
- t = clamp(t, (S)0.0, (S)1.0);
+ if (a <= kEpsSquared) {
+ // First segment degenerates into a point.
+ *s = 0.0;
+ // t = (b * s + f) / e, s = 0 --> t = f / e.
+ *t = clamp(f / e, (S)0.0, (S)1.0);
  } else {
- S c = d1.dot(r);
- if (e <= EPSILON) {
- // Second segment degenerates into a point
- t = 0.0;
- s = clamp(-c / a, (S)0.0, (S)1.0); // t = 0 => s = (b*t - c) / a = -c / a
+ const S c = p_P1Q1.dot(p_P2P1);
+ if (e <= kEpsSquared) {
+ // Second segment degenerates into a point.
+ *t = 0.0;
+ // s = (b*t - c) / a, t = 0 --> s = -c / a.
+ *s = clamp(-c / a, (S)0.0, (S)1.0);
  } else {
- // The general nondegenerate case starts here
- S b = d1.dot(d2);
- S denom = a*e-b*b; // Always nonnegative
- // 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;
+ // The general non-degenerate case starts here.
+ const S b = p_P1Q1.dot(p_P2Q2);
+ // Mathematically, ae - b² ≥ 0, but we need to protect ourselves from
+ // possible rounding error near zero that _might_ produce -epsilon.
+ using std::max;
+ const S denom = max(S(0), a*e-b*b);
+
+ // If segments are not parallel, compute closest point on L1 to L2 and
+ // clamp to segment S1. Else pick arbitrary s (here 0).
+ if (denom > kEpsSquared) {
+ *s = clamp((b*f - c*e) / denom, (S)0.0, (S)1.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;
-
- //
- //If t in [0,1] done. Else clamp t, recompute s for the new value
- //of t using s = Dot((P2 + D2*t) - P1,D1) / Dot(D1,D1)= (t*b - c) / a
- //and clamp s to [0, 1]
- if(t < 0.0) {
- t = 0.0;
- s = clamp(-c / a, (S)0.0, (S)1.0);
- } else if (t > 1.0) {
- t = 1.0;
- s = clamp((b - c) / a, (S)0.0, (S)1.0);
+ // t = Dot((P1 + D1*s) - P2, D2) / Dot(D2,D2) = (b*s + f) / e
+ *t = (b*(*s) + f) / e;
+
+ // If t in [0,1] done. Else clamp t, recompute s for the new value
+ // of t using s = Dot((P2 + D2*t) - P1, D1) / Dot(D1,D1) = (t*b - c) / a
+ // and clamp s to [0, 1].
+ if(*t < 0.0) {
+ *t = 0.0;
+ *s = clamp(-c / a, (S)0.0, (S)1.0);
+ } else if (*t > 1.0) {
+ *t = 1.0;
+ *s = clamp((b - c) / a, (S)0.0, (S)1.0);
  }
  }
  }
- c1 = p1 + d1 * s;
- c2 = p2 + d2 * t;
- Vector3<S> diff = c1-c2;
- S res = diff.dot(diff);
- return res;
+ *p_FC1 = p_FP1 + p_P1Q1 * (*s);
+ *p_FC2 = p_FP2 + p_P2Q2 * (*t);
+ return (*p_FC1 - *p_FC2).squaredNorm();
+}
+
+// Given a transform relating frames A and B, returns Bz_A, the z-axis of frame
+// B, expressed in frame A.
+template <typename S>
+Vector3<S> z_axis(const Transform3<S>& X_AB) {
+ return X_AB.matrix().template block<3, 1>(0, 2);
 }
 
 //==============================================================================
 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.
+ const Vector3<S> p_FC1o = X_FC1.translation();
+ const 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, R_FC. This "half arm" is the position vector from the
+ // canonical frame's origin to the z+ point: p_CoZ+_F in frame F.
+ 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 = z_axis(X_FC);
+ return half_length * Cz_F;
+ };
+
+ const Vector3<S> half_arm_1_F = calc_half_arm(s1, X_FC1);
+ const Vector3<S> p_FC1a = p_FC1o + half_arm_1_F;
+ const Vector3<S> p_FC1b = p_FC1o - half_arm_1_F;
+
+ const Vector3<S> half_arm_2_F = calc_half_arm(s2, X_FC2);
+ const Vector3<S> p_FC2a = p_FC2o + half_arm_2_F;
+ const 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.
+ const 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 = z_axis(X_FC1);
+ const Vector3<S>& C2z_F = z_axis(X_FC2);
+ 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

@@ -52,23 +52,62 @@ template <typename S>
 FCL_EXPORT
 S clamp(S n, S min, S max);
 
-// 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 pair of closest points `(p_FC1, p_FC2)` between two line
+ segments given by point pairs `(p_FP1, p_FQ1)` and (p_FP2, p_FQ2)`. If the
+ lines are parallel, there may be no _unique_ such point pair, in that case it
+ computes one from the set of nearest pairs. As a by-product, it reports the
+ squared distance between those points and the parameters (`s` and `t`) such
+ that `p_FC1 = p_FP1 + s * (p_FQ1 - p_FP1)` for segment one, and similarly for
+ `p_FC2` and `t`, with `0 ≤ s,t ≤ 1`. All points are measured and expressed in a
+ common frame `F`.
+
+ @param p_FP1 Segment 1's first point, P1.
+ @param p_FQ1 Segment 1's second point, Q1.
+ @param p_FP2 Segment 2's first point, P2.
+ @param p_FQ2 Segment 2's second point, Q2.
+ @param s The parameter relating C1 with P1 and Q1.
+ @param t The parameter relating C2 with P2 and Q2.
+ @param p_FC1 The point C1 on segment 1, nearest segment 2.
+ @param p_FC2 The point C2 on segment 2, nearest segment 1.
+ @return The squared distance between points C1 and C2.
+ @tparam S The scalar type for computation.
+ */
 template <typename S>
-FCL_EXPORT
-S closestPtSegmentSegment(
- Vector3<S> p1, Vector3<S> q1, Vector3<S> p2, Vector3<S> q2,
- S &s, S &t, Vector3<S> &c1, Vector3<S> &c2);
+FCL_EXPORT S closestPtSegmentSegment(const Vector3<S>& p_FP1,
+ const Vector3<S>& p_FQ1,
+ const Vector3<S>& p_FP2,
+ const Vector3<S>& p_FQ2, S* s, S* t,
+ Vector3<S>* p_FC1, Vector3<S>* p_FC2);
 
-// 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

src/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.cpp

@@ -48,10 +48,12 @@ template
 double clamp(double n, double min, double max);
 
 //==============================================================================
-template
-double closestPtSegmentSegment(
- Vector3d p1, Vector3d q1, Vector3d p2, Vector3d q2,
- double &s, double& t, Vector3d &c1, Vector3d &c2);
+template double closestPtSegmentSegment(const Vector3d& p_FP1,
+ const Vector3d& p_FQ1,
+ const Vector3d& p_FP2,
+ const Vector3d& p_FQ2, double* s,
+ double* t, Vector3d* p_FC1,
+ Vector3d* p_FC2);
 
 //==============================================================================
 template