Fixing capsule-capsule distance

[SeanCurtis-TRI]

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.

fixes #225

---

This change is 

[SeanCurtis-TRI]

@avalenzu you up for a quick turn around feature review of this? Or shall I find someone over here?

Reviewable status: 0 of 3 files reviewed, all discussions resolved

[avalenzu]

+@avalenzu with a few minor comments on comments.

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

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 65 at r1 (raw file):

/** Computes the signed distance between two capsules `s1` and `s2` (with
 given poses `tf1` and `tf2` of the two capsules in a common frame `F`). Further

nit: These are X_FC1 and X_FC2 now, right?

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 160 at r1 (raw file):

  // The capsules are defined centered on the origin of their canonical frame C
  // and with the central line aligned with Cz. So, the two end points are at

BTW, It's ambiguous from the text whether you mean the end points of the line segment that (together with the radius) defines the capsule or the end points of the capsule itself (i.e where Cz intersects the boundary of the capsule.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 201 at r1 (raw file):

    vhat_C1C2_F = (p_FN2 - p_FN1) / segment_dist;
  } else {
    // The points are too close. The center lines intersect. We have to cases:

nit: s/to/two

---

test/test_fcl_capsule_capsule.cpp, line 281 at r1 (raw file):

  }

  // Case: Single solution: not-linear but non-overlapping such that (p1, q0)

nit: I don't think "not-linear" is what you want here :-p Consider replacing with "neither co-linear nor overlapping such that ..."

---

test/test_fcl_capsule_capsule.cpp, line 312 at r1 (raw file):

  // Case: Single solution: not-linear but non-overlapping such that (p0, q1)
  // are the nearest.

nit: See comment above about "not-linear".

---

test/test_fcl_capsule_capsule.cpp, line 549 at r1 (raw file):

           ○○○               ○○○

        Separated        Intersecting       Single overlap    Multiple overlap

So pretty!

---

test/test_fcl_capsule_capsule.cpp, line 557 at r1 (raw file):

  Single overlap:   The capsule center lines intersect so there is no unique
                    witness pair to the negative signed distance.
  Multiple overlap: The capsule center lines overlap over an internal, so there

nit: s/internal/interval

[SeanCurtis-TRI]

Comments addressed. As well as CI failures.

+@sherm1 for platform review, please.

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

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 65 at r1 (raw file):

Previously, avalenzu (Andres Valenzuela) wrote…

nit: These are X_FC1 and X_FC2 now, right?

Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 160 at r1 (raw file):

Previously, avalenzu (Andres Valenzuela) wrote…

BTW, It's ambiguous from the text whether you mean the end points of the line segment that (together with the radius) defines the capsule or the end points of the capsule itself (i.e where Cz intersects the boundary of the capsule.

Done (I believe).

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 201 at r1 (raw file):

Previously, avalenzu (Andres Valenzuela) wrote…

nit: s/to/two

Done

---

test/test_fcl_capsule_capsule.cpp, line 281 at r1 (raw file):

Previously, avalenzu (Andres Valenzuela) wrote…

nit: I don't think "not-linear" is what you want here :-p Consider replacing with "neither co-linear nor overlapping such that ..."

Done

---

test/test_fcl_capsule_capsule.cpp, line 549 at r1 (raw file):

Previously, avalenzu (Andres Valenzuela) wrote…

So pretty!

Thanks :)

[sherm1]

Checkpoint.

Reviewed 3 of 4 files at r2.
Reviewable status: 3 of 4 files reviewed, 9 unresolved discussions (waiting on @avalenzu and @SeanCurtis-TRI)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 80 at r2 (raw file):

 @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.

nit: missing ")"

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 113 at r2 (raw file):

      // The general nondegenerate case starts here
      S b = d1.dot(d2);
      S denom = a*e-b*b; // Always nonnegative

minor: consider assert(denom >= 0) here to call the comment's bluff? It's hard for me to believe this could never come out -eps ish. Or maybe this should be denom = max(0, a*e-b*b) to clean up teeny negative noise?

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 155 at r2 (raw file):

  assert(p_FW2 != nullptr);

  // Origin' of the capsules' frames relative to F's origin.

nit: spurious single quote

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 157 at r2 (raw file):

  // Origin' of the capsules' frames relative to F's origin.
  Vector3<S> p_FC1o = X_FC1.translation();
  Vector3<S> p_FC2o = X_FC2.translation();

minor: these can be const Vector3<S>& I think

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 162 at r2 (raw file):

  // 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

minor: awkward to have "z" mean two different things here. I'd call the first one z+ and the second z-.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 163 at r2 (raw file):

  // 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.

minor: should say which rotation matrix (R_FC here)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 164 at r2 (raw file):

  // `z = -lz / 2 * Cz`, respectively. Cz_F is simply the third column of the
  // rotation matrix.
  auto calc_half_arm = [](const Capsule<S>& c,

minor: with the revised notation you can now say that "half arm" is defined to be the vector from Co to z+.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 168 at r2 (raw file):

    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;

BTW more common to write s*v for scalar-vector product.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 173 at r2 (raw file):

  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;

nit: const (and below)
Also, consider end of line comments // z+ and // z- if you liked my notation suggestion above. You could instead use p_FC1a and pFC1b in the comment rather than the z's. (You could also consider p_FC1p and pFC1m for "plus" and "minus" -- slightly more information content than "a" and "b".)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 191 at r2 (raw file):

  //  streamlined so there is less waste.
  S squared_dist = closestPtSegmentSegment(p_FC1a, p_FC1b, p_FC2a, p_FC2b, s, t,
                                           p_FN1, p_FN2);

BTW consider all outputs on 2nd line with // outputs. A little hard to infer without the GSG convention.
Also, const square_dist ?

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 211 at r2 (raw file):

    //        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);

nit: consider a little helper function like zaxis(X) that imbues this ugly Eigen magic with some beauty and meaning. (Also Rule of >3)

---

test/test_fcl_capsule_capsule.cpp, line 35 at r2 (raw file):

 */

