From e48faa88fd26befb6d1fc9282c5a0b42475552d1 Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Thu, 10 Sep 2020 09:37:55 -0700 Subject: [PATCH] Address EPA failure from issue 493 - 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. --- .../gjk_libccd-inl.h | 73 +++++++------------ test/test_fcl_signed_distance.cpp | 56 ++++++++++++++ 2 files changed, 83 insertions(+), 46 deletions(-) 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 106e132ad..1adc91738 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 @@ -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 @@ -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::eps(); // Only verify the feature if the nearest feature is an edge. const ccd_pt_edge_t* const nearest_edge = @@ -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 @@ -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) { @@ -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; diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index b2850a1e4..4ba0a198a 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -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 +void test_distance_box_box_regression7a() { + const Vector3 box_size(0.5, 0.5, 0.5); + // Both transforms have the same orientation. + Matrix3 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 X_WB1 = Transform3::Identity(); + X_WB1.linear() = R_WB; + // clang-format off + X_WB1.translation() << + 2.0770063995786869349, 0.48468247475641951239, 1.1543291419087275962; + // clang-format on + Transform3 X_WB2 = Transform3::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 +void test_distance_box_box_regression7b() { + const Vector3 box_size(0.25, 0.25, 0.25); + // Both transforms have the same orientation. + Matrix3 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 X_WB1 = Transform3::Identity(); + X_WB1.linear() = R_WB; + // clang-format off + X_WB1.translation() << + 0.069923301769910642389, 0.23878031894890006104, 1.2256137097479840037; + // clang-format on + Transform3 X_WB2 = Transform3::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 @@ -533,6 +587,8 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) { test_distance_box_box_regression4(); test_distance_box_box_regression5(); test_distance_box_box_regression6(); + test_distance_box_box_regression7a(); + test_distance_box_box_regression7b(); test_distance_sphere_box_regression1(); }