diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 3f7e8e33b..4b1e6038a 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -1126,6 +1126,8 @@ static bool ComputeVisiblePatchRecursiveSanityCheck( const std::unordered_set& visible_faces, const std::unordered_set& 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* border_edges, + std::unordered_set* 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* border_edges, + std::unordered_set* 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* border_edges, std::unordered_set* visible_faces, + std::unordered_set* hidden_faces, std::unordered_set* 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 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 diff --git a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp b/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp index d87c18fd7..b8ea645d9 100644 --- a/test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp +++ b/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& border_edges, const std::unordered_set& visible_faces, - const std::unordered_set internal_edges, + const std::unordered_set& internal_edges, const std::unordered_set& border_edge_indices_expected, const std::unordered_set& visible_face_indices_expected, const std::unordered_set internal_edges_indices_expected) { @@ -590,17 +590,23 @@ void CheckComputeVisiblePatchRecursive( const std::unordered_set& internal_edges_indices_expected) { std::unordered_set border_edges; std::unordered_set visible_faces; + std::unordered_set hidden_faces; visible_faces.insert(&face); std::unordered_set 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(