Skip to content

Commit

Permalink
Add a guard to faceNormalPointingOutward to detect degeneracy
Browse files Browse the repository at this point in the history 
This adds the `triangle_area_is_zero` test to `faceNormalPointingOutward`.
This is a prerequisite to the `isOutsidePolytopeFace` test. The test is
rendered meaningless if the reported normal is defined by numerical noise.
If a scenario is failing due to the `isOutsidePolytopFace` assertion
failing, this will detect if its due to face degeneracies.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI committed Aug 7, 2018
1 parent a278363 commit 2881e55
Show file tree
Hide file tree
Showing 2 changed files with 207 additions and 59 deletions.
128 changes: 69 additions & 59 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 @@ -765,6 +765,64 @@ static int simplexToPolytope4(const void *obj1, const void *obj2,
return 0;
}

/** Reports true if p and q are coincident. */
static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
// This uses a scale-dependent basis for determining coincidence. It examines
// each axis independently, and only, if all three axes are sufficiently
// close (relative to its own scale), are the two points considered
// coincident.
//
// For dimension i, two values are considered the same if:
// |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |qᵢ|)
// And the points are coincident if the previous condition holds for all
// `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions).
using std::abs;
using std::max;

const ccd_real_t eps = constants<ccd_real_t>::eps();
// NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd
// is actually float based.
for (int i = 0; i < 3; ++i) {
const ccd_real_t scale =
max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
const ccd_real_t delta = abs(p.v[i] - q.v[i]);
if (delta > scale) return false;
}
return true;
}

/** Determines if the the triangle defined by the three vertices has zero area.
Area can be zero for one of two reasons:
- the triangle is so small that the vertices are functionally coincident, or
- the vertices are co-linear.
Both conditions are computed with respect to machine precision.
@returns true if the area is zero. */
static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
const ccd_vec3_t& c) {
// First coincidence condition. This doesn't *explicitly* test for b and c
// being coincident. That will be captured in the subsequent co-linearity
// test. If b and c *were* coincident, it would be cheaper to perform the
// coincidence test than the co-linearity test.
// However, the expectation is that typically the triangle will not have zero
// area. In that case, we want to minimize the number of tests performed on
// the average, so we prefer to eliminate one coincidence test.
if (are_coincident(a, b) || are_coincident(a, c)) return true;

// We're going to compute the *sine* of the angle θ between edges (given that
// the vertices are *not* coincident). If the sin(θ) < ε, the edges are
// co-linear.
ccd_vec3_t AB, AC, n;
ccdVec3Sub2(&AB, &b, &a);
ccdVec3Sub2(&AC, &c, &a);
ccdVec3Normalize(&AB);
ccdVec3Normalize(&AC);
ccdVec3Cross(&n, &AB, &AC);
const ccd_real_t eps = constants<ccd_real_t>::eps();
// Second co-linearity condition.
if (ccdVec3Len2(&n) < eps * eps) return true;
return false;
}

