Fix epa box support

[hongkai-dai]

This is a cheap way to fix #395.

This fixes the case that if we have three vertices in the Minkowski difference A ⊖ B, with two of them coming from the opposing two vertices of a box face, and the third one comes from the middle point of a box, we will have degenerate triangle, formed by these three vertices.

It is still possible that we will have degenerate triangle (due to numerical round-off error).

---

This change is 

[hongkai-dai]

+@sherm1 for review please. I didn't assign this to Sean, as I don't want to distract him from autodiffable scenegraph work.

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

[sherm1]

Feature pending a few requests.

Reviewed 4 of 4 files at r1.
Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @hongkai-dai)

---

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

                              ccd_vec3_t* v)
{
  auto sign = [](ccd_real_t x) -> ccd_real_t {

minor: please add a comment here explaining why you want to use this custom sign() method. (I believe it is so you'll always get a vertex rather than a face or edge center?)

---

test/test_fcl_signed_distance.cpp, line 350 at r1 (raw file):

}

template <typename S>

minor: please add a comment explaining why this test uses these very-precise numbers. Also, is there an FCL or Drake issue to reference?

---

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

  // Pc1 - Pc2) contains the origin.
  const Vector3<S> p_FPa1(-1, -1, 0);
  const Vector3<S> p_FPa2(-0.1, 0.5, 0);

minor: why did you make these zero? It looks like this went from a 3d test to a 2d test. Is that intentional? If so can you add a comment saying why? Also, do we still need to test the 3d cases?

[hongkai-dai]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @sherm1)

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: please add a comment here explaining why you want to use this custom sign() method. (I believe it is so you'll always get a vertex rather than a face or edge center?)

Done.

---

test/test_fcl_signed_distance.cpp, line 350 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

minor: please add a comment explaining why this test uses these very-precise numbers. Also, is there an FCL or Drake issue to reference?

Done.

---

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

Previously, sherm1 (Michael Sherman) wrote…

minor: why did you make these zero? It looks like this went from a 3d test to a 2d test. Is that intentional? If so can you add a comment saying why? Also, do we still need to test the 3d cases?

It was a 2D test before. What we care about is the difference (Pa1 - Pa2, Pb1 - Pb2, Pc1 - Pc2), and the z component of these three vectors are all 0 before and after the change.

The reason why I changed these values was that previously I incorrectly set box 2's size with box1. As I fix that bug, the point p_FPa2 = (-0.1, 0.5, -1) is not in box 2 any more, so I have to change the value of p_FPa2 as well.

[sherm1]

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

---

test/test_fcl_signed_distance.cpp, line 300 at r2 (raw file):

template <typename S>
void test_distance_box_box_helper(const Vector3<S>& box1_size,

BTW Nice cleanup!

[sherm1]

+@SeanCurtis-TRI for final review, please

Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)

[SeanCurtis-TRI]

with a few passing thoughts/comments.

Reviewed 1 of 4 files at r1, 3 of 3 files at r2.
Reviewable status: complete! all files reviewed, all discussions resolved

---

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

    // the middle vertex on that line from the polytope, and then reconnect
    // the polytope.
    if (triangle_area_is_zero(new_vertex->v.v, border_edge->vertex[0]->v.v,

BTW I'm a bit surprised that this test isn't done in the ComputeVisiblePatch() method. It seems that's where the error/lie is made (i.e., an edge has been classified as a boundary edge when that should not be the case).

---

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 2255 at r2 (raw file):
BTW This comment addresses the what of the sign function -- the effect it has is that the support vertex will always be a box vertex. What this comment lacks is the why. Something along the lines of:

Picking support vertices on the interior of faces/edges can lead to degenerate triangles in the EPA algorithm and are no more correct than just picking box corners.

That extension will help future readers to know why they shouldn't change it back.

---

test/test_fcl_capsule_box_1.cpp, line 120 at r2 (raw file):

GTEST_TEST(FCL_GEOMETRIC_SHAPES, distance_capsule_box_ccd)
{
  test_distance_capsule_box<double>(fcl::GJKSolverType::GST_LIBCCD, 1e-8, 4e-4);

BTW Do you have any insight as to why a tighter solve tolerance requires a looser result tolerance? (Ignoring the fact that 1e-4 is not a whole lot better bound on the solution than 4e-4.)

[hongkai-dai]

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

---

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I'm a bit surprised that this test isn't done in the ComputeVisiblePatch() method. It seems that's where the error/lie is made (i.e., an edge has been classified as a boundary edge when that should not be the case).

Done, good call.

---

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This comment addresses the what of the sign function -- the effect it has is that the support vertex will always be a box vertex. What this comment lacks is the why. Something along the lines of:

Picking support vertices on the interior of faces/edges can lead to degenerate triangles in the EPA algorithm and are no more correct than just picking box corners.

That extension will help future readers to know why they shouldn't change it back.

Done.

---

test/test_fcl_capsule_box_1.cpp, line 120 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Do you have any insight as to why a tighter solve tolerance requires a looser result tolerance? (Ignoring the fact that 1e-4 is not a whole lot better bound on the solution than 4e-4.)

Resolved after f2f discussion.

[SeanCurtis-TRI]

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