/** @author Sean Curtis <sean@tri.global> */

BTW oh do you really have that nice alias? I've been using sean.curtis@tri.global.

[sherm1]

Platform review pass done. Just one surprising thing to resolve below, ptal.

Reviewed 1 of 4 files at r2.
Reviewable status: all files reviewed, 18 unresolved discussions (waiting on @SeanCurtis-TRI)

---

test/test_fcl_capsule_capsule.cpp, line 549 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Thanks :)

👍

---

test/test_fcl_capsule_capsule.cpp, line 87 at r2 (raw file):

  // We exploit the knowledge that the segment-segment function uses an
  // epsilon of 1e-3 on *squared distance*. This test will fail if we change

Wait, what? That means the distance tolerance is .03 -- 3 cm?? How can that be right?

---

test/test_fcl_capsule_capsule.cpp, line 106 at r2 (raw file):

  // between points p0 and p1. All that we care is that the distance is no
  // longer *exactly* the distance between p0 and p1. Thus it shows that we've
  // crossed the algorithms threshold.

nit algorithm's

---

test/test_fcl_capsule_capsule.cpp, line 108 at r2 (raw file):

  // crossed the algorithms threshold.
  {
    // Just below the epsilon for squared distance.

minor: below -> above

---

test/test_fcl_capsule_capsule.cpp, line 169 at r2 (raw file):

  }

  // Case: Second zero-length segment is nearest q0.

nit q0 -> q1

---

test/test_fcl_capsule_capsule.cpp, line 198 at r2 (raw file):

  const S eps = constants<S>::eps_78();

  // A line that all of the segments will be parallel with.

nit: "A line to which all the other segments will be parallel."

---

test/test_fcl_capsule_capsule.cpp, line 209 at r2 (raw file):

  //   1. The distance between the pairs should be the expected distance.
  //   2. We can determine that the nearest point lies in the correct region
  //      of one of the two segments.

BTW it would be nice to make this deterministic (not necessarily in this PR!). When two line segments are parallel with overlapping projections: choose the midpoint of the overlap region on each segment as the nearest points. When parallel and non-overlapping the solution is unique (an end point from each segment). Examples:

-------------==*==-----      ----------===*===
             ==*==                     ===*===----------


        ------------*
                          *----------------------

---

test/test_fcl_capsule_capsule.cpp, line 216 at r2 (raw file):

    const Vector3<S> p0(-dir);
    const Vector3<S> q0(dir);
    // Segment 2 includes the origin and passes through q1 extending a similar

nit: 2 -> 1

---

