Fix FCL's EPA implementation, when calling libccd solver. #305

hongkai-dai · 2018-06-10T05:45:10Z

This PR aims at computing the signed distance function with option solver_type == GST_LIBCCD, which is the only solver in FCL that can compute the nearest points when the objects penetrate.

Prior to this PR, the implementation in FCL has several flaws.

As described already in Distance computed in EPA incorrectly uses an un-normalized vector #301, that the distance from the boundary of the polytope to the origin, is computed using an un-normalized vector. Also the termination criteria is incorrect. This issue is caused by libccd's implementation, and also reported in a libccd issue.
FCL doesn't extract the nearest points correctly. What it did before is to loop over all the vertices of the polytope, and pick the one that is closest to the origin. But due to the convexity of the polytope, it is proved that unless the origin is on one of the vertex, the closest point from the boundary of the polytope to the origin, is always an interior point of the triangle. Thus the closest point should not be a vertex.
When expanding the polytope in the EPA algorithm, FCL (and LIBCCD, where the code comes from) only delete the face where the closest point lives, and then add three faces which connects the new vertex to the edges of the removed triangle. This operation does not preserve the convexity of the expanded polytope. As a result, in many cases, the closest point from the boundary of the polytope to the origin, lies on an edge of the (nonconvex) polytope, instead of an interior point of the triangle. The correct way is to remove all the faces in the old polytope, that can be seen from the new vertex.

This PR also adds unit tests to the EPA implementation.

This change is

hongkai-dai · 2018-06-10T18:10:01Z

+@SeanCurtis-TRI for feature review please.

Review status: 0 of 5 files reviewed at latest revision, all discussions resolved.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-11T21:40:38Z

Is it your intent to squash these commits? If so, don't forget to do so. Otherwise, pre-curate your commits.

Checkpoints.

Reviewed 2 of 5 files at r1.
Review status: 2 of 5 files reviewed, 20 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

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

 * Computes the normal vector of a face on a polytope, and the normal vector
 * points outward from the polytope. Notice we assume that the origin lives
 * within the polytope.

BTW Can you change it to "...vector of a triangular face on a polytope."

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

              &(face->edge[1]->vertex[0]->v.v));
  ccd_vec3_t dir;
  ccdVec3Cross(&dir, &e1, &e2);

nit: Any worries about a degenerate triangle here? If not, please document it.

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

    ccd_pt_vertex_t* v;
    // If the magnitude of the projection is larger than tolerance, then it
    // means one of the vertex is at least 1cm away from the plane coinciding

nit: several things:

"...one of the vertices..."
Per our f2f, the justification of 1 cm as a threshold (and particular emphasis on the fact that it doesn't hurt correctness should be captured here.
Your tolerance should be 1 cm * |dir| (not 1cm / |dir|).

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

      }
    }
    // If max_projection > |min_projection|, then flip dir.

nit: The comment literally repeats the code -- there is no new information in this comment that isn't clearly expressed in the code. You should rework the comment to explain this "trick" because it takes a moment or two to reason through why this is the right operation.

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

// @param f A triangle on a polytope.
// @param pt A point.
static bool outsidePolytopeFace(const ccd_pt_t* polytope,

nit: this would be better named with a verb (e.g., isOutsidePolytopeFace)

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

                                const ccd_pt_face_t* f, const ccd_vec3_t* pt) {
  ccd_vec3_t n = faceNormalPointingOutward(polytope, f);
  return ccdVec3Dot(&n, pt) > ccdVec3Dot(&n, &(f->edge[0]->vertex[0]->v.v));

BTW Here you're doing 2 dot products. Other than the fact that ccd makes this more painful to do, it would be cheaper to do a subtraction and a dot product.

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

}

/**

I would advocate a different organization of this code. It should be

void floodFillSilhouettte(const ccd_pt_t& polytope, const ccd_pt_face_t& f, const ccd_vec3& test_vertex, ...) {
}

void floodFillSilhouetteRecurse(polytope, f, int edge_index, test_vertex, ..) {
}

The first function should include the lines from expandPolytope that include the face in the obsolete_faces and iterate through its edges. Then, in expandPolytope, the only thing it should do is identify the start_face and pass in the collections. This hides the implementation from the function that just wants to know what to keep and what to throw out and doesn't it require to do extra work.

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

/**
 * Test if the face neighbouring triangle f, and sharing the common edge

After reading through all of the documentation and the function, I realize this description is incorrect.

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

/**
 * Test if the face neighbouring triangle f, and sharing the common edge
 * f->edge[edge_index] can be seen from the new vertex or not. If the face

BTW

I love the language "can be seen", but I think there should be a more formal mathematical definition of "can be seen" as well.
You don't need the final "or not".
There should be a comma: "...common edge f->edge[edge_index], can..."

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

 * we add the common edge to obsolete_edges. The face f will be added to 
 * obsolete_faces.
 * We will call this function recursively to traverse all faces that can be seen

This being recursive is not part of the API. It's an implementation detail and, as such, doesn't belong in this documentation.

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

 * @param[in] edge_index We will check if the face neighbouring f with this
 * common edge f->edge[edge_index] can be seen from the new vertex.
 * @param[in] new_vertex The new vertex to be added to the polytope.

This description is inconsistent with the description given above -- above, this is the vertex from which visibility is tested, but here, it has become the new vertex for the polytope. This should be addressed in the rephrasing of the documentation.

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

 * common edge f->edge[edge_index] can be seen from the new vertex.
 * @param[in] new_vertex The new vertex to be added to the polytope.
 * @param[in/out] visited_faces If the neighbouring face hasn't been visited,

This parameter does not exist.

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

 * the new vertex, then the common edge will be preserved, after adding the new
 * vertex, and this common edge will be added to silhouette_edges.
 * @param[in/out] obsolete_faces If the neighbouring face can be seen from

BTW Why not name it "visible_faces"?

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

 * the new vertex, then add it to obsolete_faces.
 * @param[in/out] obsolete_edges If the neighbouring face can be seen from
 * the new vertex, then add this common edge to obsolete_edges.

BTW add @pre that the polytope is convex. As you've seen, nothing in the polytope class guarantees convexity if someone applies a bad operation. But at least, you've reminded people of the importance.

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

 * the new vertex, then add this common edge to obsolete_edges.
 */
