Skip to content

Commit

Permalink
Address EPA failure from issue 493
Browse files Browse the repository at this point in the history 
 - Introduce two regression tests (the two posted in the issue).
 - Correct a few confusing whitepsace formatting issues.
 - Modify the validateNearestFeatureOfPolytopeBeingEdge() function in two
   ways
   - Distance from origin to face must now be greater than epsilon (instead
     of zero). Justificaiton for this change provided.
   - Test to determine if the edge *truly* is the nearest feature has been
     simplified and documentation updated. This led to the removal of the
     is_point_on_line_segment() function.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed Sep 10, 2020
1 parent bdfd995 commit e48faa8
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 46 deletions.
73 changes: 27 additions & 46 deletions include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -966,28 +966,6 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
return false;
}

/**
* Determines if the point P is on the line segment AB.
* If A, B, P are coincident, report true.
*/
static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a,
const ccd_vec3_t& b) {
if (are_coincident(a, b)) {
return are_coincident(a, p);
}
// A and B are not coincident, if the triangle ABP has non-zero area, then P
// is not on the line adjoining AB, and hence not on the line segment AB.
if (!triangle_area_is_zero(a, b, p)) {
return false;
}
// P is on the line adjoinging AB. If P is on the line segment AB, then
// PA.dot(PB) <= 0.
ccd_vec3_t PA, PB;
ccdVec3Sub2(&PA, &p, &a);
ccdVec3Sub2(&PB, &p, &b);
return ccdVec3Dot(&PA, &PB) <= 0;
}

/**
* Computes the normal vector of a triangular face on a polytope, and the normal
* vector points outward from the polytope. Notice we assume that the origin
Expand Down Expand Up @@ -1691,6 +1669,8 @@ static int __ccdGJK(const void *obj1, const void *obj2,
*/
static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
assert(polytope->nearest_type == CCD_PT_EDGE);

constexpr ccd_real_t kEps = constants<ccd_real_t>::eps();
// Only verify the feature if the nearest feature is an edge.

const ccd_pt_edge_t* const nearest_edge =
Expand All @@ -1709,27 +1689,29 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
// n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0 (since n̂ ⋅ o = 0).
origin_to_face_distance[i] =
-ccdVec3Dot(&face_normals[i], &nearest_edge->vertex[0]->v.v);
if (origin_to_face_distance[i] > 0) {
// If the origin lies *on* the edge, then it also lies on the two adjacent
// faces. Rather than failing on strictly *positive* signed distance, we
// introduce an epsilon threshold. This usage of epsilon is to account for a
// discrepancy in the signed distance computation. How GJK (and partially
// EPA) compute the signed distance from origin to face may *not* be exactly
// the same as done in this test (i.e. for a given set of vertices, the
// plane equation can be defined various ways. Those ways are
// *mathematically* equivalent but numerically will differ due to rounding).
// We account for those differences by allowing a very small positive signed
// distance to be considered zero. We assume that the GJK/EPA code
// ultimately clasifies inside/outside around *zero* and *not* epsilon.
if (origin_to_face_distance[i] > kEps) {
FCL_THROW_FAILED_AT_THIS_CONFIGURATION(
"The origin is outside of the polytope. This should already have "
"been identified as separating.");
}
}
// We compute the projection of the origin onto the plane as
// -face_normals[i] * origin_to_face_distance[i]
// If the projection to both faces are on the edge, the the edge is the
// closest feature.
bool is_edge_closest_feature = true;
for (int i = 0; i < 2; ++i) {
ccd_vec3_t origin_projection_to_plane = face_normals[i];
ccdVec3Scale(&(origin_projection_to_plane), -origin_to_face_distance[i]);
if (!is_point_on_line_segment(origin_projection_to_plane,
nearest_edge->vertex[0]->v.v,
nearest_edge->vertex[1]->v.v)) {
is_edge_closest_feature = false;
break;
}
}