test/test_fcl_capsule_capsule.cpp, line 281 at r2 (raw file):

  }

  // Case: Single solution: co-linear but non-overlapping such that (p1, q0)

minor: this says "co-linear" but I think these lines are just parallel?

---

test/test_fcl_capsule_capsule.cpp, line 311 at r2 (raw file):

  }

  // Case: Single solution: co-linear but non-overlapping such that (p0, q1)

minor: co-linear or just parallel?

---

test/test_fcl_capsule_capsule.cpp, line 401 at r2 (raw file):

    const Vector3<S> q1_shift = q1 + offset;
    Vector3<S> expected_N1;
    S expected_t;

BTW stupid Reviewable highlights expected_t like a keyword since likely it thinks that's a typename (since "_t" is a sometimes-conventional way of naming types, e.g. size_t). Consider t_expected to avoid trouble?

---

test/test_fcl_capsule_capsule.cpp, line 512 at r2 (raw file):

    EXPECT_TRUE(CompareMatrices(this->n1_, p1_shift, eps));
  }
}

BTW Wow! That is an amazing set of tests -- I have a lot of confidence that this code works if it can make it through those. (And I'll be so disillusioned if a bug squeaks through anyway!)

---

test/test_fcl_capsule_capsule.cpp, line 533 at r2 (raw file):

    kMultipleOverlap
  };
  /* Configure the scene such that neither transform is even near an identity

nit: "even" or "ever"?

---

test/test_fcl_capsule_capsule.cpp, line 739 at r2 (raw file):

TYPED_TEST(CapsuleCapsuleSegmentTest, OverlappingCenterLines) {
  EXPECT_TRUE(this->RunTestConfiguration(
      CapsuleCapsuleSegmentTest<TypeParam>::kMultipleOverlap));

BTW this capsule/capsule test is masterful!

[SeanCurtis-TRI]

Comments addressed. Resolving CI issues independently.

Reviewable status: 1 of 5 files reviewed, 4 unresolved discussions (waiting on @avalenzu and @sherm1)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 113 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: consider assert(denom >= 0) here to call the comment's bluff? It's hard for me to believe this could never come out -eps ish. Or maybe this should be denom = max(0, a*e-b*b) to clean up teeny negative noise?

Is this a question of numerical accuracy or math?

Mathematically, it is definitely true. a = |d1|², e = |d2|², b = d1⋅d2. So, |d1|²|d2|² >= (d1⋅d2)² = |d1|²|d2|²(d̂₁⋅ d̂₂) by definition.

Numerically, it's certainly conceivable that we might get a microscopic

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 164 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: with the revised notation you can now say that "half arm" is defined to be the vector from Co to z+.

While I think that's correct, I think it harms the readability of this particular code. You get stuff like this:

const Vector 3<S> p_C1oZplus = calc_p_C1Zplus(s1, X_FC1);
const Vector3s<S> p_FC1a = p_FC1o + p_C1oZplus;
const Vector3s<S> p_FC1b = p_FC1o - p_C1oZplus;

If it weren't for the plus or and the injection of the capsule enumeration, I'd be all in. But with those extra qualifiers, I think it becomes harder to reason about.

If you feel strongly about it or have an alternate suggestion for names, I'm open to that as well.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 191 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider all outputs on 2nd line with // outputs. A little hard to infer without the GSG convention.
Also, const square_dist ?

Done

Re: "outputs". My first thought was to leave as much of the code intact as possible. However, I clearly didn't do that with the test. Since the scope of this function is limited to capsule-capsule intersection, I figured I might was well finish cleaning it up to match GSG.

---

test/test_fcl_capsule_capsule.cpp, line 35 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW oh do you really have that nice alias? I've been using sean.curtis@tri.global.

Part of the wonderful 2016 legacy. :)

---

test/test_fcl_capsule_capsule.cpp, line 87 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Wait, what? That means the distance tolerance is .03 -- 3 cm?? How can that be right?

Resolved
Yeah... I didn't like that either, but my original goal was to leave the segment function alone. However, your shock is well taken.

---

test/test_fcl_capsule_capsule.cpp, line 169 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit q0 -> q1

No, q0 is correct.

---

test/test_fcl_capsule_capsule.cpp, line 209 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW it would be nice to make this deterministic (not necessarily in this PR!). When two line segments are parallel with overlapping projections: choose the midpoint of the overlap region on each segment as the nearest points. When parallel and non-overlapping the solution is unique (an end point from each segment). Examples:

-------------==*==-----      ----------===*===
             ==*==                     ===*===----------


        ------------*
                          *----------------------

Agreed; I've tweaked the comment a little. (I realized it was inconsistent with some tests I'd written lower in the file.)

---

test/test_fcl_capsule_capsule.cpp, line 401 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW stupid Reviewable highlights expected_t like a keyword since likely it thinks that's a typename (since "_t" is a sometimes-conventional way of naming types, e.g. size_t). Consider t_expected to avoid trouble?

Done

---

test/test_fcl_capsule_capsule.cpp, line 512 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW Wow! That is an amazing set of tests -- I have a lot of confidence that this code works if it can make it through those. (And I'll be so disillusioned if a bug squeaks through anyway!)

I was so suspicious of the code, that I wrote these tests defensively.

---

test/test_fcl_capsule_capsule.cpp, line 739 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW this capsule/capsule test is masterful!

Thanks

[SeanCurtis-TRI]

Reviewable status: 1 of 5 files reviewed, 3 unresolved discussions (waiting on @avalenzu and @sherm1)

---

test/test_fcl_capsule_capsule.cpp, line 87 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Resolved
Yeah... I didn't like that either, but my original goal was to leave the segment function alone. However, your shock is well taken.

Done.

[sherm1]

Thanks for the revision! I am especially relieved that we do better than 3cm tolerance :) Platform with a few minor doc suggestions.

Reviewed 4 of 4 files at r3.
Reviewable status: all files reviewed, 7 unresolved discussions (waiting on @SeanCurtis-TRI)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 59 at r3 (raw file):

 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

BTW consider promising that 0 ≤ s,t ≤ 1.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 73 at r3 (raw file):

 @return    The squared distance between points C1 and C2.
 @tparam S  The scalar type for computation.
 */