static void floodFillSilhouette(

You've introduced concepts of "silhouette" and "visible", you should do the following:

Clearly discuss the metaphor so that the reader can understand how to interpret these terms geometrically.
Make sure all of your parameter names and documentation are consistent with this metaphor. In other words, this shouldn't be about adding vertices or deleting edges. It simply flags the:
- visible faces
- boundary edges (edges shared by one visible face)
- visible edges (edges shared by two visible faces)

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

    std::unordered_set<ccd_pt_edge_t*>* silhouette_edges,
    std::unordered_set<ccd_pt_face_t*>* obsolete_faces,
    std::unordered_set<ccd_pt_edge_t*>* obsolete_edges) {

btw: Ideally, you should confirm that none of the pointers are null. That said, FCL doesn't do that nor provide a uniform mechanism for doing so.

Thoughts:

Those objects you're passing in as const pointers, pass in as const references.
For those objects that are being passed in as mutable pointers, add @pre statement that they must not be nullptr.

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

                                   ? f->edge[edge_index]->faces[1]
                                   : f->edge[edge_index]->faces[0];
  const auto it = obsolete_faces->find(f_neighbour);

It isn't clear to me that this serves the role of "visited faces". What's to prevent me from visiting an occluded face multiple times? In fact, I'm sure I can conceive of a (simple) scenario in which an occluded face would be visited twice. (The simplest example would be a tetrahedron, where the visible silhouette is two triangles -- the other two triangles would each get visited twice.)

If you refactor as I suggest above, you can introduce the visited faces as an internal detail that the caller won't even know about.

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

                                   : f->edge[edge_index]->faces[0];
  const auto it = obsolete_faces->find(f_neighbour);
  if (it == obsolete_faces->end()) {

BTW You could replace both of these lines with:

if (obsolete_faces->count(f_neighbor) == 0) {

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

The edges/faces on the original polytope that would be interior to the new convex hull are discarded.

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

 * discarded.
 * @param[in/out] pt The polytope.
 * @param[in] el The point on the boundary of the old polytope that is nearest

BTW This description of el doesn't seem relevant to this function. It doesn't seem that this function relies on the indicated property. Instead, it seems to rely on the property of "a point on a feature visible from newv." Although, if you use "visible" in this function, you'll have to define what that means here.

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

  // Now remove all the obsolete faces.
  for (const auto& f : obsolete_faces) {
    ccdPtDelFace(pt, f);

BTW This is unfortunate, we're doing a linear search on the faces for each face in the unordered set. It would be better if we could iterate through the list once and just do lookups in the obsolete_face. Same for edges.

If this isn't straight-forward to do in this PR, please put a TODO.

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

  // Now remove all the obsolete edges.
  for (const auto& e : obsolete_edges) {
    ccdPtDelEdge(pt, e);

BTW Food for thought in the future. Rather than deleting the edge/face/vertex, it might be worth just removing it from the network and deleting it when the whole polytope gets deleted (rather than making lots of little calls to delete.) It means the data structure gets bloated with "deleted" objects, but we minimize system calls.

Alternatively, some internal memory management that keeps the memory but re-uses it would be slick as well. Again, minimizing system calls. Not part of this PR -- just a thought for the future.

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

    ccdPtDelEdge(pt, e);
  }
  // A vertex cannot be obsolete, since a vertex is always on the boundary of

I don't understand this statement. Up above, you talk about deleting vertices (when all of the supporting edges have been deleted), but you're not doing it now.

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

  ccd_pt_vertex_t* new_vertex = ccdPtAddVertex(pt, newv);

  // Now add the new edges and faces, by connecting the new vertex with vertices

nit: This documentation and name seems a bit wrong. Really, what this is is the set of new edges. Each of these edges has the new vertex on one end and a silhouette vertex at the other and and can be looked up via their silhouette vertices.

btw: if the silhouette edges were ordered, then you could throw out the the map entirely.

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

/** In each iteration of EPA algorithm, given the nearest point on the polytope
 * boundary to the origin, a sampled direction will be computed, to find the

nit: "sampled" is not the word you want. I'd simply say "support" direction (here and below).

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

/** In each iteration of EPA algorithm, given the nearest point on the polytope
 * boundary to the origin, a sampled direction will be computed, to find the
 * support of the Minkowski sum A⊖B along that direction, so as to expand the

BTW This is pedantic, but I'd say either

"Minkowski difference A ⊖ B" or

"Minkoski sum A ⊕ -B".

(And I'd include white space around the operator).

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

 * If we denote the nearest point as v, when the v is not the origin, then the
 * sampled direction is v. If v is the origin, then v should be an interior
 * point on a face, then the sampled direction is the normal of that face,

BTW "...an interior point on a face, and the sampled direction..."

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

 * point on a face, then the sampled direction is the normal of that face,
 * pointing outward from the polytope.
 * @param polytope The polytoped contained in A⊖B.

BTW s/polytoped/polytope

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

 * that dir is a normalized vector.
 */
static ccd_vec3_t sampledEPADirection(const ccd_pt_t* polytope,

BTW Look at this! Returning a ccd_vec3_t instead of passing by pointer. Huzzah!

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

                                      const ccd_pt_el_t* nearest_pt) {
  ccd_vec3_t dir;
  if (ccdIsZero(nearest_pt->dist)) {

BTW This function would read better if you started with the line:

const ccd_real_t dist_squared = nearest_pt->dist; :)

Then the madness is isolated to one line and all other lines that reference the squared distance would be clear.

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

        throw std::runtime_error(
            "The nearest point to the origin is a vertex of the polytope. This "
            "should be identified as a touching contact, before calling this "

BTW I'd replace "this function" with the actual function name.

Isn't this considered an error?
It isn't clear to me that nextSupport, below, is actually doing this pre-test.

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

      }
      case CCD_PT_EDGE: {
        ccd_pt_edge_t* edge = (ccd_pt_edge_t*)nearest_pt;

nit: static_cast<ccd_pt_edge_t*>(nearest_pt);, please. We need to stanch the flow of the C sickness.

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

      case CCD_PT_EDGE: {
        ccd_pt_edge_t* edge = (ccd_pt_edge_t*)nearest_pt;
        dir = faceNormalPointingOutward(polytope, edge->faces[0]);

Two questions: don't we consider this to be a degenerate case? And can you provide some documentation arguing why picking one arbitrary face direction is the right thing to do?

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

 * @retval status If the program converges, then return -1. Otherwise return 0.
 * There are several cases that the program could converge.
 * 1. If the nearest point on the boundary of the polytope to the origin is a

If this property applied to the input element, your program would throw an exception. Probably want to document that distinction.

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

  if (el->type == CCD_PT_VERTEX) return -1;

  const ccd_vec3_t dir = sampledEPADirection(polytope, el);

BTW If we were to apply drake standards, this would be: sampledEpaDirection well, the leading "s" would be capitalized as well. But it might be easier to read if EPA weren't all caps.

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1030 at r1 (raw file):
BTW Could you fix this comment?

Compute distance of support point in the support direction.

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

  // so we can determine whether we expanded a polytope surrounding the
  // origin a bit.
  dist = ccdVec3Dot(&out->v, &dir);

BTW This seems odd to me. This distance merely seems to be the projected distance along the support direction. However, but down below, the distance we consider below is actual Euclidian distance. It seems counter-intuitive. Is there a rational explanation? Or shall we put in a TODO to figure it out and document it?

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

 * p1 is the deepest penetration point on A, and p2 is the deepest penetration
 * point on B.
 * @param[in] pt The polytope inside Minkowski sum A⊖B. Unused in this function.

You can remove this comment and the FCL_UNUSED at the end simply by remove the variable name from the signature.

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

{
  // We reconstruct the simplex on which the nearest point lives, and then
  // compute the deepest penetration point on each geometric objects.

BTW Worth noting that we only support up to a 3-simplex. IN this code, that's worth making clear because, generally, we do allow 4-simplices.

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

  EXPECT_TRUE(res);
  const S distance_tolerance = std::sqrt(request.distance_tolerance);

nit: This really needs documenting. The code ends up reading like:

d = √d

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

  EXPECT_TRUE(res);
  const S distance_tolerance = std::sqrt(request.distance_tolerance);
  EXPECT_TRUE(std::abs(result.min_distance - (-5)) < distance_tolerance);

nit: Can you change this test to:

EXPECT_NEAR(result.min_distance, -5, distance_tolerance);

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

  {
    EXPECT_TRUE(result.nearest_points[0].isApprox(Vector3<S>(20, 0, 0),
                                                  distance_tolerance));

BTW We so desperately need to include Drakes CompareMatrices into FCL. Eigen's isApprox is horrible.

Comments from Reviewable

hongkai-dai · 2018-06-12T04:25:14Z

Review status: 1 of 5 files reviewed at latest revision, 12 unresolved discussions.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Can you change it to "...vector of a triangular face on a polytope."

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Any worries about a degenerate triangle here? If not, please document it.

Done, I ignore the degeneracy case here.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: several things:

"...one of the vertices..."

Per our f2f, the justification of 1 cm as a threshold (and particular emphasis on the fact that it doesn't hurt correctness should be captured here.

Your tolerance should be 1 cm * |dir| (not 1cm / |dir|).

Done. Good call.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: The comment literally repeats the code -- there is no new information in this comment that isn't clearly expressed in the code. You should rework the comment to explain this "trick" because it takes a moment or two to reason through why this is the right operation.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: this would be better named with a verb (e.g., isOutsidePolytopeFace)

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Here you're doing 2 dot products. Other than the fact that ccd makes this more painful to do, it would be cheaper to do a subtraction and a dot product.

Done. Good point

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I would advocate a different organization of this code. It should be
void floodFillSilhouettte(const ccd_pt_t& polytope, const ccd_pt_face_t& f, const ccd_vec3& test_vertex, ...) {
}

void floodFillSilhouetteRecurse(polytope, f, int edge_index, test_vertex, ..) {
}
The first function should include the lines from expandPolytope that include the face in the obsolete_faces and iterate through its edges. Then, in expandPolytope, the only thing it should do is identify the start_face and pass in the collections. This hides the implementation from the function that just wants to know what to keep and what to throw out and doesn't it require to do extra work.

Done, good point!

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

After reading through all of the documentation and the function, I realize this description is incorrect.

Done, I rewrite this part of the documentation.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW

I love the language "can be seen", but I think there should be a more formal mathematical definition of "can be seen" as well.

You don't need the final "or not".

There should be a comma: "...common edge f->edge[edge_index], can..."

Done. I rewrote this documentation.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This being recursive is not part of the API. It's an implementation detail and, as such, doesn't belong in this documentation.

Done. I rewrote this documentation.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This description is inconsistent with the description given above -- above, this is the vertex from which visibility is tested, but here, it has become the new vertex for the polytope. This should be addressed in the rephrasing of the documentation.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This parameter does not exist.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Why not name it "visible_faces"?

Hmm I was trying to use the same term obsolete for both faces and edges. If we use visible for edges also, then a silhouette edge is also visible, which can cause confusion.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW add @pre that the polytope is convex. As you've seen, nothing in the polytope class guarantees convexity if someone applies a bad operation. But at least, you've reminded people of the importance.

Done, good point.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

You've introduced concepts of "silhouette" and "visible", you should do the following:

Clearly discuss the metaphor so that the reader can understand how to interpret these terms geometrically.

Make sure all of your parameter names and documentation are consistent with this metaphor. In other words, this shouldn't be about adding vertices or deleting edges. It simply flags the:

visible faces

boundary edges (edges shared by one visible face)

visible edges (edges shared by two visible faces)

That is a good point. I am not so sure about the name visible edges, and the boundary edges are still visible.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw: Ideally, you should confirm that none of the pointers are null. That said, FCL doesn't do that nor provide a uniform mechanism for doing so.

Thoughts:

Those objects you're passing in as const pointers, pass in as const references.

For those objects that are being passed in as mutable pointers, add @pre statement that they must not be nullptr.

Hmm, I think FCL does not use the google style guide, which requires passing const reference, instead of const pointers. If we want to stay consistent with the rest of the code in this file gjk_libccd-inl.h, it is better to use const pointers?

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

It isn't clear to me that this serves the role of "visited faces". What's to prevent me from visiting an occluded face multiple times? In fact, I'm sure I can conceive of a (simple) scenario in which an occluded face would be visited twice. (The simplest example would be a tetrahedron, where the visible silhouette is two triangles -- the other two triangles would each get visited twice.)

If you refactor as I suggest above, you can introduce the visited faces as an internal detail that the caller won't even know about.

Exactly as you said, we can access a face twice, from two different edges. And I think we need to access the face for twice, so as to determine if its edges are silhouette edges or visible edges.

Say we have an occluded face, but both its neightbouring faces are visible, then we need to access the occluded face for twice, each time for one edge that neighbours the visible face.

As you said, we can add a "visited face" parameter in the recursive function. I regard that as an optimization of the current implementation.

I will optimize the recursive function later, to avoid storing the obsolete face/edges, and to store visited faces.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW You could replace both of these lines with:

if (obsolete_faces->count(f_neighbor) == 0) {

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW

The edges/faces on the original polytope that would be interior to the new convex hull are discarded.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This description of el doesn't seem relevant to this function. It doesn't seem that this function relies on the indicated property. Instead, it seems to rely on the property of "a point on a feature visible from newv." Although, if you use "visible" in this function, you'll have to define what that means here.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This is unfortunate, we're doing a linear search on the faces for each face in the unordered set. It would be better if we could iterate through the list once and just do lookups in the obsolete_face. Same for edges.

If this isn't straight-forward to do in this PR, please put a TODO.

Done, add a TODO here.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Food for thought in the future. Rather than deleting the edge/face/vertex, it might be worth just removing it from the network and deleting it when the whole polytope gets deleted (rather than making lots of little calls to delete.) It means the data structure gets bloated with "deleted" objects, but we minimize system calls.

Alternatively, some internal memory management that keeps the memory but re-uses it would be slick as well. Again, minimizing system calls. Not part of this PR -- just a thought for the future.

Totally agree, when we implement our own polytope representation, we will add a flag obsolete there. And instead of actually removing the face/edges, we will just set obsolete flag to true.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I don't understand this statement. Up above, you talk about deleting vertices (when all of the supporting edges have been deleted), but you're not doing it now.

Forgot to update the documentation above. A vertex cannot be removed.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This documentation and name seems a bit wrong. Really, what this is is the set of new edges. Each of these edges has the new vertex on one end and a silhouette vertex at the other and and can be looked up via their silhouette vertices.

btw: if the silhouette edges were ordered, then you could throw out the the map entirely.

Done, rename the variable.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: "sampled" is not the word you want. I'd simply say "support" direction (here and below).

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This is pedantic, but I'd say either

"Minkowski difference A ⊖ B" or

"Minkoski sum A ⊕ -B".

(And I'd include white space around the operator).

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "...an interior point on a face, and the sampled direction..."

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW s/polytoped/polytope

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Look at this! Returning a ccd_vec3_t instead of passing by pointer. Huzzah!

Should we pass by pointer here? Since it is static size I assume it is OK to return ccd_vec3_t?

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This function would read better if you started with the line:

const ccd_real_t dist_squared = nearest_pt->dist; :)

Then the madness is isolated to one line and all other lines that reference the squared distance would be clear.

Done. nearest_pt->dist is only called at the very end of the function, so I put dist = std::sqrt(nearest_pt->dist just before it is being called.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I'd replace "this function" with the actual function name.

Isn't this considered an error?

It isn't clear to me that nextSupport, below, is actually doing this pre-test.

In next_support, just before it calls supportEPADirection, it has the line

if (el->type == CCD_PT_VERTEX) return -1;

so it actually prevents calling supportEPADirection, if the nearest point is a vertex.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: static_cast<ccd_pt_edge_t*>(nearest_pt);, please. We need to stanch the flow of the C sickness.

Compiler is unhappy when using static_cast, with the error message

/home/hongkai/fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:1042:69: error: invalid static_cast from type ‘const ccd_pt_el_t* {aka const _ccd_pt_el_t*}’ to type ‘ccd_pt_edge_t* {aka _ccd_pt_edge_t*}’
         ccd_pt_edge_t* edge = static_cast<ccd_pt_edge_t*>(nearest_pt);

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Two questions: don't we consider this to be a degenerate case? And can you provide some documentation arguing why picking one arbitrary face direction is the right thing to do?

Why this is degenerate? When the closest point is on an edge, the origin must be on that edge. But in this case the distance from the Minkowski difference boundary to the origin can be non-zero.

I add documentation here.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

If this property applied to the input element, your program would throw an exception. Probably want to document that distinction.

It should not throw an exception here, the program terminates with -1, according to the line

if (el->type == CCD_PT_VERTEX) return -1;

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW If we were to apply drake standards, this would be: sampledEpaDirection well, the leading "s" would be capitalized as well. But it might be easier to read if EPA weren't all caps.

Hmm I think we haven't applied drake standard yet, as the leading letter in each function name is in lower case.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Could you fix this comment?

Compute distance of support point in the support direction.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This seems odd to me. This distance merely seems to be the projected distance along the support direction. However, but down below, the distance we consider below is actual Euclidian distance. It seems counter-intuitive. Is there a rational explanation? Or shall we put in a TODO to figure it out and document it?

I think that is the EPA algorithm, the projection distance is an upper bound of the distance from the Minkowski difference boundary to the origin, and the actual Euclidean distance is an lower bound. I added some documentation here.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

You can remove this comment and the FCL_UNUSED at the end simply by remove the variable name from the signature.

Done. Good point.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Worth noting that we only support up to a 3-simplex. IN this code, that's worth making clear because, generally, we do allow 4-simplices.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: This really needs documenting. The code ends up reading like:

d = √d

Done, good point.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

nit: Can you change this test to:

EXPECT_NEAR(result.min_distance, -5, distance_tolerance);

Done. good point.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW We so desperately need to include Drakes CompareMatrices into FCL. Eigen's isApprox is horrible.

Agree, something we can do in a separate PR.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-12T16:09:50Z

Checkpoint on main implementation -- moving on to tests.

Reviewed 2 of 4 files at r2.
Review status: 3 of 5 files reviewed, 6 unresolved discussions (waiting on @SeanCurtis-TRI and @hongkai-dai)

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done, I ignore the degeneracy case here.

You put in a "todo" that doesn't say what is to be done. The indirect implication is that we should stop ignoring the degeneracy. Can you state this more directly?

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done. Good point

I've been leaking drake notation into FCL with links to drake's notation. So, for this one, I'd probably do r_VP. But I'll defer to you. The more I use it, the happier I am with the code.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done, I rewrite this part of the documentation.

I've proposed an alternate write-up to the entry-point function. See what you think and modify this one to match (if you like it.)

One general note, the function level of documentation shouldn't explain how the function works internally, only what I need to know about invoking it (interpretation of inputs and outputs and requirements on inputs.)

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Hmm I was trying to use the same term obsolete for both faces and edges. If we use visible for edges also, then a silhouette edge is also visible, which can cause confusion.

See note on entry point function.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Hmm, I think FCL does not use the google style guide, which requires passing const reference, instead of const pointers. If we want to stay consistent with the rest of the code in this file gjk_libccd-inl.h, it is better to use const pointers?

See note on entry point function.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Exactly as you said, we can access a face twice, from two different edges. And I think we need to access the face for twice, so as to determine if its edges are silhouette edges or visible edges.

Say we have an occluded face, but both its neightbouring faces are visible, then we need to access the occluded face for twice, each time for one edge that neighbours the visible face.

As you said, we can add a "visited face" parameter in the recursive function. I regard that as an optimization of the current implementation.

I will optimize the recursive function later, to avoid storing the obsolete face/edges, and to store visited faces.

I buy that -- and its merely regrettable we compute the normal twice. But, in the future, when the normal is saved with the face, that cost won't exist.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Forgot to update the documentation above. A vertex cannot be removed.

Good point. Given that the new vertex (and every vertex) is on the boundary of the Minkowski sum, that means every other vertex will inevitably have at least one face that isn't visible to the new vertex.

Could you put in a TODO to assert that all vertices are still valid? I can imagine a circumstance where, due to numerical issues, an occluded face is considered visible. This could lead to a vertex no longer referenced by any faces/edges and having a debug check on that might expose problems in the future.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Should we pass by pointer here? Since it is static size I assume it is OK to return ccd_vec3_t?

I think this is a great deviation from what has been done in FCL (based on libccd). I'd strongly advocate for this kind of thing.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Compiler is unhappy when using static_cast, with the error message

/home/hongkai/fcl/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h:1042:69: error: invalid static_cast from type ‘const ccd_pt_el_t* {aka const _ccd_pt_el_t*}’ to type ‘ccd_pt_edge_t* {aka _ccd_pt_edge_t*}’
         ccd_pt_edge_t* edge = static_cast<ccd_pt_edge_t*>(nearest_pt);

It's because of constness. You'd have to cast it to const ccd_pt_edge_t*.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Why this is degenerate? When the closest point is on an edge, the origin must be on that edge. But in this case the distance from the Minkowski difference boundary to the origin can be non-zero.

I add documentation here.

Thanks for the docs. There's a part of my soul that is dissatisfied by this. Since the origin is inside the polytope, there is no voronoi region for the edge. So, saying the closest feature is an edge feels a bit of a lie. In other words, if the origin is on the edge, it's also on a face.

Furthermore, numerically, for the origin to lie on the edge (and not a face), it has to be right on the edge when considered from either adjacent face. That seems highly unlikely. Still, I'll mark myself as satisfied based on the added documentation.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

I think that is the EPA algorithm, the projection distance is an upper bound of the distance from the Minkowski difference boundary to the origin, and the actual Euclidean distance is an lower bound. I added some documentation here.

Thanks

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

/**
 * Traverse the faces of the polytope, to find out all the faces that can be

I've taken the liberty of writing up what I think the documentation should be. This is the documentation for the "main entry point function" I indicated above.

/** Defines the "visible" patch on the given convex `polytope` (with respect to 
 the given `query_vertex`). 

 A patch is characterized by:
   - a contiguous set of visible faces
   - internal edges (the edges for which *both* adjacent faces are in the patch)
   - border edges (edges for which only *one* adjacent face is in the patch)

 A face `f` (with vertices `(v₀, v₁, v₂)` and outward pointing normal `n̂`) is 
 "visible" and included in the patch if, for query vertex `q`:
 
    `n̂ ⋅ (q - v₀) > 0`

@param polytope             The polytope to evaluate.
@param f                    A face known to be visible to the query point.
@param query_point          The point from which visibility is evaluated.
@param[out] border_edges    The collection of patch border edges.
@param[out] visible_faces   The collection of patch faces.
@param[out] internal_edges  The collection of internal edges.

@pre The `polytope` is convex.
@pre The face `f` is visible from `query_point`.
@pre Output parameters are non-null.
**/
static void ComputeVisiblePatch(
    const ccd_pt_t& polytope, ccd_pt_face_t& f,
    const ccd_vec3_t& query_point,
    std::unordered_set<ccd_pt_edge_t*>* border_edges,
    std::unordered_set<ccd_pt_face_t*>* visible_faces,
    std::unordered_set<ccd_pt_edge_t*>* internal_edges)

This addresses several issues

This function has no awareness of why it is called. It doesn't know that internal edges or visible faces are going to be deleted.
It uses consistent language with the operation that is being performed. (E.g., rename of new_vertex to query_point.
I pushed this away from FCL's "pass pointers" style, not for google style guide issues, but for sanity. If the input value cannot be null, then passing references is much better -- the compiler will catch calling errors. That said, I did leave the output parameters as pointers (swayed by the style guide). I think we'll inevitably be pushing fcl's style in that direction and that's a good thing.
The new function name describes what is done but doesn't hint at how it is done.

BTW I'd also support creating a Patch struct that contained the various sets. :)

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

 * @param[in] f The starting face for the traversal. f is a face that can be
 * seen from the new vertex. 
 * @param[in] new_vertex A point ouside of the polytope.

btw s/ouside/outside

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

 * @pre polytope is convex, new_vertex is outside of the polytope.
 * silhouette_edges, obsolete_faces and obsolete_edges cannot be null.
 * TODO(hongkai.dai): do not store obsolete_faces/edges in a set, but remove

I'm not sure what the first sentence means. Why not store in a set?

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

 * TODO(hongkai.dai): do not store obsolete_faces/edges in a set, but remove
 * them within this function. Also we can record the faces being visited, 
 * together with if they are visible/occuluded. So that we do not need to

btw s/occuluded/occluded

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

 * @param[in] el A feature that is visible from the point `newv`. A face is
 * visible from a point ouside the original polytope, if the point is on the
 * "outer" side of the face. An edge is visible from that point, if one of its

See my note below about the feature being an edge. But, if the visibility of an edge is defined by the visibility of its neighboring face, then one has to wonder why isn't el always a face?

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

{
  // The outline of the algorithm is as follows:
  // 1. Remove all the faces that can be seen from the new vertex, namely the

BTW If you go with my ComputeVisiblePatch() proposal above, then you can rephrase all of this in terms of that and "patches". In fact, all of this becomes something along the lines of:

//  1. Compute the visible patch relative to the new vertex (See ComputeVisiblePatch() for details).
//  2. Delete the faces and internal edges.
//  3. Build a new face from each border edge and the new vertex.

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

  // vertex.
  // TODO([email protected]): it is inefficient to store obsolete
  // faces/edges. A better implementation should remove obsolete faces/edges

BTW Is this what the other TODO refers to about getting rid of the set? If you do that, then my "ComputeVisiblePatch" would become "DeleteVisiblePatch" and simply return the border edges. :)

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

  // TODO([email protected]): it is inefficient to store obsolete
  // faces/edges. A better implementation should remove obsolete faces/edges
  // inside silhouetteFloodFill function, when travering the faces on the

BTW s/travering/traversing

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

  std::unordered_set<ccd_pt_edge_t*> obsolete_edges;
  std::unordered_set<ccd_pt_edge_t*> silhouette_edges;
  floodFillSilhouette(pt, start_face, &newv->v, &silhouette_edges,

BTW I hate pt as "polytope". It looks like a more common abbreviation of point. :( Who writes this stuff?

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

 * support of the Minkowski difference A ⊖ B along that direction, so as to
 * expand the polytope.
 * If we denote the nearest point as v, when the v is not the origin, then the

BTW ", when v is not the origin," (no article "the").

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

 * support of the Minkowski difference A ⊖ B along that direction, so as to
 * expand the polytope.
 * If we denote the nearest point as v, when the v is not the origin, then the

BTW Also, this paragraph is really an implementation detail. The function takes the polytope and the nearest point and gives the support vector for the next iteration. The fact that it handles the nearest point in special and tricky ways is an internal detail. It's good information, but it documents the implementation and not the interface.

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

            "The nearest point to the origin is a vertex of the polytope. This "
            "should be identified as a touching contact, before calling "
            "function supportEPADirection");

BTW When referencing function names, you should add a terminal () to emphasize a function call.

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

        // edge. The support direction could be in the range between
        // edge.faces[0].normal and edge.faces[1].normal, where the face normals
        // point outward from the polytope. In this implementatin, we

BTW s/implementatin/implementation

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

 * the origin. 
 * @param[out] out The next support point.
 * @retval status If the program converges, then return -1. Otherwise return 0.

BTW "the program converges" doesn't feel quite right. This function doesn't know about "the program". This function basically says, "I can/can't compute a meaningful next support vector." So, if it returns 0, the support vector in out is meaningful and can/should be used. If it returns -1, no further support vector can be made. So, let's keep the documentation local to what this function knows.

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

  dist = ccdVec3Dot(&out->v, &dir);

  // el->dist is the squared distance from the feature "el".

BTW "...from the feature el to the origin."

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

    if (nearest->type == CCD_PT_EDGE) {
      ccd_pt_edge_t* e = (ccd_pt_edge_t*)nearest;
      if (e == NULL) {

To the best of my knowledge, this condition will never evaluate to true. This is the equivalent of a static_cast and we've already established that nearest is not nullptr. (Same for the others, but this is the one that jumped out at me.)

BTW: also prefer nullptr over NULL.

That also turns your return value into something less meaningful -- you'll never return -1.

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

            third_vertex != f->edge[0]->vertex[1]) {
          ccdSimplexAdd(&s, &(third_vertex->v));
          //break;

BTW Leftover comment here.

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

      throw std::logic_error(
          "Unsupported point type. The closest point should be either a "
          "vertex, on an edge, or an a face.\n");

BTW "...or a face." (and no newline in the exception message).

Comments from Reviewable

SeanCurtis-TRI · 2018-06-12T18:03:28Z

test_epa checkpoint

Reviewed 1 of 4 files at r2.
Review status: 4 of 5 files reviewed, 22 unresolved discussions (waiting on @hongkai-dai and @SeanCurtis-TRI)

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 2 at r2 (raw file):

/*
 * Software License Agreement (BSD License)

I'd recommend renaming this file:

test_gjk_libccd-inl_epa.cpp

We'll have a bunch of different gtest files that test functions in gjk_libccd-inl, by naming them with a common prefix, they'll all cluster together.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 37 at r2 (raw file):

/** @author Hongkai Dai ([email protected]) */

/** Tests the EPA implementation inside FCL. EPA computes the penetration

BTW Might be worth spelling out EPA in this first comment.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 79 at r2 (raw file):

  }

  ccd_pt_vertex_t* v(int i) const { return v_[i]; }

BTW Return a const reference unless nullptr is a meaningful result. (Would the constness be a problem?)

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 89 at r2 (raw file):

  ~EquilateralTetrahedron() {
    ccdPtDestroy(polytope_);
    delete polytope_;

This delete seems very surprising. I'd assume ccdPtDestroy would take care of things. You might consider putting the polytop_ in a unique_ptr. Then all you have to do is call ccdPtDestroy and not delete. Failing that, a comment indicating that ccdPtDestroy only frees up the vertices, faces, and edges contained in the polytope. (Same on Hexagram.)

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 100 at r2 (raw file):

GTEST_TEST(FCL_GJK_EPA, faceNormalPointingOutward) {
  // Construct a equilateral tetrahedron, compute the normal on each face.

This test is incomplete.

Each face on the tetrahedron represents a single, potential test. However, three of the tests are fundamentally redundant (and leaves some code paths untested).

Face 0: counter-clockwise winding, origin on plane.
Face 1, 2, & 3: clockwise winding (origin more than 1 cm off plane)

Faces 1-3 invoke the test that the projection is negative and the direction needs to be flipped.

Face 0 tests projection = 0. However, since there's only one vertex off the plane (and it is farther than 1 cm), it exercises the the branch that leaves the vector alone.

What is not being tested:

Origin on plane requires flipping (i.e., face 0 with reversed winding).
Small tetrahedron -- smaller than 1 cm so that the min_projection/max_projection has to be exercised.
Winding is correct and origin is not on the plane.
Degenerate triangle -- this captures the fact that it's not working and should essentially return 0. Then when the code is updated, the test will flag that behavior has changed.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 108 at r2 (raw file):

    for (int j = 0; j < 4; ++j) {
      EXPECT_LE(ccdVec3Dot(&n, &p.v(j)->v.v),
                ccdVec3Dot(&n, &p.f(i)->edge[0]->vertex[0]->v.v) + 1E-6);

BTW Interesting tolerance

I'd recommend: fcl::constants<ccd_real_t>::eps() (or maybe eps_78() or eps_34() if that is too tight.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 116 at r2 (raw file):

  auto CheckSampledEPADirection = [](
      const ccd_pt_t* polytope, const ccd_pt_el_t* nearest_pt, ccd_real_t dir_x,
      ccd_real_t dir_y, ccd_real_t dir_z, ccd_real_t tol) {

You could wrap these three up in a Vector3<ccd_real_t>. I know in my unit tests, I expressed my tests in Eigen as much as possible and only converted to ccd_* when necessary. This particular usage would make the code easier to read. The whole PR would benefit.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 143 at r2 (raw file):

  EXPECT_THROW(libccd_extension::supportEPADirection(
                   p3.polytope(), (const ccd_pt_el_t*)p3.v(0)),
               std::runtime_error);

Missing some tests:

How about a test where the direction isn't axis aligned? Pick a nearest point on one of the other faces.
Another test would be to rotate the the tetrahedron some arbitrary amount so that none of the faces have nice zeros in them.
You also need a test where the origin is on a face (should report the face normal).

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 162 at r2 (raw file):

  // point (0, 0, 0.1) is inside the tetrahedron.
  CheckPointOutsidePolytopeFace(0, 0, 0.1, 0, false);

BTW I'd recommend the following:

const bool expect_inside = false;
const bool expect_outside = true;

CheckPointOutsidePolytopeFace(0, 0, 0.1, 1, expect_inside);
...

The code would be more readable.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 200 at r2 (raw file):

                                  y + bottom_center_y, z + bottom_center_z);
    };
    // right corner of upper triangle

BTW This documentation might flow better if you labeled the vertices in your diagram above and then just referenced the labels.

Also, a comment that defining them in this order facilitates defining the connecting edges between the top/bottom triangles.

Also, some comment about the winding of the face definitions so that readers don't have to deduce it.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 307 at r2 (raw file):

  std::unordered_set<ccd_pt_face_t*> obsolete_faces;
  std::unordered_set<ccd_pt_edge_t*> obsolete_edges;
  libccd_extension::floodFillSilhouette(polytope.polytope(), face, &new_vertex,

The only thing that floodFillSilhouette() does that the recursive function doesn't do is insert f into the obsolete_faces. As such, having two pieces of code that are so similar doesn't feel worthwhile. I'd recommend simply invoking CheckFloodFillSilhouette and remove CheckFloodFillSilhouetteRecursive.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 328 at r2 (raw file):

}

GTEST_TEST(FCL_GJK_EPA, floodFillSilhouette1) {

BTW Can you have a better name? E.g., floodFillSilhouetteSingleFace and the next would be floodFillSIlhouetteMultiFace, etc.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 390 at r2 (raw file):

}

// Returns true if the the position difference between the two vertices are

"Position difference" is vague. Could you simple call it "distance"?

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 429 at r2 (raw file):

    e2_expected.insert(it->second);
  }
  for (int i = 0; i < 3; ++i) {

This test can be fooled if f1 consists of a single pointer over and over. All of f1 needs to map to a feature in f2, but they must also be a proper one-to-one correspondence.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 460 at r2 (raw file):

  ccdListForEachEntry(feature1_list, f, T, list) { feature1->insert(f); }
  ccdListForEachEntry(feature2_list, f, T, list) { feature2->insert(f); }
  EXPECT_EQ(feature1->size(), feature2->size());

This test passes if they have the same number of unique features. You should document this. That the feature lists may include duplicates, but they must have the same number of unique features.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 475 at r2 (raw file):

  // Every feature in feature1_list should be matched to a feature in
  // feature2_list.
  EXPECT_EQ(map_feature1_to_feature2->size(), feature1->size());

Can this test fail if EXPECT_TRUE(found_match); passes? I think the "onto"-ness of the mapping is already handled. I'm not sure if you've handled the 1-to-1-ness.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 532 at r2 (raw file):

      auto it = map_e1_to_e2.find(v1_e);
      if (it == map_e1_to_e2.end()) {
        throw std::runtime_error("v1_e is not found in map_e1_to_e2.\n");

This should be gtest EXPECT and not a throw.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 535 at r2 (raw file):

      }
      auto v2_e = it->second;
      if (v2_edges.find(v2_e) == v2_edges.end()) {

BTW This debug cout should probably not be committed.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 556 at r2 (raw file):

}

GTEST_TEST(FCL_GJK_EPA, expandPolytope1) {

Same comment on names "expandPolytopN" with more descriptive names.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 556 at r2 (raw file):

}

GTEST_TEST(FCL_GJK_EPA, expandPolytope1) {

BTW These are very cool tests and justify all of the work done above.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 736 at r2 (raw file):

}

GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest1) {

BTW same comment on names.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 751 at r2 (raw file):

                (const ccd_pt_el_t*)tetrahedron.v(0), &p1, &p2),
            0);
  CompareCcdVec3(p1, tetrahedron.v(0)->v.v1, 1E-14);

constants<ccd_real_t>::eps78() for tolerance.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 757 at r2 (raw file):

GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest2) {
  // The nearest point is on an edge of the polytope.
  // tetrahedron.e(0) contains the origin.

Edge 0 is axis aligned, yes? This would be more interesting if it were not so.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 788 at r2 (raw file):

  ccdVec3Scale(&p2_expected, ccd_real_t(0.5));

  CompareCcdVec3(p1, p1_expected, 1E-6);

MUCH lower tolerance! See previous note.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 794 at r2 (raw file):

GTEST_TEST(FCL_GJK_EPA, penEPAPosClosest3) {
  // The nearest point is on a face of the polytope, It is the center of
  // tetrahedron.f(0).

Same note on axis-aligned feature.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-12T20:33:30Z

Woo hoo! I got all the way through.

Reviewed 1 of 4 files at r2.
Review status: all files reviewed, 28 unresolved discussions (waiting on @hongkai-dai)

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl.cpp, line 158 at r2 (raw file):
BTW

Given two boxes, we can perturb the pose of one box so the boxes are penetrating, touching, or separated.

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

  // measured in the world frame, X_WB2 is the pose of box 2 expressed and
  // measured in the world frame.
  fcl::Transform3<S> X_WB1, X_WB2;

I strongly recommend you redefine this test w.r.t. to a reference frame F. In the test you currently have, F = I. And then repeat the tests with F != I so we don't fall victim to the axis-aligned fun and games.

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

  // First fix the orientation of box 2, such that one of its diagonal (the one
  // connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is horizontal. If

"horizontal" - other than parallel with the z = 0 plane, can you make any strong statement?

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

  auto CheckDistance = [&box1_size, &box2_size, &X_WB1](
      const Transform3<S>& X_WB2, S distance_expected,
      const Vector2<S>& p_xy_WNa_expected, const Vector2<S>& p_xy_WNb_expected,

I think p_xy_WNb could use some documentation.

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

        &p1, &p2);

    if (distance_expected < 0) {

BTW I have a redflag when I see that this doesn't account for distance_expected == 0. That'll require some documentation, or something like:

EXPECT_EQ(res, distance_expected > 0); (or >= where ever the = goes.

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

    // The z height of the closest points should be the same.
    EXPECT_NEAR(p1(2), p2(2), tol);
    EXPECT_GE(p1(2), 0);

I need some documentation on these last four expects.

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

  };

  auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance](

BTW A lambda that captures another lambda? It's lambdas all the way down. :)

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

  //---------------------------------------------------------------
  //                      Touching contact
  // An edge of box 2 is touching a face of box 1

Why particularly edge-face contact and not face-face or vertex-face?

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

  CheckBoxEdgeBoxFaceDistance(X_WB2, tol);

  // Shift box 2 on y axis by 0.1m.

The why of these apparently orthogonal shifts should be documented. It's not immediately obvious to me.

Comments from Reviewable

hongkai-dai · 2018-06-13T00:20:15Z

I will address the comments in test_gjk_libccd-inl.cpp this evening.

Review status: 2 of 5 files reviewed, 27 unresolved discussions (waiting on @SeanCurtis-TRI and @hongkai-dai)

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

You put in a "todo" that doesn't say what is to be done. The indirect implication is that we should stop ignoring the degeneracy. Can you state this more directly?

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I've been leaking drake notation into FCL with links to drake's notation. So, for this one, I'd probably do r_VP. But I'll defer to you. The more I use it, the happier I am with the code.

Sure, I will make your happier.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I've proposed an alternate write-up to the entry-point function. See what you think and modify this one to match (if you like it.)

One general note, the function level of documentation shouldn't explain how the function works internally, only what I need to know about invoking it (interpretation of inputs and outputs and requirements on inputs.)

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

See note on entry point function.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

See note on entry point function.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Good point. Given that the new vertex (and every vertex) is on the boundary of the Minkowski sum, that means every other vertex will inevitably have at least one face that isn't visible to the new vertex.

Could you put in a TODO to assert that all vertices are still valid? I can imagine a circumstance where, due to numerical issues, an occluded face is considered visible. This could lead to a vertex no longer referenced by any faces/edges and having a debug check on that might expose problems in the future.

Done, add a TODO here.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

It's because of constness. You'd have to cast it to const ccd_pt_edge_t*.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Thanks for the docs. There's a part of my soul that is dissatisfied by this. Since the origin is inside the polytope, there is no voronoi region for the edge. So, saying the closest feature is an edge feels a bit of a lie. In other words, if the origin is on the edge, it's also on a face.

Furthermore, numerically, for the origin to lie on the edge (and not a face), it has to be right on the edge when considered from either adjacent face. That seems highly unlikely. Still, I'll mark myself as satisfied based on the added documentation.

I think I actually have a case that the origin is on an edge, in my test case. I agree that conceptually when the closest point is on an edge, it is also on a face, and it makes little sense to differentiate them. But that is how nearestPoint from libccd reports the nearest features, it favors an edge over a face.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I've taken the liberty of writing up what I think the documentation should be. This is the documentation for the "main entry point function" I indicated above.
/** Defines the "visible" patch on the given convex `polytope` (with respect to 
 the given `query_vertex`). 

 A patch is characterized by:
   - a contiguous set of visible faces
   - internal edges (the edges for which *both* adjacent faces are in the patch)
   - border edges (edges for which only *one* adjacent face is in the patch)

 A face `f` (with vertices `(v₀, v₁, v₂)` and outward pointing normal `n̂`) is 
 "visible" and included in the patch if, for query vertex `q`:
 
    `n̂ ⋅ (q - v₀) > 0`

@param polytope             The polytope to evaluate.
@param f                    A face known to be visible to the query point.
@param query_point          The point from which visibility is evaluated.
@param[out] border_edges    The collection of patch border edges.
@param[out] visible_faces   The collection of patch faces.
@param[out] internal_edges  The collection of internal edges.

@pre The `polytope` is convex.
@pre The face `f` is visible from `query_point`.
@pre Output parameters are non-null.
**/
static void ComputeVisiblePatch(
    const ccd_pt_t& polytope, ccd_pt_face_t& f,
    const ccd_vec3_t& query_point,
    std::unordered_set<ccd_pt_edge_t*>* border_edges,
    std::unordered_set<ccd_pt_face_t*>* visible_faces,
    std::unordered_set<ccd_pt_edge_t*>* internal_edges)
This addresses several issues

This function has no awareness of why it is called. It doesn't know that internal edges or visible faces are going to be deleted.

It uses consistent language with the operation that is being performed. (E.g., rename of new_vertex to query_point.

I pushed this away from FCL's "pass pointers" style, not for google style guide issues, but for sanity. If the input value cannot be null, then passing references is much better -- the compiler will catch calling errors. That said, I did leave the output parameters as pointers (swayed by the style guide). I think we'll inevitably be pushing fcl's style in that direction and that's a good thing.

The new function name describes what is done but doesn't hint at how it is done.

BTW I'd also support creating a Patch struct that contained the various sets. :)

Thanks! This is great. I copied your documentation and function signature.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw s/ouside/outside

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'm not sure what the first sentence means. Why not store in a set?

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

btw s/occuluded/occluded

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

See my note below about the feature being an edge. But, if the visibility of an edge is defined by the visibility of its neighboring face, then one has to wonder why isn't el always a face?

Add more explanation here, do you think it makes sense now?

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW If you go with my ComputeVisiblePatch() proposal above, then you can rephrase all of this in terms of that and "patches". In fact, all of this becomes something along the lines of:
//  1. Compute the visible patch relative to the new vertex (See ComputeVisiblePatch() for details).
//  2. Delete the faces and internal edges.
//  3. Build a new face from each border edge and the new vertex.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Is this what the other TODO refers to about getting rid of the set? If you do that, then my "ComputeVisiblePatch" would become "DeleteVisiblePatch" and simply return the border edges. :)

That is right, the TODO is to get rid of the set on visible_faces and internal_edges, but we still need the set border_edges, which will be used later, when adding the new edges and faces.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW s/travering/traversing

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I hate pt as "polytope". It looks like a more common abbreviation of point. :( Who writes this stuff?

Done. Good point, I inherit that variable name from previous implementation in FCL (which copies the code from LIBCCD)

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW ", when v is not the origin," (no article "the").

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Also, this paragraph is really an implementation detail. The function takes the polytope and the nearest point and gives the support vector for the next iteration. The fact that it handles the nearest point in special and tricky ways is an internal detail. It's good information, but it documents the implementation and not the interface.

Done. I moved the documentation inside the function implementation.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW When referencing function names, you should add a terminal () to emphasize a function call.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW s/implementatin/implementation

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "the program converges" doesn't feel quite right. This function doesn't know about "the program". This function basically says, "I can/can't compute a meaningful next support vector." So, if it returns 0, the support vector in out is meaningful and can/should be used. If it returns -1, no further support vector can be made. So, let's keep the documentation local to what this function knows.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "...from the feature el to the origin."

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

To the best of my knowledge, this condition will never evaluate to true. This is the equivalent of a static_cast and we've already established that nearest is not nullptr. (Same for the others, but this is the one that jumped out at me.)

BTW: also prefer nullptr over NULL.

That also turns your return value into something less meaningful -- you'll never return -1.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Leftover comment here.

Done. Good catch.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW "...or a face." (and no newline in the exception message).

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 2 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

I'd recommend renaming this file:

test_gjk_libccd-inl_epa.cpp

We'll have a bunch of different gtest files that test functions in gjk_libccd-inl, by naming them with a common prefix, they'll all cluster together.

Good point.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 37 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Might be worth spelling out EPA in this first comment.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 79 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Return a const reference unless nullptr is a meaningful result. (Would the constness be a problem?)

Hmm, I think we do not want to make it const reference here, since we will remove and add features into the polytope, and the existing vertices/edges/faces will be changed.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 89 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This delete seems very surprising. I'd assume ccdPtDestroy would take care of things. You might consider putting the polytop_ in a unique_ptr. Then all you have to do is call ccdPtDestroy and not delete. Failing that, a comment indicating that ccdPtDestroy only frees up the vertices, faces, and edges contained in the polytope. (Same on Hexagram.)

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 100 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This test is incomplete.

Each face on the tetrahedron represents a single, potential test. However, three of the tests are fundamentally redundant (and leaves some code paths untested).

Face 0: counter-clockwise winding, origin on plane.

Face 1, 2, & 3: clockwise winding (origin more than 1 cm off plane)

Faces 1-3 invoke the test that the projection is negative and the direction needs to be flipped.

Face 0 tests projection = 0. However, since there's only one vertex off the plane (and it is farther than 1 cm), it exercises the the branch that leaves the vector alone.

What is not being tested:

Origin on plane requires flipping (i.e., face 0 with reversed winding).

Small tetrahedron -- smaller than 1 cm so that the min_projection/max_projection has to be exercised.

Winding is correct and origin is not on the plane.

Degenerate triangle -- this captures the fact that it's not working and should essentially return 0. Then when the code is updated, the test will flag that behavior has changed.

Thanks a lot! That is super helpful.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 108 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Interesting tolerance

I'd recommend: fcl::constants<ccd_real_t>::eps() (or maybe eps_78() or eps_34() if that is too tight.

For some reasons 1E-7 fails on Mac CI.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 116 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

You could wrap these three up in a Vector3<ccd_real_t>. I know in my unit tests, I expressed my tests in Eigen as much as possible and only converted to ccd_* when necessary. This particular usage would make the code easier to read. The whole PR would benefit.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 143 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Missing some tests:

How about a test where the direction isn't axis aligned? Pick a nearest point on one of the other faces.

Another test would be to rotate the the tetrahedron some arbitrary amount so that none of the faces have nice zeros in them.

You also need a test where the origin is on a face (should report the face normal).

Done. Good point.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 162 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW I'd recommend the following:

const bool expect_inside = false;
const bool expect_outside = true;

CheckPointOutsidePolytopeFace(0, 0, 0.1, 1, expect_inside);
...

The code would be more readable.

Done, good call.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 200 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This documentation might flow better if you labeled the vertices in your diagram above and then just referenced the labels.

Also, a comment that defining them in this order facilitates defining the connecting edges between the top/bottom triangles.

Also, some comment about the winding of the face definitions so that readers don't have to deduce it.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 307 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

The only thing that floodFillSilhouette() does that the recursive function doesn't do is insert f into the obsolete_faces. As such, having two pieces of code that are so similar doesn't feel worthwhile. I'd recommend simply invoking CheckFloodFillSilhouette and remove CheckFloodFillSilhouetteRecursive.

CheckFloodFillSilhouetteRecursive is more granular than CheckFloodFillSilhouette, so I think we should keep it to improve our test granularity.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 328 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW Can you have a better name? E.g., floodFillSilhouetteSingleFace and the next would be floodFillSIlhouetteMultiFace, etc.

Hmm, we have another test that only a single face is visible, and other faces that multiple faces are visible.

I hope the documentation at the beginning of the test is clear about the intention of the test?

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 390 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

"Position difference" is vague. Could you simple call it "distance"?

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 429 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This test can be fooled if f1 consists of a single pointer over and over. All of f1 needs to map to a feature in f2, but they must also be a proper one-to-one correspondence.

Done. Add the check EXPECT_EQ(e2_expected.size(), 3u).

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 460 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This test passes if they have the same number of unique features. You should document this. That the feature lists may include duplicates, but they must have the same number of unique features.

Done. Good call, I add the assertion that the features should be distinct.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 475 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Can this test fail if EXPECT_TRUE(found_match); passes? I think the "onto"-ness of the mapping is already handled. I'm not sure if you've handled the 1-to-1-ness.

I think the 1-to-1 is handled in this line. Since we already have onto, and the size of feature1 is the size as feature2, as checked above, then feature1 and feature2 has to be one on one.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 532 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

This should be gtest EXPECT and not a throw.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 535 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW This debug cout should probably not be committed.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 556 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Same comment on names "expandPolytopN" with more descriptive names.

Hmm, I hope the documentation at the beginning of the test is informative enough?

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 556 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW These are very cool tests and justify all of the work done above.

Thanks!

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 751 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

constants<ccd_real_t>::eps78() for tolerance.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 757 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Edge 0 is axis aligned, yes? This would be more interesting if it were not so.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 788 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

MUCH lower tolerance! See previous note.

Done.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 794 at r2 (raw file):

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

Same note on axis-aligned feature.

Done.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-13T18:52:40Z

Checkpoint

Reviewed 2 of 3 files at r3, 1 of 3 files at r4.
Review status: 3 of 5 files reviewed, 16 unresolved discussions (waiting on @SeanCurtis-TRI and @hongkai-dai)

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

Previously, hongkai-dai (Hongkai Dai) wrote…

I think I actually have a case that the origin is on an edge, in my test case. I agree that conceptually when the closest point is on an edge, it is also on a face, and it makes little sense to differentiate them. But that is how nearestPoint from libccd reports the nearest features, it favors an edge over a face.

Per our f2f, you are completely right and I was just fixating on the wrong thing.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Add more explanation here, do you think it makes sense now?

It seems that this could provide the elaboration you gave me -- if the feature is an edge, then the origin must lie on that edge. That seems like a sufficiently strong result that should be documented.

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done. Good catch.

I'm sure the break isn't necessary: No switch, no while, and no for. What am I missing?

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

/**
 * This function contains the implementation detail of ComputeVisiblePatch

BTW Add terminal "()" to these function names.

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

  This function will be called recursively. It first checks if
  the face neighouring face `f` along the common edge `f->edge[edge_index]` can
  be seen from the point `query_point`. We denote this face as "g", If this face

BTW I somehow missed this on the previous pass, but:

I find "this face" to be ambiguous. Perhaps: "We denote the neighboring face as g".
You have a comma instead of a period.
Putting "g" in quotes doesn't feel mathematical. I'd say either backticks or nothing. (I've picked up the habit of putting backticks....don't know why.)

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 881 at r3 (raw file):
Unfortunately, we lost the TODOs.

TODO(hongkai.dai) Replace patch computation with patch deletion and return border edges as an optimization.
TODO(hongkai.dai) Consider caching results of per-face visiblity status to prevent redundant recalculation -- or by associating the face normal with the face.

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

    e_[4] = ccdPtAddEdge(polytope_.get(), v_[1], v_[3]);
    e_[5] = ccdPtAddEdge(polytope_.get(), v_[2], v_[3]);
    f_[0] = ccdPtAddFace(polytope_.get(), e_[0], e_[1], e_[2]);

Now that you've explicitly defined the ordering of the edges, I'd recommend further documentation. Specifically, I'd explain the designed properties of the tetrahedron:

/** 
  Simple equilateral tetrahedron. 

Geometrically, its edge lengths are the given length (default to unit length). Its "bottom" face is parallel with the z = 0 plane. It's default configuration places the bottom face *on* the z = 0 plane with the origin contained in the bottom face.

In representation, the edge ordering is arbitrary (i.e., an edge can be defined as (vᵢ, vⱼ) or (vⱼ, vᵢ). However, given an arbitrary definition of edges, the *faces* have been defined to have a specific winding which causes e₀ × e₁ to point inwards or outwards for that face. This allows us to explicitly fully exercise the functionality for computing an outward normal.
  - face 0: points outward
  - face 1: points inward (requires flipping)
  - face 2: points inward (requires flipping)
  - face 3: points outward

All property accessors are *mutable*.
*/

Please confirm these directions.

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

    }
  };
  EquilateralTetrahedron p1;

This needs some documentation. The significance of each test should be captured.

Here's my take as near as I can tell:

p1-p4: The tetrahedron is positioned so that the origin is placed on each face (some requiring flipping, some not)
p5: Origin is well within
p6: Origin on the bottom face, but the tetrahedron is too small; it must evaluate all vertices and do a min/max comparison.
p7: Small tetrahedron with origin properly inside.

I think you should have a variation of p6 where the origin lies on a side face.

Also, is it worth testing it where the origin is one of the vertices? If not, you should
a) Indicate why not and
b) (possibly) update the documentation of faceNormalPointingOutward to indicate that the function shouldn't be called with the origin coincident with a polytope vertex.

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

  EquilateralTetrahedron p4(-1.0 / 6, -1.0 / (6 * std::sqrt(3)),
                            -std::sqrt(6) / 9);
  CheckTetrahedronFaceNormal(p3);

Typo: p4 is not tested, p3 is tested twice.

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

  EquilateralTetrahedron p1(0, 0, -0.1);
  // The computation on Mac is very imprecise, thus the tolerance is big.
  const ccd_real_t tol = 3E-5;

FYI BLECH! Mac!

Can you put a TODO on this? As soon as we resolve #291 this should be cranked up.

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

    CheckSupportEPADirection(
        p2.polytope(), reinterpret_cast<const ccd_pt_el_t*>(p2.e(0)),
        Vector3<ccd_real_t>(0, -2 * std::sqrt(2) / 3, 1.0 / 3), tol);

BTW Should I be surprised that these values changed? It's particularly interesting because the tetrahedron definition didn't change, but the solution did.

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

}

template <typename T>

BTW If you take the suggestion of the Polytope class above, the template goes away here and it just becomes a const Polytope& polytope parameter. :)

If you choose not to do that, I'd swap T for PolytopeType and document it.

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

void CheckComputeVisiblePatchRecursive(
    const T& polytope, ccd_pt_face_t* face,
    const std::vector<int>& edge_indices, const ccd_vec3_t& new_vertex,

The significance of the edge_indices parameter needs to be documented.

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

template <typename T>
void CheckComputeVisiblePatch(
    const T& polytope, ccd_pt_face_t* face, const ccd_vec3_t& new_vertex,

ComputeVisiblePatch() takes a reference to a face. And if you change to a common Polytope class, then face(i) will also return a reference. So, I'd say make it references all around.

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

  p.v[2] = 1.1;
  const std::unordered_set<int> empty_set;
  CheckComputeVisiblePatchRecursive(hex, hex.f(0), {0}, p, {0}, {0}, empty_set);

I really don't like these two tests. Same polytope, same face, same point, but different output results...all because of the hard to grok {0} parameter. (And it's a parameter which appears a lot with different interpretations. This is one of the reasons why I don't feel that the exercising the recursive work function direction provides much value.

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

// @param map_feature1_to_feature2[out] Maps a feature in feature1_list to
// a feature in feature2_list.
// @note The features in feature1_list should be distince, so are in

BTW s/distince/distinct

Also, I don't fully understand the note. Is it that the features in each list must be unique?

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

  // e(1) connects two vertices v(1) and v(2), make sure that v(1).v1 - v(1).v2
  // = v(1).v, also v(2).v1 - v(2).v2 = v(2).v
  tetrahedron.v(1)->v.v1.v[0] = 1;

BTW

ccdVec3Set(&tetrahedron.v(1)->v.v1, 1, 2, 3);

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 79 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Hmm, I think we do not want to make it const reference here, since we will remove and add features into the polytope, and the existing vertices/edges/faces will be changed.

FYI This kind of confuses me -- if you're going to remove/add features into the polytope, then it's not still tetrahedron. :)

That said, here's what I'd softly suggest that all of these "getters" return non-const references. Returning a pointer that you know can't be null sends mixed messages.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 89 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

As is turns out, I'm not big fan of how this turned out. New recommendation:

Create Polytope class with the getters you have on this.
1. Add a virtual void DoInitialize() = 0 method (invoked in constructor).
2. Add various getters I suggested above.
3. This will require changing v_, e_, and f_ to vectors so that they can be sized by the sub-class.
4. Leave polytope a raw pointer that gets deleted in the Polytope class's destructor (with the call to ccdPtDestroy()) and the following documentation:

ccdPtDestroy() destroys the vertices, edges, and faces, contained in the polytope, allowing the polytope itself to be subsequently deleted.

Sub-class Tetrahedron and Hexagram from Polytope, overriding the DoInitialize() method.

How's that sound?

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 108 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

For some reasons 1E-7 fails on Mac CI.

Did you try eps_34()? This really needs to scale with the precision of ccd_real_t.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 200 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

Face winding?

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 307 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

CheckFloodFillSilhouetteRecursive is more granular than CheckFloodFillSilhouette, so I think we should keep it to improve our test granularity.

I'll accept it -- I simply doubt the value of the granularity in this particular context compared with the complexity of the testing code.

One of the problems I have with this test design is how much the full function is being exercised compared to the recursive function. The recursive function has branches and conditions and should be exercised in many different ways. The main function is completely deterministic. It has one purpose, initialize the recursive function (making sure the initial face ends up in the patch). One test should be sufficient for that purpose. As long as it does that, then its correctness can depends 100% on the recursive function. So, I believe you can be more strategic about these tests.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 328 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Hmm, we have another test that only a single face is visible, and other faces that multiple faces are visible.

I hope the documentation at the beginning of the test is clear about the intention of the test?

I guess its alright -- I just hate looking at the gtest output report that "Test1" failed. The failure report provides little info in that case.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 475 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

I think the 1-to-1 is handled in this line. Since we already have onto, and the size of feature1 is the size as feature2, as checked above, then feature1 and feature2 has to be one on one.

I disagree...feature1 contains the keys. This test confirms that you have as many keys in the map as you had in the provided feature 1 list. It doesn't guarantee that every entry in feature 2 is mapped to.

And the first question: are there as many keys as entries in feature1, is satisfied by EXPECT_TRUE(found_match) which is invoked once for each entry in feature1. What am I missing? It may be that you're assuming that for a given feature f1, cmp_feature can return true for at most one feature f2 from feature2. Yes? That assumption isn't enforced.

test/narrowphase/detail/convexity_based_algorithm/test_epa.cpp, line 751 at r2 (raw file):

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

I don't see it.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-19T04:04:54Z

Is this ready for another review? I know there was a fair amount of churn due to more bugs being found.

Review status: 1 of 7 files reviewed, 17 unresolved discussions (waiting on @SeanCurtis-TRI and @hongkai-dai)

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

  }
  // Form a tetrahedron with abc as one face, pick either d or d2, based
  // on which one has larger distance to the face abc.

Is there a rationale for picking the most distant? If so, it should be documented. If not, I'd say just pick one.

Comments from Reviewable

… simplex is an edge.

… and the un-normalized support direction.

hongkai-dai · 2018-06-27T17:16:16Z

Review status: 3 of 5 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW s/triagnel/triangle

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

FYI I like the fact that, below, you used mathematical symbols. And, even here, you introduced the "transpose" operator. I definitely prefer: aᵀ · (b × c) = 0 as better documentation. Same for down below (a × b)ᵀ · (b × c) ≥ 0.

Humorously, my first thought was "this dot product could be zero if aᵀ is the zero vector...that's a problem!" Then I realized, nope, that's proof that the origin lies on the triangle. :) Similarly, if b or c are the origin.

Done.

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

Previously, SeanCurtis-TRI (Sean Curtis) wrote…

BTW s/points to/point in

Done.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-27T17:25:56Z

Reviewed 2 of 2 files at r9.
Review status: complete! all files reviewed, all discussions resolved

Comments from Reviewable

hongkai-dai · 2018-06-27T22:02:56Z

+@sherm1 for platform review please.

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

Comments from Reviewable

sherm1 · 2018-06-28T03:21:31Z

Review status: all files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp, line 311 at r9 (raw file):

  X_WF.setIdentity();
  X_WF.linear() =
      Eigen::AngleAxis<S>(0.1 * M_PI, Vector3<S>::UnitZ()).toRotationMatrix();

This doesn't compile on Windows because M_PI is not part of the C++ standard. Instead used const S pi = fcl::constants<S>::pi();.

Comments from Reviewable

hongkai-dai · 2018-06-28T03:31:55Z

Review status: 4 of 5 files reviewed, 1 unresolved discussion (waiting on @SeanCurtis-TRI and @sherm1)

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_signed_distance.cpp, line 311 at r9 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

This doesn't compile on Windows because M_PI is not part of the C++ standard. Instead used const S pi = fcl::constants<S>::pi();.

Done.

Comments from Reviewable

SeanCurtis-TRI · 2018-06-28T14:08:46Z

Reviewed 1 of 1 files at r10.
Review status: all files reviewed, 1 unresolved discussion (waiting on @sherm1)

Comments from Reviewable

sherm1 · 2018-06-28T17:15:49Z

Checkpoint.

Review status: all files reviewed, 18 unresolved discussions (waiting on @sherm1 and @hongkai-dai)

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

/**
 * Computes the normal vector of a triangular face on a polytope, and the normal

Minor: I believe this method is returning the unnormalized face normal. A reader might assume the normal is normalized (since that is so common) so it would be good to be explicit here.

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

 * @param[in] simplex The simplex (with three vertices) that contains the
 * origin.
 * @param[out] pt The polytope (tetrahedron) that also contains the origin.

Minor: from the code it looks as though you are expecting pt to be empty at the start but it doesn't say so here. If it isn't empty on entry then the result won't be a tetrahedron.

Also, do you want to clarify that the returned polytope has the origin on its face rather than the interior? On first reading I assume the polytope would be constructed with the origin inside.

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

 * @param[in] simplex The simplex (with three vertices) that contains the
 * origin.
 * @param[out] pt The polytope (tetrahedron) that also contains the origin.

Minor: consider calling this "polytope" rather than "pt" as you did in faceNormalPointingOutward() below. I kept reading "pt" as "point".

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

 * -2 on non-recoverable failure (mostly due to memory allocation bug).
 */
static int Convert2SimplexToTetrahedron(const void* obj1, const void* obj2,

Minor: (META) Per f2f I think we should stick to FCL's informal style of initialLowerCaseLetter() function names. (Would be good to establish a formal style guide but not as part of this PR!)

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

  *nearest = NULL;

  a = ccdSimplexPoint(simplex, 0);

Minor: can you conveniently assert that the provided simplex is a triangle, and that the supplied polytope is empty?

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

    }
    dist = ccdVec3PointTriDist2(ccd_vec3_origin, &a->v, &c->v, &d->v, NULL);
    if (ccdIsZero(dist)){

Minor: Should these be mutually exclusive cases? Currently even when it finds the origin in one of the faces it keeps checking the other faces. Is that necessary?

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

 * @param[in] polytope The polytope on which the face lives. We assume that the
 * origin also lives inside the polytope.
 * @param[in] face The face for which the unit length normal vector is computed.

Minor: here it says "unit length normal vector" but I don't see that in the code?

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

 * @param[in] face The face for which the unit length normal vector is computed.
 * @retval dir The vector normal to the face, and points outward from the
 * polytope.

Minor: unit vector or not?

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

  // most distant vertex.

  // For these tests, we use the arbitrary distance of 1 cm as a "reliable"

Minor: do we know that lengths in FCL are always in meters (we do have that rule in Drake)? If not, better call this ".01" length units rather than "cm". (And below)

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

// Return true if the point @p pt is on the outward side of the half-plane, on
// which the triangle @p f lives. Notice if the point @p pt is exactly on the

Nit: in ComputeVisiblePatch() you used backticks for parameter names but here you used @p. I think the backticks are easier to read but either way it should be consistent.

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

// Return true if the point @p pt is on the outward side of the half-plane, on
// which the triangle @p f lives. Notice if the point @p pt is exactly on the
// half-plane, the return if false.

Nit: if -> is

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

// half-plane, the return if false.
// @param f A triangle on a polytope.
// @param pt A point.

Minor: here pt is short for "point" which seems reasonable, except that above you used pt as short for "polytope". Should use abbreviations for only a single meaning or spell out "point" and "polytope" always.

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

    bool has_edge_internal = false;
    for (int i = 0; i < 3; ++i) {
      if (internal_edges.count(f->edge[i]) == 1) {

Nit: I always find count()==1 hard to read correctly. It looks like you are checking for exactly one internal edge, but actually this is boilerplate for internal_edges.contains(edge) that works only because internal_edges is a set (which isn't visible at the call site). Please consider a comment like "// Using "count" to check if edge i is present in internal_edges set." (once for the whole method would be enough).

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

      border_edges->insert(f.edge[edge_index]);
      return;
    } else {

Minor: you don't need an "else" here since the "if" returns unconditionally. The method would be easier to read since the rest of the code could be un-indented.

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

  } else {
    // Face f is visible (prerequisite), and g has been previously
    // marked visible (via a different path); their shared age should be marked

Nit: age -> edge

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

/** Defines the "visible" patch on the given convex `polytope` (with respect to
 the given `query_vertex`).

Minor: consider saying here that the query_vertex is assumed to be outside the polytope.

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

  assert(isOutsidePolytopeFace(&polytope, &f, &query_point));
  visible_faces->insert(&f);
  for (int i = 0; i < 3; ++i) {

Minor: not obvious here that you are iterating over edges. Consider a comment and/or renaming the counter from i to edge_index or whatever.

Comments from Reviewable

hongkai-dai · 2018-06-28T18:54:27Z

Review status: 3 of 5 files reviewed, 2 unresolved discussions (waiting on @SeanCurtis-TRI, @sherm1, and @hongkai-dai)

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: I believe this method is returning the unnormalized face normal. A reader might assume the normal is normalized (since that is so common) so it would be good to be explicit here.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: from the code it looks as though you are expecting pt to be empty at the start but it doesn't say so here. If it isn't empty on entry then the result won't be a tetrahedron.

Also, do you want to clarify that the returned polytope has the origin on its face rather than the interior? On first reading I assume the polytope would be constructed with the origin inside.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: consider calling this "polytope" rather than "pt" as you did in faceNormalPointingOutward() below. I kept reading "pt" as "point".

Done. Good call.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: (META) Per f2f I think we should stick to FCL's informal style of initialLowerCaseLetter() function names. (Would be good to establish a formal style guide but not as part of this PR!)

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: can you conveniently assert that the provided simplex is a triangle, and that the supplied polytope is empty?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: Should these be mutually exclusive cases? Currently even when it finds the origin in one of the faces it keeps checking the other faces. Is that necessary?

Very likely they are mutually exclusive. But I haven't touched this function simplexToPolytope4 yet. I suspect we can optimize (and probably rewrite) simplexToPolytope4, but I would like to do it in a separate PR.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: here it says "unit length normal vector" but I don't see that in the code?

Done. Good catch.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: unit vector or not?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: do we know that lengths in FCL are always in meters (we do have that rule in Drake)? If not, better call this ".01" length units rather than "cm". (And below)

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: in ComputeVisiblePatch() you used backticks for parameter names but here you used @p. I think the backticks are easier to read but either way it should be consistent.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: if -> is

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: here pt is short for "point" which seems reasonable, except that above you used pt as short for "polytope". Should use abbreviations for only a single meaning or spell out "point" and "polytope" always.

Done, use ccd_pt_t* polytope in this file.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: I always find count()==1 hard to read correctly. It looks like you are checking for exactly one internal edge, but actually this is boilerplate for internal_edges.contains(edge) that works only because internal_edges is a set (which isn't visible at the call site). Please consider a comment like "// Using "count" to check if edge i is present in internal_edges set." (once for the whole method would be enough).

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: you don't need an "else" here since the "if" returns unconditionally. The method would be easier to read since the rest of the code could be un-indented.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: age -> edge

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: consider saying here that the query_vertex is assumed to be outside the polytope.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: not obvious here that you are iterating over edges. Consider a comment and/or renaming the counter from i to edge_index or whatever.

Done.

Comments from Reviewable

sherm1 · 2018-06-28T21:55:48Z

Checkpoint: made it through gjk_libccd-inl.h!

Just a note: much of this code is not templatized so could be in a .cpp file rather than a header. This would require some build system modifications which I am not proposing for this PR!

Reviewed 1 of 2 files at r11.
Review status: 4 of 5 files reviewed, 18 unresolved discussions (waiting on @SeanCurtis-TRI, @sherm1, and @hongkai-dai)

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

I don't see the asserts?

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Very likely they are mutually exclusive. But I haven't touched this function simplexToPolytope4 yet. I suspect we can optimize (and probably rewrite) simplexToPolytope4, but I would like to do it in a separate PR.

How about adding a TODO here?

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

Previously, hongkai-dai (Hongkai Dai) wrote…

Done.

Still an @p pt here.

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

 * origin.
 * @param[out] polytope The polytope (tetrahedron) that also contains the origin
 * on one of its face. At input, the polytope should be empty.

Nit: face -> faces

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

 * vector points outward from the polytope. Notice we assume that the origin
 * lives within the polytope, and the normal vector may not have the unit
 * length.

Nit: "~~the~~ unit length" -> "unit length"

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

    bool has_edge_internal = false;
    for (int i = 0; i < 3; ++i) {
      // Sinve internal_edges is a set, internal_edges.count(e) means that e

Nit: Sinve -> Since

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

/** Expands the polytope by adding a new vertex @p newv to the polytope. The
 * new polytope is the convex hull of the new vertex together with the old

Nit: here you have an initial * at the start of each comment line but above you used the no-punctuation form. Personally I like it without the extra noise, but either way it should be consistent within a file.

Also there is both @p newv and the backticks form (below) in this comment.

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

 * interior to the new convex hull are discarded.
 * @param[in/out] polytope The polytope.
 * @param[in] el A feature that is visible from the point `newv`. A face is

Minor: from the code it looks like point features are not allowed. So this should say that el is "a face or edge".

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

 * closest to the origin on the boundary of the polytope. If the feature is
 * an edge, and the two neighbouring faces of that edge are not co-planar, then
 * the origin must lie on that edge.

Minor: I found the last two sentences confusing. Are they modifying the case where the feature is an edge, or is this a property that every el must have. If the latter, move to the first sentence like "el A feature that is visible from newv and contains the polytope boundary point that is closest to the origin".

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

    } else {
      throw std::logic_error(
          "This should not happen. Both the nearest point and the new vertex "

Minor: consider starting this message with "FCL expandPolytope(): " or something like that so that there will be some hint of where it came from (FCL is often buried in some larger application like Gazebo so it wouldn't necessarily be obvious that this is an FCL problem).

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

 * expand the polytope.
 * @param polytope The polytope contained in A ⊖ B.
 * @param nearest_pt The nearest point on the boundary of the polytope to the

Minor: this seems misleading -- I believe this parameter is a feature not a point. Could you call it feature_with_nearest_pt or even nearest_feature?

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

      case CCD_PT_VERTEX: {
        throw std::runtime_error(
            "The nearest point to the origin is a vertex of the polytope. This "

Nit: logic_error. Also consider starting the message with "FCL supportEPADirection(): "

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

    // The "dist" field in ccd_pt_el_t is actually the square of the distance.
    const ccd_real_t dist = std::sqrt(nearest_pt->dist);
    ccdVec3Scale(&dir, ccd_real_t(1.0) / dist);

Minor: this relies on sqrt(dist) actually being the right normal, which really should be asserted. Consider instead just computing ccdVec3Normalize(&dir) for all cases in which case you could move that to the bottom just before the return and only do it once in the function. It doesn't seem like there would be much cost since you have to do the expensive operations sqrt and divide here anyway.

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

 * @param[in] ccd The libccd solver.
 * @param[in] el The current nearest point on the boundary of the polytope to
 * the origin.

Minor: this is a feature, not a point. Maybe "The polytope boundary feature that contains the point nearest the origin." ?

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

 * @param[out] out The next support point.
 * @retval status If the next support point is found, then returns 0; otherwise
 * returns -1. There are several reasons why the newxt support point is not

Nit: newxt -> next

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

 * penetration point on B.
 * @param[in] nearest The point that is nearest to the origin on the boundary of
 * the polytope.

Minor: again this is a feature not a point.

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

 * the polytope.
 * @param[out] p1 the deepest penetration point on A.
 * @param[out] p2 the deepest penetration point on B.

Minor: how are these points measured & expressed? p1 from Ao expressed in A and p2 from Bo expressed in B? Or are they points in the world frame?

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

    } else {
      throw std::logic_error(
          "Unsupported point type. The closest point should be either a "

Minor: point -> feature. Also, consider starting this message with "FCL penEPAPosClosest(): ".

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

    ccd_vec3_t p;
    ccdVec3Copy(&p, &(nearest->witness));
    extractClosestPoints(&s, p1, p2, &p);

Nit: could the above three lines be replaced with just
extractClosestPoints(&s, p1, p2, &nearest->witness);

Comments from Reviewable

sherm1 · 2018-06-28T23:04:23Z

Wow! test_gjk_libccd-inl_epq.cpp is the most elaborate set of unit tests I've every seen! Thanks so much for doing that, Hongkai. pending a few minor comments (and check that CI fail is just the expected one).

Reviewed 1 of 4 files at r2, 1 of 3 files at r8, 1 of 1 files at r10, 1 of 2 files at r11.
Review status: all files reviewed, 19 unresolved discussions (waiting on @hongkai-dai)

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

          throw std::logic_error(
              "There should be only one element in feature2_list that matches "
              "with an element in feature1_list.");

Minor: consider:

GTEST_FAIL() << "There should be only one element in feature2_list that matches "
              "with an element in feature1_list.";

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

               edges_expected[2]);

  ComparePolytope(&polytope, &polytope_expected, 1E-3);

Minor: just checking -- is 1e-3 really all we can expect here?

Comments from Reviewable

hongkai-dai · 2018-06-29T06:35:57Z

Review status: 3 of 5 files reviewed, 3 unresolved discussions (waiting on @sherm1 and @hongkai-dai)

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

Previously, sherm1 (Michael Sherman) wrote…

I don't see the asserts?

I put it in line 606 and line 607 https://github.com/hongkai-dai/fcl/blob/box_box_test/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L606~L607

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

Previously, sherm1 (Michael Sherman) wrote…

How about adding a TODO here?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Still an @p pt here.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: face -> faces

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: "~~the~~ unit length" -> "unit length"

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: Sinve -> Since

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: here you have an initial * at the start of each comment line but above you used the no-punctuation form. Personally I like it without the extra noise, but either way it should be consistent within a file.

Also there is both @p newv and the backticks form (below) in this comment.

Done. I changed the documentation to to start with * at each line, since that is what my editor vim default to.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: from the code it looks like point features are not allowed. So this should say that el is "a face or edge".

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: I found the last two sentences confusing. Are they modifying the case where the feature is an edge, or is this a property that every el must have. If the latter, move to the first sentence like "el A feature that is visible from newv and contains the polytope boundary point that is closest to the origin".

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: consider starting this message with "FCL expandPolytope(): " or something like that so that there will be some hint of where it came from (FCL is often buried in some larger application like Gazebo so it wouldn't necessarily be obvious that this is an FCL problem).

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: this seems misleading -- I believe this parameter is a feature not a point. Could you call it feature_with_nearest_pt or even nearest_feature?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: logic_error. Also consider starting the message with "FCL supportEPADirection(): "

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: this is a feature, not a point. Maybe "The polytope boundary feature that contains the point nearest the origin." ?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: newxt -> next

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: again this is a feature not a point.

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: how are these points measured & expressed? p1 from Ao expressed in A and p2 from Bo expressed in B? Or are they points in the world frame?

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: point -> feature. Also, consider starting this message with "FCL penEPAPosClosest(): ".

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Nit: could the above three lines be replaced with just
extractClosestPoints(&s, p1, p2, &nearest->witness);

I guess not, the type of nearest is const pt_el_t*, so nearest->witness should be const. On the other hand, extractClosestPoints expect a non-const input https://github.com/hongkai-dai/fcl/blob/box_box_test/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L1518

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: consider:

GTEST_FAIL() << "There should be only one element in feature2_list that matches "
              "with an element in feature1_list.";

Done.

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: just checking -- is 1e-3 really all we can expect here?

You are right, we can get better accuracy.

Comments from Reviewable

sherm1 · 2018-06-29T15:00:42Z

Reviewed 2 of 2 files at r12.
Review status: all files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)

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

Previously, hongkai-dai (Hongkai Dai) wrote…

I put it in line 606 and line 607 https://github.com/hongkai-dai/fcl/blob/box_box_test/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h#L606~L607

Oh, apparently I'm blind!

Comments from Reviewable

hongkai-dai · 2018-07-02T16:43:07Z

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

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

Previously, sherm1 (Michael Sherman) wrote…

Minor: this relies on sqrt(dist) actually being the right normal, which really should be asserted. Consider instead just computing ccdVec3Normalize(&dir) for all cases in which case you could move that to the bottom just before the return and only do it once in the function. It doesn't seem like there would be much cost since you have to do the expensive operations sqrt and divide here anyway.

Done.

Comments from Reviewable

sherm1 · 2018-07-02T20:17:51Z

Reviewed 1 of 1 files at r13.
Review status: complete! all files reviewed, all discussions resolved

Comments from Reviewable

…ollision-library#305) Rewrite FCL's EPA implementation and add many unit tests.

wxmerkt · 2018-07-28T14:38:34Z

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

+    } else if (isOutsidePolytopeFace(polytope, f[1], &newv->v)) {
+      start_face = f[1];
+    } else {
+      throw std::logic_error(


@hongkai-dai Is this really not meant to happen? I just got this error on a long running optimisation job.

Could you send me the code to reproduce the bug?

I think if the starting polytope is non-degenerate (namely it does not have two neighbouring faces being co-planar), this case should not happen.

I am exclusively using shape primitives (boxes, cylinders, spheres). The case that throws this error is a box vs cylinder (radius 0.18, length 0.18). I will open a new issue with more information on how to reproduce.

hongkai-dai force-pushed the box_box_test branch 2 times, most recently from 45adedf to dcdb239 Compare June 10, 2018 17:43

hongkai-dai assigned SeanCurtis-TRI Jun 10, 2018

hongkai-dai force-pushed the box_box_test branch from e91f23b to 6cf6037 Compare June 12, 2018 04:25

hongkai-dai force-pushed the box_box_test branch from f79ec52 to ca09f07 Compare June 13, 2018 00:19

hongkai-dai force-pushed the box_box_test branch from dd242f1 to 757feca Compare June 14, 2018 03:55

hongkai-dai added 15 commits June 25, 2018 14:54

Add a box-to-box touching contact, EPA gives me the wrong closest point.

cd4f30e

Now EPA returns the correct deepest penetration point when the active…

05ea427

… simplex is an edge.

Add the branch to generate the simplexof size 3 from nearest.

b03d846

Add the code to handle closest point on the boundary of the polytope,…

483fda5

… and the un-normalized support direction.

Work on silhouette flood hill function.

b929ed4

change the expandPolytope with silhouette algorithm.

0bef0ee

Work on the unit test for EPA algorithm.

2de6bfa

Start to test expandPolytope.

c60a65b

Add a function to delete the vertex when expanding the polytope.

da506fa

Continue to test expandPolytope.

5699d91

Now the test passes.

6ab6e49

Fix CI bugs with different compilers.

61c7230

Relax the tolerance for Mac.

5417f20

Add a test on sampledEPADirection

a7cd9fd

Remove unused code.

e434429

more unicode

db6ec87

hongkai-dai assigned sherm1 Jun 27, 2018

Address Sherm's comment

9a8cd00

Address Sherm's comments.

657254a

Addres Sherm's comments.

64c9285

hongkai-dai force-pushed the box_box_test branch from cd1a21c to 64c9285 Compare June 29, 2018 06:35

Address Sherm's comments.

f322f5c

hongkai-dai force-pushed the box_box_test branch from 0618811 to f322f5c Compare July 2, 2018 16:42

sherm1 merged commit 07c8480 into flexible-collision-library:master Jul 3, 2018

nicovanduijn pushed a commit to nicovanduijn/fcl that referenced this pull request Jul 5, 2018

Fix FCL's EPA implementation, when calling libccd solver. (flexible-c…

3507df9

…ollision-library#305) Rewrite FCL's EPA implementation and add many unit tests.

amcastro-tri mentioned this pull request Jul 6, 2018

Distance computed in EPA incorrectly uses an un-normalized vector #301

Closed

This was referenced Jul 6, 2018

Fix extractClosestPoint method, reset dir variable #251

Closed

Incorrect nearest points when using distance(...) #275

Open

fcl::distance() erroneous penetration depth query when using solvers different than INDEP #244

Open

wxmerkt reviewed Jul 28, 2018

View reviewed changes

wxmerkt mentioned this pull request Jul 28, 2018

libccd EPA expandPolytope: "this should not happen" #319

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix FCL's EPA implementation, when calling libccd solver. #305

Fix FCL's EPA implementation, when calling libccd solver. #305

hongkai-dai commented Jun 10, 2018 •

edited by sherm1

Loading

hongkai-dai commented Jun 10, 2018

SeanCurtis-TRI commented Jun 11, 2018

hongkai-dai commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

hongkai-dai commented Jun 13, 2018

SeanCurtis-TRI commented Jun 13, 2018

SeanCurtis-TRI commented Jun 19, 2018

hongkai-dai commented Jun 27, 2018

SeanCurtis-TRI commented Jun 27, 2018

hongkai-dai commented Jun 27, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 28, 2018

SeanCurtis-TRI commented Jun 28, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 28, 2018

sherm1 commented Jun 28, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 29, 2018

sherm1 commented Jun 29, 2018

hongkai-dai commented Jul 2, 2018

sherm1 commented Jul 2, 2018

wxmerkt Jul 28, 2018

hongkai-dai Jul 28, 2018

wxmerkt Jul 28, 2018

Fix FCL's EPA implementation, when calling libccd solver. #305

Fix FCL's EPA implementation, when calling libccd solver. #305

Conversation

hongkai-dai commented Jun 10, 2018 • edited by sherm1 Loading

hongkai-dai commented Jun 10, 2018

SeanCurtis-TRI commented Jun 11, 2018

hongkai-dai commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

SeanCurtis-TRI commented Jun 12, 2018

hongkai-dai commented Jun 13, 2018

SeanCurtis-TRI commented Jun 13, 2018

SeanCurtis-TRI commented Jun 19, 2018

hongkai-dai commented Jun 27, 2018

SeanCurtis-TRI commented Jun 27, 2018

hongkai-dai commented Jun 27, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 28, 2018

SeanCurtis-TRI commented Jun 28, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 28, 2018

sherm1 commented Jun 28, 2018

sherm1 commented Jun 28, 2018

hongkai-dai commented Jun 29, 2018

sherm1 commented Jun 29, 2018

hongkai-dai commented Jul 2, 2018

sherm1 commented Jul 2, 2018

wxmerkt Jul 28, 2018

Choose a reason for hiding this comment

hongkai-dai Jul 28, 2018

Choose a reason for hiding this comment

wxmerkt Jul 28, 2018

Choose a reason for hiding this comment

hongkai-dai commented Jun 10, 2018 •

edited by sherm1

Loading