Add primitive cylinder-sphere collision and distance query

[SeanCurtis-TRI]

Add custom sphere-cylinder collision and distance tests

By default, the GJK solver was being used for performing distance queries between cylinders and spheres. For small features, the answer was being dominated by the iterative tolerance and producing wildly problematic values. The logical thing to do is to perform sphere-cylinder collisions using knowledge of the primitives.

This commit adds the following:

• A new test illustrating the error of GJK is used
  (see test_fcl_sphere_cylinder.cpp)
• Adds the custom sphere-cylinder collision/distance
  (sphere_cylinder.h and sphere_cylinder-inl.h)
• Adds extensive unit tests on the custom algorithm.
• Ties the custom algorithm into the libccd and indep GJK solvers.

NOTE: Not for review until after #316 merges.

---

This change is 

[SeanCurtis-TRI]

-(Status: on hold) Ready for review -- box-sphere has been merged.

+@amcastro-tri Sorry for the size. It's a bit larger than the box-sphere, but you should see a lot of familiar patterns. I'm going to submit an issue that will refactor the sphere-* tests into some common code so that they can all be tested with the same framework. But with just two instances, I'm letting the redundancy go.

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

[amcastro-tri]

First pass. Minor comments mostly

Reviewed 9 of 10 files at r1.
Reviewable status: 9 of 10 files reviewed, 15 unresolved discussions (waiting on @SeanCurtis-TRI and @amcastro-tri)

---

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

 the barrel is closer than the end face, but it doesn't provide a unique
 solution, the +x direction is assumed to be the contact direction (yielding a
 contact normal in the -x direction.)

nit: is there an implicit assumption here regarding the direction of say, the z axis , in the canonical frame?

---

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

  // in the cylinder frame C. If the center is inside the cylinder, this will
  // be the zero vector.
  Vector3<S> p_SN_C = p_CN - p_CS;

minor: const

---

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

    // lose bits of precision. For an arbitrary non-identity transform, 4 bits
    // is the maximum possible precision loss. So, we only consider a point to
    // be outside the box if it's distance is at least that epsilon.

minor: box->cylinder

---

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

    // than this epsilon closer to the sphere center (see the test in the
    // else branch).
    auto eps = 16 * constants<S>::eps();

minor: const

---

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

      // Distance from closest point (N) to sphere center (S).
      S d_NS = sqrt(p_SN_squared_dist);

minor: const.

---

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

      // Distance from closest point (N) to sphere center (S).
      S d_NS = sqrt(p_SN_squared_dist);

nit: do we need using std::sqrt for ADL to work?

---

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

      // (with preference for the end face).
      const S& h = cylinder.lz;
      S face_distance = p_CS(2) >= 0 ? h / 2 - p_CS(2) : p_CS(2) + h / 2;

nit: const

---

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

      // The direction from the sphere to the nearest point on the barrel on
      // the z = 0 plane.
      Vector2<S> n_SB_xy = Vector2<S>(p_CS(0), p_CS(1));

nit: const
here and below in other places within this scope for other variables.