BTW thanks for adding that nice documentation!

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 113 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Is this a question of numerical accuracy or math?

Mathematically, it is definitely true. a = |d1|², e = |d2|², b = d1⋅d2. So, |d1|²|d2|² >= (d1⋅d2)² = |d1|²|d2|²(d̂₁⋅ d̂₂) by definition.

Numerically, it's certainly conceivable that we might get a microscopic

Numerical, fixed now.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 164 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

While I think that's correct, I think it harms the readability of this particular code. You get stuff like this:

const Vector 3<S> p_C1oZplus = calc_p_C1Zplus(s1, X_FC1);
const Vector3s<S> p_FC1a = p_FC1o + p_C1oZplus;
const Vector3s<S> p_FC1b = p_FC1o - p_C1oZplus;

If it weren't for the plus or and the injection of the capsule enumeration, I'd be all in. But with those extra qualifiers, I think it becomes harder to reason about.

If you feel strongly about it or have an alternate suggestion for names, I'm open to that as well.

I wasn't proposing to change the variable names here -- just saying that the comment could now explain that calc_half_arm() returns the Co to z+ vector rather than the Co to z- vector.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 89 at r3 (raw file):

  Vector3<S> p_P1Q1 = p_FQ1 - p_FP1; // Direction vector of segment S1.
  Vector3<S> p_P2Q2 = p_FQ2 - p_FP2; // Direction vector of segment S2.
  Vector3<S> p_P2P1 = p_FP1 - p_FP2;

nit: conventionally "direction vector" implies a unit vector (because it has only directional information, not length). The vectors here all have meaningful length. Consider instead

// Vector from P1 to Q1.
// Vector from P2 to Q2.
// Vector from P2 to P1.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 93 at r3 (raw file):

  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);

BTW would be nice to have a comment for f also but I'm not sure what to call it.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 107 at r3 (raw file):

    *s = 0.0;
    *t = f / e; // t = (b * s + f) / e, s = 0 --> t = f / e.
    *t = clamp(*t, (S)0.0, (S)1.0);

nit: consider formatting this like the other case below to emphasize the parallel, e.g. *t = clamp(f / e, (S)0.0, (S)1.0);.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 109 at r3 (raw file):

    *t = clamp(*t, (S)0.0, (S)1.0);
  } else {
    S c = p_P1Q1.dot(p_P2P1);

btw could be const (a few more below also)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 131 at r3 (raw file):

      }
      // Compute point on L2 closest to S1(s) using
      // t = Dot((P1 + D1*s) - P2, D2) / Dot(D2,D2) = (b*s + f) / e

