Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

ComputeVisiblePatch tracks all visited faces #435

Merged
merged 1 commit into from
Jan 31, 2020
Merged

ComputeVisiblePatch tracks all visited faces #435

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
188 changes: 123 additions & 65 deletions include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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(
Expand All @@ -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.
Expand Down Expand Up @@ -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
Expand Down
10 changes: 8 additions & 2 deletions test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -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(
Expand Down