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 15, 2020
1 parent 97cf77d commit 3d67bee
Show file tree
Hide file tree
Showing 3 changed files with 110 additions and 49 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@
[#472](https://github.com/flexible-collision-library/fcl/pull/472)
* Documentation for OcTree no longer mistakenly excluded from doxygen:
[#472](https://github.com/flexible-collision-library/fcl/pull/472)
* Another failure mode in the GJK/EPA signed distance query patched:
[#494](https://github.com/flexible-collision-library/fcl/pull/494)

* Build/Test/Misc

Expand Down
81 changes: 35 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 @@ -1701,6 +1681,14 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) {
// normalized.
std::array<ccd_vec3_t, 2> face_normals;
std::array<double, 2> origin_to_face_distance;

// We define the plane equation using vertex[0]. If vertex[0] is far away
// from the origin, it can magnify rounding error. We scale epsilon to account
// for this possibility.
const ccd_real_t v0_dist =
std::sqrt(ccdVec3Len2(&nearest_edge->vertex[0]->v.v));
const ccd_real_t plane_threshold = kEps * std::max(1.0, v0_dist);

for (int i = 0; i < 2; ++i) {
face_normals[i] =
faceNormalPointingOutward(polytope, nearest_edge->faces[i]);
Expand All @@ -1709,27 +1697,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 classifies inside/outside around *zero* and *not* epsilon.
if (origin_to_face_distance[i] > plane_threshold) {
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 +1769,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 +1781,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
76 changes: 73 additions & 3 deletions test/test_fcl_signed_distance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -325,12 +325,22 @@ void test_distance_box_box_helper(const Vector3<S>& box1_size,
// p_B1P1 is the position of the witness point P1 on box 1, measured
// and expressed in the box 1 frame B1.
const Vector3<S> p_B1P1 = X_WB1.inverse() * result.nearest_points[0];
const double tol = 10 * std::numeric_limits<S>::epsilon();
EXPECT_TRUE((p_B1P1.array().abs() <= (box1_size / 2).array() + tol).all());
constexpr double tol = 10 * constants<S>::eps();
const double tol_1 = tol * std::max(S(1), (box1_size / 2).maxCoeff());
EXPECT_TRUE(
(p_B1P1.array().abs() <= (box1_size / 2).array() + tol_1).all())
<< "\n p_B1P1: " << p_B1P1.transpose()
<< "\n box1_size / 2: " << (box1_size / 2).transpose()
<< "\n tol: " << tol_1;
// p_B2P2 is the position of the witness point P2 on box 2, measured
// and expressed in the box 2 frame B2.
const double tol_2 = tol * std::max(S(1), (box2_size / 2).maxCoeff());
const Vector3<S> p_B2P2 = X_WB2.inverse() * result.nearest_points[1];
EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all());
EXPECT_TRUE(
(p_B2P2.array().abs() <= (box2_size / 2).array() + tol_2).all())
<< "\n p_B2P2: " << p_B2P2.transpose()
<< "\n box2_size / 2: " << (box2_size / 2).transpose()
<< "\n tol: " << tol_2;

// An expected distance has been provided; let's test that the value is as
// expected.
Expand All @@ -345,6 +355,7 @@ void test_distance_box_box_helper(const Vector3<S>& box1_size,
// reported in https://github.com/flexible-collision-library/fcl/issues/388
template <typename S>
void test_distance_box_box_regression1() {
SCOPED_TRACE("test_distance_box_box_regression1");
const Vector3<S> box1_size(0.03, 0.12, 0.1);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
Expand All @@ -370,6 +381,7 @@ void test_distance_box_box_regression1() {
// reported in https://github.com/flexible-collision-library/fcl/issues/395
template <typename S>
void test_distance_box_box_regression2() {
SCOPED_TRACE("test_distance_box_box_regression2");
const Vector3<S> box1_size(0.46, 0.48, 0.01);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.matrix() << 1,0,0, -0.72099999999999997424,
Expand All @@ -393,6 +405,7 @@ void test_distance_box_box_regression2() {
// reported in https://github.com/flexible-collision-library/fcl/issues/415
template <typename S>
void test_distance_box_box_regression3() {
SCOPED_TRACE("test_distance_box_box_regression3");
const Vector3<S> box1_size(0.49, 0.05, 0.21);
Transform3<S> X_WB1 = Transform3<S>::Identity();
// clang-format off
Expand All @@ -416,6 +429,7 @@ void test_distance_box_box_regression3() {
// reported in https://github.com/flexible-collision-library/fcl/issues/398
template <typename S>
void test_distance_box_box_regression4() {
SCOPED_TRACE("test_distance_box_box_regression4");
const Vector3<S> box1_size(0.614, 3, 0.37);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.translation() << -0.675, 0, 0.9115;
Expand All @@ -429,6 +443,7 @@ void test_distance_box_box_regression4() {
// reported in https://github.com/flexible-collision-library/fcl/issues/428
template <typename S>
void test_distance_box_box_regression5() {
SCOPED_TRACE("test_distance_box_box_regression5");
const Vector3<S> box1_size(0.2, 0.33, 0.1);
Transform3<S> X_WB1 = Transform3<S>::Identity();
X_WB1.translation() << -0.071000000000000035305, -0.77200000000000001954, 0.79999999999999993339;
Expand All @@ -440,6 +455,7 @@ void test_distance_box_box_regression5() {

template <typename S>
void test_distance_box_box_regression6() {
SCOPED_TRACE("test_distance_box_box_regression6");
const Vector3<S> box1_size(0.31650000000000000355, 0.22759999999999999676, 0.1768000000000000127);
Transform3<S> X_WB1 = Transform3<S>::Identity();
// clang-format off
Expand All @@ -460,11 +476,64 @@ void test_distance_box_box_regression6() {
test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance);
}

// Issue #493 outlines a number of scenarios that caused signed distance
// failure. They consisted of two identical, stacked boxes. The boxes are
// slightly tilted. The boxes were essentially touching but were separated by
// infinitesimally small distances. The issue outlines three different examples.
// Rather than reproducing each of them verbatim, this test attempts to
// generalize those cases by testing the stacked scenario across various box
// sizes and separation amounts (ranging from slightly penetrating to slightly
// separated). These should essentially cover the variations described in the
// issue.
template <typename S>
void test_distance_box_box_regression_tilted_kissing_contact() {
SCOPED_TRACE("test_distance_box_box_regression_tilted_kissing_contact");
// The boxes are posed relative to each other in a common frame F (such that
// it is easy to reason about their separation). The stack is rotated around
// box A's origin and translated into the world frame.
Matrix3<S> R_WF;
R_WF <<
0.94096063217417758029, 0.29296840037289501035, 0.16959541586174811667,
-0.23569836841299879326, 0.92661523595848427348, -0.29296840037289506586,
-0.2429801799032638987, 0.23569836841299884878, 0.94096063217417758029;

for (const S dim : {S(0.01), S(0.25), S(0.5), S(10), S(1000)}) {
const Vector3<S> box_size(dim, dim, dim);

const Vector3<S> p_WA(0, 0, 5 * dim);
Transform3<S> X_WA;
X_WA.linear() = R_WF;
X_WA.translation() = p_WA;
Transform3<S> X_WB;
X_WB.linear() = R_WF;

// Both boxes always have the same orientation and the *stack* is always
// located at p_WA. Only the translational component of X_WB changes with
// varying separation distance.

// By design, the distances are all near epsilon. We'll scale them up for
// larger boxes to make sure the distance doesn't simply disappear in
// the rounding noise.
for (const S distance : {S(-1e-15), S(-2.5e-16), S(-1e-16), S(0), S(1e-16),
S(2.5e-16), S(1e-15)}) {
const S scaled_distance = distance * std::max(S(1), dim);
const Vector3<S> p_AB_F = Vector3<S>(0, dim + scaled_distance, 0);

X_WB.translation() = p_WA + R_WF * p_AB_F;
SCOPED_TRACE("dim: " + std::to_string(dim) +
", distance: " + std::to_string(distance));
test_distance_box_box_helper(box_size, X_WA, box_size, X_WB,
&scaled_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
template <typename S>
void test_distance_sphere_box_regression1() {
SCOPED_TRACE("test_distance_sphere_box_regression1");
using CollisionGeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
const S sphere_radius = 0.06;
CollisionGeometryPtr_t sphere_geo(new fcl::Sphere<S>(sphere_radius));
Expand Down Expand Up @@ -533,6 +602,7 @@ 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_regression_tilted_kissing_contact<double>();
test_distance_sphere_box_regression1<double>();
}

Expand Down

0 comments on commit 3d67bee

Please sign in to comment.