minor: D1, D2 leftover from some earlier notation?

---

test/test_fcl_capsule_capsule.cpp, line 87 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Done.

Whew!

---

test/test_fcl_capsule_capsule.cpp, line 169 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

No, q0 is correct.

btw for me, "Second zero-length segment is nearest end point of first segment" would have been clear.

---

test/test_fcl_capsule_capsule.cpp, line 164 at r3 (raw file):

  // The tests below use the same parameters as above, but we reverse the order
  // to test segment 2 against segment 1 in the same relative configurations.

nit maybe better to say "segment 1 against segment 0" to match the code below?

---

test/test_fcl_capsule_capsule.cpp, line 166 at r3 (raw file):

  // to test segment 2 against segment 1 in the same relative configurations.

  // Case: Second zero-length segment is nearest p0.

minor: per f2f avoid using different meanings for "p0" and other symbols. I'd be happy e.g. if this one said "Second zero-length segment is nearest start point of first segment."

[SeanCurtis-TRI]

I took a slightly different tack on the test documentation. PTAL

Reviewable status: 2 of 5 files reviewed, 3 unresolved discussions (waiting on @sherm1)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 59 at r3 (raw file):

0 ≤ s,t ≤ 1
Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule.h, line 73 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW thanks for adding that nice documentation!

Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 113 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Numerical, fixed now.

(that comment slipped through -- I started responding, distracted myself, changed the code and then forgot about the comment. :-/)

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 164 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

I wasn't proposing to change the variable names here -- just saying that the comment could now explain that calc_half_arm() returns the Co to z+ vector rather than the Co to z- vector.

Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 89 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: conventionally "direction vector" implies a unit vector (because it has only directional information, not length). The vectors here all have meaningful length. Consider instead

// Vector from P1 to Q1.
// Vector from P2 to Q2.
// Vector from P2 to P1.

OK I didn't like it at the time, but left it.

I think the monogram notation largely stands on its own (coupled with the simplicity of the math).

That said, I've modified the comment to reflect a subsequent note about D1 and D2 quantities.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 93 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW would be nice to have a comment for f also but I'm not sure what to call it.

That's one of the reasons I put in a TODO. :) I made a passing effort to figure out what those were, but I wasn't able to directly derive this math. That's a task for some other day.

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 107 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit: consider formatting this like the other case below to emphasize the parallel, e.g. *t = clamp(f / e, (S)0.0, (S)1.0);.

Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 109 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

btw could be const (a few more below also)

Done

---

include/fcl/narrowphase/detail/primitive_shape_algorithm/capsule_capsule-inl.h, line 131 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: D1, D2 leftover from some earlier notation?

OK

Nope. Those would be the "directions" (i.e., d1, d2).

I'm inclined to leave it alone until the TODO gets resolved and all of the math is meaningfully discussed with coherent symbols. I started tweaking it, but it got bigger without really becoming more helpful.

---

test/test_fcl_capsule_capsule.cpp, line 169 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

btw for me, "Second zero-length segment is nearest end point of first segment" would have been clear.

See new solution (I can describe it in terms of p_i, and the code literally matches the description.

---

test/test_fcl_capsule_capsule.cpp, line 164 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

nit maybe better to say "segment 1 against segment 0" to match the code below?

I've gone a different route. See below.

---

test/test_fcl_capsule_capsule.cpp, line 166 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: per f2f avoid using different meanings for "p0" and other symbols. I'd be happy e.g. if this one said "Second zero-length segment is nearest start point of first segment."

See alternate solution.

[sherm1]

Reviewed 3 of 3 files at r4.
Reviewable status: complete! all files reviewed, all discussions resolved

[sherm1]

Looks like this needs a manual merge or rebase.

Reviewable status: complete! all files reviewed, all discussions resolved

[SeanCurtis-TRI]

Yep. I merged the other PR without thinking about this one....

Reviewable status: complete! all files reviewed, all discussions resolved

[sherm1]

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

[SeanCurtis-TRI]

Another attempt to clean up windows CI.

Reviewable status: 4 of 5 files reviewed, all discussions resolved (waiting on @sherm1)

[SeanCurtis-TRI]

@sherm - waiting on you.

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

[sherm1]

Oh, I see that worked! Nice.

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