ComputeVisiblePatch tracks all visited faces (#435)

The previous version of the code only tracked those faces that were
explicitly known to be visible. This led to two problems:

 1. A "hidden" face could be visited multiple times and the calculation to
    determine its visibility could be needlessly repeated.
 2. The different visits could actually produce *different* results leading
    to inherently contradictory classifications of edges.

In this case, we track every face we've visited and lock its classification
into the first classification computed.

Furthermore, we extend the sanity check to look for duplicate
classificaitons and tweak documentation.

include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h

@@ -1126,6 +1126,8 @@ static bool ComputeVisiblePatchRecursiveSanityCheck(
     const std::unordered_set<ccd_pt_face_t*>& visible_faces,
     const std::unordered_set<ccd_pt_edge_t*>& internal_edges) {
   ccd_pt_face_t* f;
+  // TODO(SeanCurtis-TRI): Instead of returning false, have this throw the
+  //  exception itself so that it can provide additional information.
   ccdListForEachEntry(&polytope.faces, f, ccd_pt_face_t, list) {
     bool has_edge_internal = false;
     for (int i = 0; i < 3; ++i) {
@@ -1157,83 +1159,130 @@ static bool ComputeVisiblePatchRecursiveSanityCheck(
       }
     }
   }
+  for (const auto b : border_edges) {
+    if (internal_edges.count(b) > 0) {
+      return false;
+    }
+  }
   return true;
 }
 #endif
+
+/** Attempts to classify the given `edge` as a border edge, confirming it hasn't
+ already been classified as an internal edge.
+ @param edge              The edge to classify.
+ @param border_edges      The set of edges already classified as border.
+ @param internal_edges    The set of edges already classified as internal.
+ @throws ThrowFailedAtThisConfiguration if the edge is being re-classified.
+ */
+static void ClassifyBorderEdge(ccd_pt_edge_t* edge,
+                        std::unordered_set<ccd_pt_edge_t*>* border_edges,
+                        std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
+  border_edges->insert(edge);
+  if (internal_edges->count(edge) > 0) {
+    FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+        "An edge is being classified as border that has already been "
+        "classifed as internal");
+  }
+}
+
+/** Attempts to classify the given `edge` as an internal edge, confirming it
+ hasn't already been classified as a border edge.
+ @param edge              The edge to classify.
+ @param border_edges      The set of edges already classified as border.
+ @param internal_edges    The set of edges already classified as internal.
+ @throws ThrowFailedAtThisConfiguration if the edge is being re-classified.
+ */
+static void ClassifyInternalEdge(ccd_pt_edge_t* edge,
+                          std::unordered_set<ccd_pt_edge_t*>* border_edges,
+                          std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
+  internal_edges->insert(edge);
+  if (border_edges->count(edge) > 0) {
+    FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
+        "An edge is being classified as internal that has already been "
+        "classified as border");
+  }
+}
+
 /**
  * This function contains the implementation detail of ComputeVisiblePatch()
  * function. It should not be called by any function other than
  * ComputeVisiblePatch().
+ * The parameters are documented as in ComputeVisiblePatch() but has the
+ * additional parameter: hidden_faces. Every visited face will be categorized
+ * explicitly as either visible or hidden.
+ * By design, whatever classification is _first_ given to a face (visible or
+ * hidden) it can't change. This doesn't purport that the first classification
+ * is more correct than any subsequent code that might otherwise classify it
+ * differently. This simply precludes the possibility of classifying edges
+ * multiple ways.
  */
 static void ComputeVisiblePatchRecursive(
     const ccd_pt_t& polytope, ccd_pt_face_t& f, int edge_index,
     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_face_t*>* hidden_faces,
     std::unordered_set<ccd_pt_edge_t*>* internal_edges) {
-  /*
-  This function will be called recursively. It first checks if the face `g`
-  neighouring face `f` along the common edge `f->edge[edge_index]` can be seen
-  from the point `query_point`. If this face g cannot be seen, then stop.
-  Otherwise, we continue to check the neighbouring faces of g, by calling this
-  function recursively.
-  */
-  ccd_pt_face_t* g = f.edge[edge_index]->faces[0] == &f
-                         ? f.edge[edge_index]->faces[1]
-                         : f.edge[edge_index]->faces[0];
-  if (visible_faces->count(g) == 0) {
-    // g is not a visible face
-    if (!isOutsidePolytopeFace(&polytope, g, &query_point)) {
-      // Cannot see the neighbouring face from the new vertex.
-
-      if (!triangle_area_is_zero(query_point,
-                                 f.edge[edge_index]->vertex[0]->v.v,
-                                 f.edge[edge_index]->vertex[1]->v.v)) {
-        // If query point is outside of the face g, and the triangle
-        // (query_point, v[0], v[1]) has non-zero area, then the edge
-        // f.edge[edge_index] is a border edge, and we will connect the query
-        // point with this border edge to form a triangle. Otherwise, this edge
-        // is not a border edge.
-        border_edges->insert(f.edge[edge_index]);
-        return;
-      }
-    }
-    // We regard the edge f.edge[edge_index] not as an internal edge (not a
-    // border edge), if it satisfies one of the following two conditions
-    // 1. The face g is visible to the query point.
-    // 2. The triangle formed by the edge and the query point has zero area.
-    // The first condition is obvious. Here we explain the second condition:
-    // For this triangle to have no area, the query point must be co-linear with
-    // a candidate border edge. That means it is simultaneously co-planar with
-    // the two faces adjacent to that edge. But to be in this branch, one face
-    // was considered to be visible and the other to not be visible -- an
-    // inconsistent classification.
-
-    // The solution is to unify that classification. We can consider both
-    // faces as being visible or not. If we were to consider them not
-    // visible, we would shrink the size of the visible patch (making this
-    // slightly faster), but would risk introducing co-planar faces into the
-    // polytope. We choose to consider both faces as being visible. At the
-    // cost of a patch boundary with more edges, we guarantee that we don't
-    // add co-planar faces.
-
-    // It may be that co-planar faces are permissible and a smaller
-    // patch is preferred. This is still an open problem.For now, we go with
-    // the "safe" choice.
+  ccd_pt_edge_t* edge = f.edge[edge_index];
+
+  ccd_pt_face_t* g = edge->faces[0] == &f ? edge->faces[1] : edge->faces[0];
+  assert(g != nullptr);
+
+  bool is_visible = visible_faces->count(g) > 0;
+  bool is_hidden = hidden_faces->count(g) > 0;
+  assert(!(is_visible && is_hidden));
+
+  // First check to see if face `g` has been classifed already.
+  if (is_visible) {
+    // Face f is visible (prerequisite), and g has been previously
+    // marked visible (via a different path); their shared edge should be
+    // marked internal.
+    ClassifyInternalEdge(edge, border_edges, internal_edges);
+    return;
+  }
+
+  if (is_hidden) {
+    // Face *has* already been classified as hidden; we just need to classify
+    // the edge.
+    ClassifyBorderEdge(edge, border_edges, internal_edges);
+    return;
+  }
+
+  // Face `g` has not been classified yet. Try to classify it as visible (i.e.,
+  // the `query_point` lies outside of this face).
+  is_visible = isOutsidePolytopeFace(&polytope, g, &query_point);
+  if (!is_visible) {
+    // Face `g` is *apparently* not visible from the query point. However, it
+    // might _still_ be considered visible. The query point could be co-linear
+    // with `edge` which, by definition means that `g` *is* visible and
+    // numerical error led to considering it hidden. We can detect this case
+    // if the triangle formed by edge and query point has zero area.
+    //
+    // It may be that the previous designation that `f` was visible was a
+    // mistake and now face `g` is inheriting that visible status. This is a
+    // conservative resolution that will prevent us from creating co-planar
+    // faces (at the cost of a longer border).
+    is_visible = triangle_area_is_zero(query_point, edge->vertex[0]->v.v,
+                                       edge->vertex[1]->v.v);
+  }
+
+  if (is_visible) {
     visible_faces->insert(g);
-    internal_edges->insert(f.edge[edge_index]);
+    ClassifyInternalEdge(edge, border_edges, internal_edges);
     for (int i = 0; i < 3; ++i) {
-      if (g->edge[i] != f.edge[edge_index]) {
+      if (g->edge[i] != edge) {
         // One of the neighbouring face is `f`, so do not need to visit again.
         ComputeVisiblePatchRecursive(polytope, *g, i, query_point, border_edges,
-                                     visible_faces, internal_edges);
+                                     visible_faces, hidden_faces,
+                                     internal_edges);
       }
     }
   } else {
-    // Face f is visible (prerequisite), and g has been previously
-    // marked visible (via a different path); their shared edge should be marked
-    // internal.
-    internal_edges->insert(f.edge[edge_index]);
+    // No logic could classify the face as visible, so mark it hidden and
+    // mark the edge as a border edge.
+    ClassifyBorderEdge(edge, border_edges, internal_edges);
+    hidden_faces->insert(g);
   }
 }
 
@@ -1284,12 +1333,15 @@ static void ComputeVisiblePatch(
   assert(visible_faces->empty());
   assert(internal_edges->empty());
   assert(isOutsidePolytopeFace(&polytope, &f, &query_point));
+  std::unordered_set<ccd_pt_face_t*> hidden_faces;
   visible_faces->insert(&f);
   for (int edge_index = 0; edge_index < 3; ++edge_index) {
     ComputeVisiblePatchRecursive(polytope, f, edge_index, query_point,
-                                 border_edges, visible_faces, internal_edges);
+                                 border_edges, visible_faces, &hidden_faces,
+                                 internal_edges);
   }
 #ifndef NDEBUG
+  // TODO(SeanCurtis-TRI): Extend the sanity check to include hidden_faces.
   if (!ComputeVisiblePatchRecursiveSanityCheck(
           polytope, *border_edges, *visible_faces, *internal_edges)) {
     FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
@@ -1308,14 +1360,14 @@ static void ComputeVisiblePatch(
  * @param[in/out] polytope The polytope.
  * @param[in] el A feature that is visible from the point `newv` and contains
  * the polytope boundary point that is closest to the origin. This feature
- * should be either a face or an edge. 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 neighbouring faces is visible. This
- * feature contains the point that is 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. The
- * feature should not be a vertex, as that would imply the two objects are in
- * touching contact, causing the algorithm to exit before calling
+ * should be either a face or an edge. A face is visible from a point outside
+ * the original polytope, if the point is on the "outer" side of the face. If an
+ * edge is visible from that point, at least one of its neighbouring faces is
+ * visible. This feature contains the point that is 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. The feature should not be a vertex, as that would imply the two
+ * objects are in touching contact, causing the algorithm to exit before calling
  * expandPolytope() function.
  * @param[in] newv The new vertex add to the polytope.
  * @retval status Returns 0 on success. Returns -2 otherwise.
@@ -1390,6 +1442,12 @@ static int expandPolytope(ccd_pt_t *polytope, ccd_pt_el_t *el,
     ccdPtDelEdge(polytope, e);
   }
 
+  // Note: this does not delete any vertices that were on the interior of the
+  // deleted patch. There are no longer any faces or edges that reference them.
+  // Technically, they are still part of the polytope but have no effect (except
+  // for footprint in memory). It's just as simple to leave them, knowing the
+  // whole polytope will be destroyed when we're done with the query.
+
   // A vertex cannot be obsolete, since a vertex is always on the boundary of
   // the Minkowski difference A ⊖ B.
   // TODO(hongkai.dai@tri.global): as a sanity check, we should make sure that

test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp

@@ -558,7 +558,7 @@ void CheckComputeVisiblePatchCommon(
     const Polytope& polytope,
     const std::unordered_set<ccd_pt_edge_t*>& border_edges,
     const std::unordered_set<ccd_pt_face_t*>& visible_faces,
-    const std::unordered_set<ccd_pt_edge_t*> internal_edges,
+    const std::unordered_set<ccd_pt_edge_t*>& internal_edges,
     const std::unordered_set<int>& border_edge_indices_expected,
     const std::unordered_set<int>& visible_face_indices_expected,
     const std::unordered_set<int> internal_edges_indices_expected) {
@@ -590,17 +590,23 @@ void CheckComputeVisiblePatchRecursive(
     const std::unordered_set<int>& internal_edges_indices_expected) {
   std::unordered_set<ccd_pt_edge_t*> border_edges;
   std::unordered_set<ccd_pt_face_t*> visible_faces;
+  std::unordered_set<ccd_pt_face_t*> hidden_faces;
   visible_faces.insert(&face);
   std::unordered_set<ccd_pt_edge_t*> internal_edges;
   for (const int edge_index : edge_indices) {
     libccd_extension::ComputeVisiblePatchRecursive(
         polytope.polytope(), face, edge_index, new_vertex, &border_edges,
-        &visible_faces, &internal_edges);
+        &visible_faces, &hidden_faces, &internal_edges);
   }
   CheckComputeVisiblePatchCommon(polytope, border_edges, visible_faces,
                                  internal_edges, border_edge_indices_expected,
                                  visible_face_indices_expected,
                                  internal_edges_indices_expected);
+
+  // Confirm that visible and hidden faces are disjoint sets.
+  for (const auto hidden_face : hidden_faces) {
+    EXPECT_EQ(visible_faces.count(hidden_face), 0u);
+  }
 }
 
 void CheckComputeVisiblePatch(