/**
* 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 All @@ -778,6 +836,17 @@ static int simplexToPolytope4(const void *obj1, const void *obj2,
*/
static ccd_vec3_t faceNormalPointingOutward(const ccd_pt_t* polytope,
const ccd_pt_face_t* face) {
// This doesn't necessarily define a triangle; I don't know that the third
// vertex added here is unique from the other two.
#ifndef NDEBUG
// quick test for degeneracy
const ccd_vec3_t& a = face->edge[0]->vertex[1]->v.v;
const ccd_vec3_t& b = face->edge[0]->vertex[0]->v.v;
const ccd_vec3_t& test_v = face->edge[1]->vertex[0]->v.v;
const ccd_vec3_t& c = are_coincident(test_v, a) || are_coincident(test_v, b) ?
face->edge[1]->vertex[1]->v.v : test_v;
assert(!triangle_area_is_zero(a, b, c));
#endif
// We find two edges of the triangle as e1 and e2, and the normal vector
// of the face is e1.cross(e2).
ccd_vec3_t e1, e2;
Expand Down Expand Up @@ -1363,65 +1432,6 @@ static int __ccdEPA(const void *obj1, const void *obj2,
return 0;
}


/** Reports true if p and q are coincident. */
static bool are_coincident(const ccd_vec3_t& p, const ccd_vec3_t& q) {
// This uses a scale-dependent basis for determining coincidence. It examines
// each axis independently, and only, if all three axes are sufficiently
// close (relative to its own scale), are the two points considered
// coincident.
//
// For dimension i, two values are considered the same if:
// |pᵢ - qᵢ| <= ε·max(1, |pᵢ|, |pᵢ|)
// And the points are coincident if the previous condition for all
// `i ∈ {0, 1, 2}` (i.e. the x-, y-, *and* z-dimensions).
using std::abs;
using std::max;

const ccd_real_t eps = constants<ccd_real_t>::eps();
// NOTE: Wrapping "1.0" with ccd_real_t accounts for mac problems where ccd
// is actually float based.
for (int i = 0; i < 3; ++i) {
const ccd_real_t scale =
max({ccd_real_t{1}, abs(p.v[i]), abs(q.v[i])}) * eps;
const ccd_real_t delta = abs(p.v[i] - q.v[i]);
if (delta > scale) return false;
}
return true;
}

/** Determines if the the triangle defined by the three vertices has zero area.
Area can be zero for one of two reasons:
- the triangle is so small that the vertices are functionally coincident, or
- the vertices are co-linear.
Both conditions are computed with respect to machine precision.
@returns true if the area is zero. */
static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b,
const ccd_vec3_t& c) {
// First coincidence condition. This doesn't *explicitly* test for b and c
// being coincident. That will be captured in the subsequent co-linearity
// test. If b and c *were* coincident, it would be cheaper to perform the
// coincidence test than the co-linearity test.
// However, the expectation is that typically the triangle will not have zero
// area. In that case, we want to minimize the number of tests performed on
// the average, so we prefer to eliminate one coincidence test.
if (are_coincident(a, b) || are_coincident(a, c)) return true;

// We're going to compute the *sine* of the angle θ between edges (given that
// the vertices are *not* coincident). If the sin(θ) < ε, the edges are
// co-linear.
ccd_vec3_t AB, AC, n;
ccdVec3Sub2(&AB, &b, &a);
ccdVec3Sub2(&AC, &c, &a);
ccdVec3Normalize(&AB);
ccdVec3Normalize(&AC);
ccdVec3Cross(&n, &AB, &AC);
const ccd_real_t eps = constants<ccd_real_t>::eps();
// Second co-linearity condition.
if (ccdVec3Len2(&n) < eps * eps) return true;
return false;
}

/** Given a single support point, `q`, extract the point `p1` and `p2`, the
points on object 1 and 2, respectively, in the support data of `q`. */
static void extractObjectPointsFromPoint(ccd_support_t *q, ccd_vec3_t *p1,
Expand Down
138 changes: 138 additions & 0 deletions test/narrowphase/detail/convexity_based_algorithm/test_gjk_libccd-inl_epa.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -293,6 +293,144 @@ GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace) {
CheckPointOutsidePolytopeFace(0, 0, 0, 3, expect_inside);
}

#ifndef NDEBUG

/** A degenerate tetrahedron due to vertices considered to be coincident.
It is, strictly speaking a valid tetrahedron, but the points are so close that
the calculations on edge lengths cannot be trusted.
More particularly, one face is *very* small but the other three faces are quite
long with horrible aspect ratio.
Vertices v0, v1, and v2 are all close to each other, v3 is distant.
Edges e0, e1, and e2 connect vertices (v0, v1, and v2) and, as such, have very
short length. Edges e3, e4, and e5 connect to the distance vertex and have
long length.
Face 0 is the small face. Faces 1-3 all include one edge of the small face.
All faces should be considered degenerate due to coincident points. */
class CoincidentTetrahedron : public Polytope {
public:
CoincidentTetrahedron() : Polytope() {
const ccd_real_t delta = constants<ccd_real_t>::eps() / 4;
v().resize(4);
e().resize(6);
f().resize(4);
auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y,
ccd_real_t z) {
return ccdPtAddVertexCoords(&this->polytope(), x, y, z);
};
v()[0] = AddTetrahedronVertex(0.5, delta, delta);
v()[1] = AddTetrahedronVertex(0.5, -delta, delta);
v()[2] = AddTetrahedronVertex(0.5, -delta, -delta);
v()[3] = AddTetrahedronVertex(-0.5, 0, 0);
e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
}
};

// Tests against a polytope with a face where all the points are too close to
// distinguish.
GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident0) {
::testing::FLAGS_gtest_death_test_style = "threadsafe";
CoincidentTetrahedron p;

// The test point doesn't matter; it'll never get that far.
// NOTE: For platform compatibility, the assertion message is pared down to
// the simplest component: the actual function call in the assertion.
ccd_vec3_t pt{{10, 10, 10}};
ASSERT_DEATH(
libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0), &pt),
".*!triangle_area_is_zero.*");
}