---

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

      S d_CS_xy = n_SB_xy.norm();
      S barrel_distance = cylinder.radius - d_CS_xy;
      if (barrel_distance < face_distance - eps) {

Not sure if I follow. Is this eps needed here? why not just barrel_distance < face_distance in this if statement?

---

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

  if (S_is_outside) {
    // If N is not S, we know the sphere center is *outside* the box (but we

minor: box->cylinder

---

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

// This collides a cylinder with a sphere. The cylinder is disk-like with a
// large radius (r_c) and small height (h)c) and its geometric frame is aligned

minor: h_c

---

test/test_fcl_sphere_cylinder.cpp, line 90 at r1 (raw file):

// Properties of expected outcome:
//  - One contact *should* be reported,
//  - Penetration depth δ should be: r_s - (sz - h / 2),

minor: h--> h_c for consistency here and below.

---

test/test_fcl_sphere_cylinder.cpp, line 92 at r1 (raw file):

//  - Penetration depth δ should be: r_s - (sz - h / 2),
//  - Contact point should be at: [sx, sy, h / 2 - δ / 2], and
//  - Contact normal should be [0, 0, -1] (pointing from sphere into box).

search "box" withing this file.

---

test/test_fcl_sphere_cylinder.cpp, line 95 at r1 (raw file):

//
// NOTE: Orientation of the sphere should *not* make a difference and is not
// tested here; it relies on the sphere-box primitive algorithm unit tests to

sphere-cylinder

---

test/test_fcl_sphere_cylinder.cpp, line 120 at r1 (raw file):

  // Poses of the cylinder in the world frame.
  fcl::Transform3<S> X_WC = fcl::Transform3<S>::Identity();

nit: const

[SeanCurtis-TRI]

Comments addressed.

Reviewable status: 6 of 10 files reviewed, 4 unresolved discussions (waiting on @amcastro-tri and @SeanCurtis-TRI)

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: is there an implicit assumption here regarding the direction of say, the z axis , in the canonical frame?

Done

While I didn't fully understand your question, in re-reading my comment I felt I could make it clearer.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: const

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: box->cylinder

done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: const

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: const.

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: do we need using std::sqrt for ADL to work?

Not in this case. If I was actually invoking sdt::sqrt(p_SN_squared_distance then, yes, I would do the using and change to this spelling. But this spelling is already ADL-ready.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: const

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: const
here and below in other places within this scope for other variables.

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Not sure if I follow. Is this eps needed here? why not just barrel_distance < face_distance in this if statement?

1. It's the same as the condition in the sphere-box collision.
2. The reason is that if the center is near the Voronoi boundary, small perturbations in the bits would cause arbitrary selection of barrel over face (perturbations due, simply to rigid transformations). This provides insurance that the answer won't change just because the fixed relationship is evaluated in a different frame.

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: box->cylinder

Done

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: h_c

Done

---

test/test_fcl_sphere_cylinder.cpp, line 90 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: h--> h_c for consistency here and below.

Done

---

test/test_fcl_sphere_cylinder.cpp, line 92 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

search "box" withing this file.

Done

---

test/test_fcl_sphere_cylinder.cpp, line 95 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

sphere-cylinder

Done

---

test/test_fcl_sphere_cylinder.cpp, line 120 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: const

Done

[amcastro-tri]

Excellent! feature pass done.

Reviewed 1 of 10 files at r1.
Reviewable status: 7 of 10 files reviewed, 6 unresolved discussions (waiting on @amcastro-tri and @SeanCurtis-TRI)

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 67 at r1 (raw file):

//    3.3-beta1) can pass with a tolerance of 16 * ε.
// 3. Mac CI requires another bump in the multiplier for floats. So, floats here
//    are 24.

btw. for Mac, is this bump independent of the Eigen version?

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 219 at r1 (raw file):

}

// Returns a collection of configurations where sphere and cylinder are simliar

btw: typo, similar

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 677 at r1 (raw file):

template <typename S>
using EvalFunc =
std::function<void(const TestConfiguration<S> &, const Transform3<S> &,

minor: & placement.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 781 at r1 (raw file):

void QueryWithVaryingWorldFrames(
    const std::vector<TestConfiguration<S>>& configurations,
    EvalFunc<S> query_eval, const Matrix3<S> &R_CS = Matrix3<S>::Identity()) {

nit: & placement.

[amcastro-tri]

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

---

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

1. It's the same as the condition in the sphere-box collision.
2. The reason is that if the center is near the Voronoi boundary, small perturbations in the bits would cause arbitrary selection of barrel over face (perturbations due, simply to rigid transformations). This provides insurance that the answer won't change just because the fixed relationship is evaluated in a different frame.

Re 1: I might have missed that in the box review.
Re 2: I am still unsure if needed at all (I just thought it'd work anyways and why to complicate the logic if not needed.). However, as long as I made you think twice about this I got my job done and I'll trust you on this one :-)

[SeanCurtis-TRI]

Thanks for the fast turnaround. Over to you @sherm1

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

---

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

Previously, amcastro-tri (Alejandro Castro) wrote…

Re 1: I might have missed that in the box review.
Re 2: I am still unsure if needed at all (I just thought it'd work anyways and why to complicate the logic if not needed.). However, as long as I made you think twice about this I got my job done and I'll trust you on this one :-)

In fact, if you remove the eps, one of the tests will fail. :)

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 67 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw. for Mac, is this bump independent of the Eigen version?

Yep

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 219 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

btw: typo, similar

Done

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 677 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

minor: & placement.

Done

This will be an inevitable pain point while fcl doesn't have a styleguide and, more particularly, doesn't have a clang-format file that enforces this. Siiiigh.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 781 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

nit: & placement.

Done

[amcastro-tri]

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

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 67 at r1 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Yep

btw. maybe worth adding that to the comment for future reference.

[sherm1]

At @amcastro-tri's suggestion I just gave this a quick look and stumbled over some trivial things like line length. The test suite here is amazing!

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

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 142 at r3 (raw file):

        EXPECT_EQ(p_CQ(0), p_CN(0)) << "Clamped by end face - " << parameters;
        EXPECT_EQ(p_CQ(1), p_CN(1)) << "Clamped by end face - " << parameters;
        EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Clamped by end face - " << parameters;

Nit: line length.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 152 at r3 (raw file):

        EXPECT_NEAR(point_on_barrel(1), p_CN(1), Eps<S>::value())
                  << "Fully clamped - " << parameters;
        EXPECT_EQ(0.5 * h * z_sign, p_CN(2)) << "Fully clamped - " << parameters;

Nit: line length.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 486 at r3 (raw file):

    config.expected_depth = r_s + h_c / 2;
    config.expected_normal = -Vector3<S>::UnitZ();
    config.expected_pos = Vector3<S>{0, 0, h_c / 2 - config.expected_depth / 2};

Nit: line length.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 506 at r3 (raw file):

  }

Nit: extra blank lines.

[amcastro-tri]

Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @SeanCurtis-TRI)

[amcastro-tri]

are we ready to merge?

[SeanCurtis-TRI]

Cleaned up a few cosmetic requests.

Reviewable status: 9 of 10 files reviewed, 1 unresolved discussion (waiting on @amcastro-tri, @sherm1, and @SeanCurtis-TRI)

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 67 at r1 (raw file):
I thought I had.

Mac CI requires another bump in the multiplier for floats. So floats here are 24.

Is there some aspect of that statement that is missing?

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 142 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Nit: line length.

Done

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 152 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Nit: line length.

Done

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 486 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Nit: line length.

Nope. No line length issue here.

---

test/narrowphase/detail/primitive_shape_algorithm/test_sphere_cylinder.cpp, line 506 at r3 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Nit: extra blank lines.

Done

[sherm1]

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