Fix "dir" in GJKDistance function

[hongkai-dai]

The variable dir is overloaded in GJKDistance function, that it means both the point within the simplex that is closest to the origin, and the directional vector along which to compute the support mapping. One issue caused by this overload, is that when calling extractClosestPoint, we should pass in the closest point, but instead passed in the directional vector. In this PR I introduce a separate variable closest_p to represent the closest point.

---

This change is 

[sherm1]

Is this ready for review, @hongkai-dai ?

[hongkai-dai]

Yes it is ready for review.

The error on Mac CI is likely due to libccd being compiled with single precision

[SeanCurtis-TRI]

@sherm1 Want to split feature/platform review?

---

Review status: 0 of 7 files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

Comments from Reviewable

[SeanCurtis-TRI]

+@SeanCurtis-TRI for feature review

I really like the directory structure in the test directory for unit tests. Thanks.

First pass done.

---

Reviewed 7 of 7 files at r1.
Review status: all files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1330 at r1 (raw file):

/// change the libccd distance to add two closest points

BTW META: Based on the work you did, could you provide a function-level documentation for this function (and the others you touched to a sufficient degree to feel confident in your ability to document it)? You had to gain that understanding, it would be nice if future generations could benefit from that.

---

test/test_fcl_sphere_sphere.cpp, line 51 at r1 (raw file):

// For two spheres 1, 2, sphere 1 has radius1, and is centered at point A, with
// coordinate p_FA in some frame F; sphere 2 has radius2, and is centered at
// point B, with coordinate p_FB in the same frame F. Compute the (signed)

BTW "Compute the (optionally signed) distance..."

---

test/test_fcl_sphere_sphere.cpp, line 85 at r1 (raw file):

    S radius1, S radius2, const fcl::Vector3<S>& p_FA,
    const fcl::Vector3<S>& p_FB, fcl::GJKSolverType solver_type,
    bool enable_nearest_points, bool enable_signed_distance,

BTW Neither of these booleans appear to be anything other than true in this test. (Applies across this entire file).

---

test/test_fcl_sphere_sphere.cpp, line 103 at r1 (raw file):

  const S radius2 = 0.6;
  const fcl::Vector3<S> p_FA(0, 0, 0);
  const fcl::Vector3<S> p_FB(1.2, 0, 0);

BTW It might be worth creating arrays of relative positions. Right now, one at the origin and one displaced along a single axis may be less exhausting than we'd really want.

---

test/test_fcl_sphere_sphere.cpp, line 105 at r1 (raw file):

  const fcl::Vector3<S> p_FB(1.2, 0, 0);

  const fcl::Vector3<S> p1_expected(0.5, 0, 0);

FYI If you expressed these in terms of your parameters it would be less brittle.

Then you could position the spheres in non-axis-aligned positions and still get the right answers.

---

test/test_fcl_sphere_sphere.cpp, line 106 at r1 (raw file):

  const fcl::Vector3<S> p1_expected(0.5, 0, 0);
  const fcl::Vector3<S> p2_expected(-0.6, 0, 0);

BTW Also, given notation p_FA, you should also do p_ANa_expected and p_BNb_expected where Na and Nb are the nearest points on spheres A and B, respectively. That makes them the position vectors from the sphere origin to its nearest point, measured and expressed in the sphere's frame.

Ultimately, what I really want is a clear statement that the answers are in the sphere's local frames.

---

test/test_fcl_sphere_sphere.cpp, line 107 at r1 (raw file):

  const fcl::Vector3<S> p1_expected(0.5, 0, 0);
  const fcl::Vector3<S> p2_expected(-0.6, 0, 0);
  for (const fcl::GJKSolverType solver_type :

BTW A lot of the other tests in fcl separate the solvers into different unit tests. Without good reason, I'd propose we do the same.

---

test/test_fcl_sphere_sphere.cpp, line 121 at r1 (raw file):

}

GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres) {

BTW It should be more clear that the tests you've created are only testing signed distance.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 67 at r1 (raw file):

    tf1.translation() = center1;
    tf2.translation() = center2;
    void* o1 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s1, tf1);

BTW These two lines constitute a memory leak; there should be an accompanying invocation of: GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 89 at r1 (raw file):
BTW You've got two typos and some slightly awkward grammar. So, I simply propose reworking the error:

This test cannot be used for spheres whose centers are coincident.

You might also consider doing a:

GTEST_FAIL() << "Sphere centers are functionally coincident:"
             << \n\tp_FA: " << p_FA.transpose()
             << \n\tp_FB: " << p_FB.transpose();

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 100 at r1 (raw file):

  };

  CheckSignedDistance(radius1, radius2, center1, center2, tol);

BTW The value of swapping the order of the spheres in this test isn't worth as much as in test_sphere_sphere.cc. The reason is that the test doesn't confirm that these both produce the same answer, only that the internals of CheckSignedDistance are consistent.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 107 at r1 (raw file):

template <typename S>
void TestNonCollidingSphereGJKSignedDistance(S tol) {
  TestSphereToSphereGJKSignedDistance<S>(0.5, 0.5, Vector3<S>(0, 0, 0),

BTW As I commented on the other tests, I'd like to see an array of scenarios rather than just this one.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 114 at r1 (raw file):

GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) {
  TestNonCollidingSphereGJKSignedDistance<double>(1E-14);

BTW you can use constants<S>::eps_78() or constants<S>::eps() (probably the former) instead of these hard-coded values. Then it will equally apply if S is ever AutoDiffXd.

In fact, you could hard-code that tolerance inside the test rather than passing it in here.

---

Comments from Reviewable

[hongkai-dai]

Review status: all files reviewed at latest revision, 13 unresolved discussions, some commit checks failed.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1330 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW META: Based on the work you did, could you provide a function-level documentation for this function (and the others you touched to a sufficient degree to feel confident in your ability to document it)? You had to gain that understanding, it would be nice if future generations could benefit from that.

Done. Good call.

---

test/test_fcl_sphere_sphere.cpp, line 51 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "Compute the (optionally signed) distance..."

Done.

---

test/test_fcl_sphere_sphere.cpp, line 85 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Neither of these booleans appear to be anything other than true in this test. (Applies across this entire file).

That is true. By setting both these booleans to true, I test the function _ccdDist in gjk_libccd-inl.h, which contains the bug being fixed in this PR. I add a TODO here, to test with these booleans to false. I will add the test cases in the future PR.

---

test/test_fcl_sphere_sphere.cpp, line 103 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW It might be worth creating arrays of relative positions. Right now, one at the origin and one displaced along a single axis may be less exhausting than we'd really want.

Done. Good call.

I added more tests. But the precision for the new tests are not as good as 1E-14. They are now about 1E-3.

---

test/test_fcl_sphere_sphere.cpp, line 106 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Also, given notation p_FA, you should also do p_ANa_expected and p_BNb_expected where Na and Nb are the nearest points on spheres A and B, respectively. That makes them the position vectors from the sphere origin to its nearest point, measured and expressed in the sphere's frame.

Ultimately, what I really want is a clear statement that the answers are in the sphere's local frames.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 107 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW A lot of the other tests in fcl separate the solvers into different unit tests. Without good reason, I'd propose we do the same.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 121 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW It should be more clear that the tests you've created are only testing signed distance.

Done, I added the test with enable_signed_distance = false.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 67 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW These two lines constitute a memory leak; there should be an accompanying invocation of: GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);.

Done. Good catch!

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 89 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW You've got two typos and some slightly awkward grammar. So, I simply propose reworking the error:

This test cannot be used for spheres whose centers are coincident.

You might also consider doing a:

GTEST_FAIL() << "Sphere centers are functionally coincident:"
             << \n\tp_FA: " << p_FA.transpose()
             << \n\tp_FB: " << p_FB.transpose();

Done. Thanks

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 100 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW The value of swapping the order of the spheres in this test isn't worth as much as in test_sphere_sphere.cc. The reason is that the test doesn't confirm that these both produce the same answer, only that the internals of CheckSignedDistance are consistent.

Without the bug fix on _ccdDist, one of these checks would fail. So I think it still makes sense to test both orders here?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 107 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW As I commented on the other tests, I'd like to see an array of scenarios rather than just this one.

Done.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 114 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW you can use constants<S>::eps_78() or constants<S>::eps() (probably the former) instead of these hard-coded values. Then it will equally apply if S is ever AutoDiffXd.

In fact, you could hard-code that tolerance inside the test rather than passing it in here.

I pass it in here as I thought the tolerance would depend on the type of S. Actually with more tests, the tolerance drops to 1E-3.

---

Comments from Reviewable

[sherm1]

with a few BTWs, ptal.

---

Reviewed 4 of 7 files at r1, 3 of 3 files at r2.
Review status: all files reviewed at latest revision, 8 unresolved discussions, some commit checks failed.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1160 at r2 (raw file):

// the distance. For more info about GJK algorithm, the readers can refer to 
// section 2.3 of Real-time Collision Detection with Implicit Objects by Leif
// Olvang, 2010

FYI you could use backticks for variables rather than @p -- somewhat easier to read and would also work in Doxygen. (Irrelevant for this non-doxygen comment)

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1171 at r2 (raw file):

                        // origin.
  ccd_vec3_t dir; // direction vector
  ccd_real_t dist, last_dist = CCD_REAL_MAX;

BTW consider moving these declarations down to just prior to their use and in the loop scope where possible. I had trouble figuring out what was used where by scanning the code. I believe iterations is just a local loop variable so can be moved to the for statement. Looks like only closest_p needs to be defined at the top?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1350 at r2 (raw file):

    return -CCD_ONE;

  return _ccdDist(obj1, obj2, ccd, &simplex, p1, p2);

BTW so great to see all this duplicate code deleted! Thanks!

---

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

 */

/** @author Hongkai Dai <hongkai.dai@tri.global>*/

FYI could add a space before closing comment.

---

test/test_fcl_sphere_sphere.cpp, line 60 at r2 (raw file):

                              bool enable_signed_distance,
                              fcl::DistanceResult<S>* result) {
  fcl::Transform3<S> X_FA, X_FB;

BTW consider a brief explanation of this notation and a link to Drake's documentation for it: http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html

Likely FCL programmers are not familiar with it yet.

---

test/test_fcl_sphere_sphere.cpp, line 180 at r2 (raw file):

GTEST_TEST(FCL_SPHERE_SPHERE, Separating_Spheres_LIBCCD) {
  TestSeparatingSpheres<double>(1E-3, fcl::GJKSolverType::GST_LIBCCD);

BTW Yikes! Is that really the best we can hope for with libccd?

---

Comments from Reviewable

[SeanCurtis-TRI]

Second pass

---

Reviewed 1 of 3 files at r2.
Review status: all files reviewed at latest revision, 7 unresolved discussions, some commit checks failed.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1157 at r2 (raw file):
BTW I have a bunch of concerns about this documentation

1. p1 and p2 are expressed in which frame? (Comment applies below as well).
2. The return value is undocumented
3. The relationship between the objects and the simplex isn't explained.
4. The semantics of the simplex isn't clear -- is it properly initialized upon invocation? Or is it an output value?

I know it's asking a lot, but if this were elevated to a Drake-quality piece of documentation, we'd all benefit.

Something along the lines of:

Computes the distance between two non-penetrating convex objects, obj1 and obj2 based on the warm-started witness simplex, returning the distance and nearest points on each object.
@param obj1 The first convex object.
@param obj2 The second convex object.
@param ccd The libccd configuration.
@param simplex A witness to the objects' separation generated by the GJK algorithm. NOTE: the simplex is not necessarily sufficiently refined to report the actual distance and may be further refined in this method.
@param p1 If the objects are non-penetrating, the point on the surface of obj1 closest to obj2 (expressed in the world frame).
@param p2 If the objects are non-penetrating, the point on the surface of obj2 closest to obj1 (expressed in the world frame).
@returns The minimum distance between the two objects. If they are penetrating, -1 is returned.

(With appropriate // injected at appropriate line width, of course.)

Unknowns: What happens if the simplex is not initialized? Or it's lying?

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1339 at r2 (raw file):

// Computes the (unsigned) distance between two geometric objects @p obj1 and @p

BTW I'd modify this to reflect the comment on the other function. The difference between the two is that this does the warm starting of the libccd GJK implementation.

---

test/test_fcl_sphere_sphere.cpp, line 49 at r2 (raw file):

#include "fcl/narrowphase/distance_result.h"

// For two spheres 1, 2, sphere 1 has radius1, and is centered at point A, with

FYI I just realized: if you had spheres A and B and radius_A and radius_B, that makes p_FB and p_FA fit better.

---

test/test_fcl_sphere_sphere.cpp, line 116 at r2 (raw file):

    min_distance = (sphere1.p_FC - sphere2.p_FC).norm() - sphere1.radius -
                   sphere2.radius;
    const fcl::Vector3<S> AB = (sphere1.p_FC- sphere2.p_FC).normalized();

BTW missing space before - operator.

---

test/test_fcl_sphere_sphere.cpp, line 117 at r2 (raw file):

                   sphere2.radius;
    const fcl::Vector3<S> AB = (sphere1.p_FC- sphere2.p_FC).normalized();
    p_S1P1 = -AB * sphere1.radius;

FYI slightly more efficient to negate the radius. But yours reads better. :)

---

test/test_fcl_sphere_sphere.cpp, line 123 at r2 (raw file):

  SphereSpecification<S> sphere2;
  S min_distance;
  // The closest points are expressed in the sphere body frame.

BTW These three comments are slightly redundant. The first one includes the next two. So, I'd suggest one of two options:

1. Delete the last two comments, or
2. delete the first comment and then move the last comment to between the two Vector declarations (i.e., comment, object, comment, object.)

---

test/test_fcl_sphere_sphere.cpp, line 143 at r2 (raw file):

    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
      SphereSphereDistance<S> sphere_sphere_distance(spheres[i], spheres[j]);
      if (sphere_sphere_distance.min_distance > 0) {

BTW This is a dangerous test -- this should be a ASSERT_GT(min_distance, 0). Otherwise, the spheres could be changed to be in collision and no testing would take place. Obviously, this will require you to guarantee that none of the sphere pairs are colliding. :)

---

test/test_fcl_sphere_sphere.cpp, line 180 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW Yikes! Is that really the best we can hope for with libccd?

BTW Not all of the tests fail at this precision -- I'd like a note/todo indicating why this value is so high and which combination of spheres is ending so badly.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 100 at r1 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Without the bug fix on _ccdDist, one of these checks would fail. So I think it still makes sense to test both orders here?

Refined f2f

---

Comments from Reviewable

[hongkai-dai]

Review status: 4 of 7 files reviewed at latest revision, 14 unresolved discussions.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1157 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I have a bunch of concerns about this documentation

1. p1 and p2 are expressed in which frame? (Comment applies below as well).
2. The return value is undocumented
3. The relationship between the objects and the simplex isn't explained.
4. The semantics of the simplex isn't clear -- is it properly initialized upon invocation? Or is it an output value?

I know it's asking a lot, but if this were elevated to a Drake-quality piece of documentation, we'd all benefit.

Something along the lines of:

Computes the distance between two non-penetrating convex objects, obj1 and obj2 based on the warm-started witness simplex, returning the distance and nearest points on each object.
@param obj1 The first convex object.
@param obj2 The second convex object.
@param ccd The libccd configuration.
@param simplex A witness to the objects' separation generated by the GJK algorithm. NOTE: the simplex is not necessarily sufficiently refined to report the actual distance and may be further refined in this method.
@param p1 If the objects are non-penetrating, the point on the surface of obj1 closest to obj2 (expressed in the world frame).
@param p2 If the objects are non-penetrating, the point on the surface of obj2 closest to obj1 (expressed in the world frame).
@returns The minimum distance between the two objects. If they are penetrating, -1 is returned.

(With appropriate // injected at appropriate line width, of course.)

Unknowns: What happens if the simplex is not initialized? Or it's lying?

Done. Thanks a lot for writing this clear documentation!

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1160 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

FYI you could use backticks for variables rather than @p -- somewhat easier to read and would also work in Doxygen. (Irrelevant for this non-doxygen comment)

Done.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1171 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider moving these declarations down to just prior to their use and in the loop scope where possible. I had trouble figuring out what was used where by scanning the code. I believe iterations is just a local loop variable so can be moved to the for statement. Looks like only closest_p needs to be defined at the top?

Done. I think last_dist needs to be defined at the top, since it is being updated in every iteration within the for loop.

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1339 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I'd modify this to reflect the comment on the other function. The difference between the two is that this does the warm starting of the libccd GJK implementation.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 105 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

FYI If you expressed these in terms of your parameters it would be less brittle.

Then you could position the spheres in non-axis-aligned positions and still get the right answers.

I think now with SphereSpecification, I can freely put the sphere anywhere?

---

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

Previously, sherm1 (Michael Sherman) wrote…

FYI could add a space before closing comment.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 49 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

FYI I just realized: if you had spheres A and B and radius_A and radius_B, that makes p_FB and p_FA fit better.

I am hesitant to do that, since the letter A will be overloaded, to mean both the sphere and the center of the sphere (as used in p_FA)

---

test/test_fcl_sphere_sphere.cpp, line 60 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider a brief explanation of this notation and a link to Drake's documentation for it: http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html

Likely FCL programmers are not familiar with it yet.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 116 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW missing space before - operator.

Done.

---

test/test_fcl_sphere_sphere.cpp, line 123 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW These three comments are slightly redundant. The first one includes the next two. So, I'd suggest one of two options:

1. Delete the last two comments, or
2. delete the first comment and then move the last comment to between the two Vector declarations (i.e., comment, object, comment, object.)

Done.

---

test/test_fcl_sphere_sphere.cpp, line 143 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This is a dangerous test -- this should be a ASSERT_GT(min_distance, 0). Otherwise, the spheres could be changed to be in collision and no testing would take place. Obviously, this will require you to guarantee that none of the sphere pairs are colliding. :)

Done. Good point!

---

test/test_fcl_sphere_sphere.cpp, line 180 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Not all of the tests fail at this precision -- I'd like a note/todo indicating why this value is so high and which combination of spheres is ending so badly.

Done. All pairs of combination are bad.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 100 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Refined f2f

Done.

---

Comments from Reviewable

[sherm1]

Reviewed 3 of 3 files at r3.
Review status: all files reviewed at latest revision, 7 unresolved discussions, some commit checks failed.

---

test/test_fcl_sphere_sphere.cpp, line 156 at r2 (raw file):

            sphere_sphere_distance.min_distance, sphere_sphere_distance.p_S2P2,
            sphere_sphere_distance.p_S1P1, tol);
        

BTW some odd characters at beginning of line here?

---

Comments from Reviewable

[SeanCurtis-TRI]

It's worth noting that this PR seems to have fixed the persistent mac CI failure we've had. That's outstanding! Now I just have to fix the box-box test so that it passes in the mac release.

---

Reviewed 2 of 3 files at r3.
Review status: all files reviewed at latest revision, 2 unresolved discussions, some commit checks failed.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 47 at r3 (raw file):

// Given two spheres, sphere 1 has radius1, and centered at point A, whose
// position is p_FA measured and expressed in frame A; sphere 2 has radius2,

BTW measured and expressed in frame F? Certainly, according to Drake's monogram notation p_FA is an abbreviation for p_FA_F.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 126 at r3 (raw file):

  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
      if ((spheres[i].center - spheres[j].center).norm() >

BTW This is a repeat of the other test's issue -- if someone changes the spheres such that they collide, no tests will be executed for that pair and the test will meaninglessly pass.

---

Comments from Reviewable

[SeanCurtis-TRI]

Reviewed 1 of 3 files at r3.
Review status: all files reviewed at latest revision, 4 unresolved discussions, some commit checks failed.

---

Comments from Reviewable

[hongkai-dai]

Review status: all files reviewed at latest revision, 4 unresolved discussions, some commit checks failed.

---

test/test_fcl_sphere_sphere.cpp, line 117 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

FYI slightly more efficient to negate the radius. But yours reads better. :)

Done.

---

test/test_fcl_sphere_sphere.cpp, line 156 at r2 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW some odd characters at beginning of line here?

Hmm I do not see them, maybe a reviewable glitch?

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 47 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW measured and expressed in frame F? Certainly, according to Drake's monogram notation p_FA is an abbreviation for p_FA_F.

Done. Good catch.

---

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 126 at r3 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This is a repeat of the other test's issue -- if someone changes the spheres such that they collide, no tests will be executed for that pair and the test will meaninglessly pass.

Done. Good catch

---

Comments from Reviewable

[SeanCurtis-TRI]

Reviewed 2 of 2 files at r4.
Review status: all files reviewed at latest revision, 1 unresolved discussion.

---

Comments from Reviewable

[sherm1]

Reviewed 2 of 2 files at r4.
Review status: all files reviewed at latest revision, all discussions resolved, some commit checks failed.

---

Comments from Reviewable