// Tests against a polytope with a face where *two* points are too close to
// distinguish.
GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Coincident1) {
::testing::FLAGS_gtest_death_test_style = "threadsafe";
CoincidentTetrahedron p;

// The test point doesn't matter; it'll never get that far.
// NOTE: For platform compatibility, the assertion message is pared down to
// the simplest component: the actual function call in the assertion.
ccd_vec3_t pt{{10, 10, 10}};
ASSERT_DEATH(
libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt),
".*!triangle_area_is_zero.*");
}

/** A degenerate tetrahedron due to vertices considered to be colinear.
It is, strictly speaking a valid tetrahedron, but the vertices are so close to
being colinear, that the area can't meaningfully be computed.
More particularly, one face is *very* large but the fourth vertex lies just
slightly off that plane *over* one of the edges. The face that is incident to
that edge and vertex will have colinear edges.
Vertices v0, v1, and v2 are form the large triangle. v3 is the slightly
off-plane vertex. Edges e0, e1, and e2 connect vertices (v0, v1, and v2). v3
projects onto edge e0. Edges e3 and e4 connect v0 and v1 to v3, respectively.
Edges e3 and e4 are colinear. Edge e5 is the remaining, uninteresting edge.
Face 0 is the large triangle.
Face 1 is the bad face. Faces 2 and 3 are irrelevant. */
class ColinearTetrahedron : public Polytope {
public:
ColinearTetrahedron() : Polytope() {
const ccd_real_t delta = constants<ccd_real_t>::eps() / 100;
v().resize(4);
e().resize(6);
f().resize(4);
auto AddTetrahedronVertex = [this](ccd_real_t x, ccd_real_t y,
ccd_real_t z) {
return ccdPtAddVertexCoords(&this->polytope(), x, y, z);
};
v()[0] = AddTetrahedronVertex(0.5, -0.5 / std::sqrt(3), -1);
v()[1] = AddTetrahedronVertex(-0.5, -0.5 / std::sqrt(3), -1);
v()[2] = AddTetrahedronVertex(0, 1 / std::sqrt(3), -1);
// This point should lie *slightly* above the edge connecting v0 and v1.
v()[3] = AddTetrahedronVertex(0, -0.5 / std::sqrt(3), -1 + delta);

e()[0] = ccdPtAddEdge(&polytope(), &v(0), &v(1));
e()[1] = ccdPtAddEdge(&polytope(), &v(1), &v(2));
e()[2] = ccdPtAddEdge(&polytope(), &v(2), &v(0));
e()[3] = ccdPtAddEdge(&polytope(), &v(0), &v(3));
e()[4] = ccdPtAddEdge(&polytope(), &v(1), &v(3));
e()[5] = ccdPtAddEdge(&polytope(), &v(2), &v(3));
f()[0] = ccdPtAddFace(&polytope(), &e(0), &e(1), &e(2));
f()[1] = ccdPtAddFace(&polytope(), &e(0), &e(3), &e(4));
f()[2] = ccdPtAddFace(&polytope(), &e(1), &e(4), &e(5));
f()[3] = ccdPtAddFace(&polytope(), &e(3), &e(5), &e(2));
}
};

// Tests against a polytope with a face where two edges are colinear.
GTEST_TEST(FCL_GJK_EPA, isOutsidePolytopeFace_DegenerateFace_Colinear) {
::testing::FLAGS_gtest_death_test_style = "threadsafe";
ColinearTetrahedron p;

// This test point should pass w.r.t. the big face.
ccd_vec3_t pt{{0, 0, -10}};
EXPECT_TRUE(libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(0),
&pt));
// Face 1, however, is definitely colinear.
// NOTE: For platform compatibility, the assertion message is pared down to
// the simplest component: the actual function call in the assertion.
ASSERT_DEATH(
libccd_extension::isOutsidePolytopeFace(&p.polytope(), &p.f(1), &pt),
".*!triangle_area_is_zero.*");
}
#endif


// Construct a polytope with the following shape, namely an equilateral triangle
// on the top, and an equilateral triangle of the same size, but rotate by 60
// degrees on the bottom. We will then connect the vertices of the equilateral
Expand Down

0 comments on commit 2881e55

Please sign in to comment.