// We know the reported squared distance to the edge. If that distance is
// functionally zero, then the edge must *truly* be the nearest feature.
// If it isn't, then it must be one of the adjacent faces.
const bool is_edge_closest_feature = nearest_edge->dist < kEps * kEps;

if (!is_edge_closest_feature) {
// We assume that libccd is not crazily wrong. Although the closest
// feature is not the edge, it is near that edge. Hence we select the
Expand Down Expand Up @@ -1779,7 +1761,7 @@ static int __ccdEPA(const void *obj1, const void *obj2,
return -2;
}

while (1){
while (1) {
// get triangle nearest to origin
*nearest = ccdPtNearest(polytope);
if (polytope->nearest_type == CCD_PT_EDGE) {
Expand All @@ -1791,14 +1773,13 @@ static int __ccdEPA(const void *obj1, const void *obj2,
*nearest = ccdPtNearest(polytope);
}

// get next support point
if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
break;
}
// get next support point
if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) {
break;
}

// expand nearest triangle using new point - supp
if (expandPolytope(polytope, *nearest, &supp) != 0)
return -2;
// expand nearest triangle using new point - supp
if (expandPolytope(polytope, *nearest, &supp) != 0) return -2;
}

return 0;
Expand Down
56 changes: 56 additions & 0 deletions test/test_fcl_signed_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -460,6 +460,60 @@ void test_distance_box_box_regression6() {
test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
}

// This is a *specific* case that has cropped up in the wild. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/493.
// This includes the *first* scenario.
template <typename S>
void test_distance_box_box_regression7a() {
const Vector3<S> box_size(0.5, 0.5, 0.5);
// Both transforms have the same orientation.
Matrix3<S> R_WB;
// clang-format off
R_WB <<
0.96193976625564348026, 0.15401279915737359216, 0.22572537250328919556,
-0.19134171618254486313, 0.96936494951283913579, 0.15401279915737359216,
-0.19509032201612822033, -0.19134171618254486313, 0.96193976625564348026;
// clang-format on
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.linear() = R_WB;
// clang-format off
X_WB1.translation() <<
2.0770063995786869349, 0.48468247475641951239, 1.1543291419087275962;
// clang-format on
Transform3<S> X_WB2 = Transform3<S>::Identity();
X_WB2.linear() = R_WB;
X_WB2.translation() << 2, 0, 1.25;
const double expected_distance{0};
test_distance_box_box_helper(box_size, X_WB1, box_size, X_WB2, &expected_distance);
}

// This is a *specific* case that has cropped up in the wild. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/493.
// This includes the *second* scenario.
template <typename S>
void test_distance_box_box_regression7b() {
const Vector3<S> box_size(0.25, 0.25, 0.25);
// Both transforms have the same orientation.
Matrix3<S> R_WB;
// clang-format off
R_WB <<
0.95233240645725869555, 0.27969320707964251405, 0.12179777307008109177,
-0.28888687719060901493, 0.95512127579560024415, 0.065480689593520297054,
-0.098017140329560589751, -0.097545161008064124042, 0.99039264020161521529;
// clang-format on
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.linear() = R_WB;
// clang-format off
X_WB1.translation() <<
0.069923301769910642389, 0.23878031894890006104, 1.2256137097479840037;
// clang-format on
Transform3<S> X_WB2 = Transform3<S>::Identity();
X_WB2.linear() = R_WB;
X_WB2.translation() << 0, 0, 1.25;
const double expected_distance{0};
test_distance_box_box_helper(box_size, X_WB1, box_size, X_WB2, &expected_distance);
}

// This is a *specific* case that has cropped up in the wild that reaches the
// unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was
// reported in https://github.com/flexible-collision-library/fcl/issues/408
Expand Down Expand Up @@ -533,6 +587,8 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) {
test_distance_box_box_regression4<double>();
test_distance_box_box_regression5<double>();
test_distance_box_box_regression6<double>();
test_distance_box_box_regression7a<double>();
test_distance_box_box_regression7b<double>();
test_distance_sphere_box_regression1<double>();
}

Expand Down

0 comments on commit e48faa8

Please sign in to comment.