Skip to content

Commit 2f001b8

Browse files
committed
EPA: fix normal in certain edge cases
Added a comment in EPA that explains it all. In a future PR, I will remove the `NonConvex` status of EPA, as it useless. Even when EPA is called "when it should not be" (i.e called on non-colliding shapes), it will work just fine and return the correct result. This is a very very cool property of EPA.
1 parent 901b4fa commit 2f001b8

File tree

2 files changed

+35
-8
lines changed

2 files changed

+35
-8
lines changed

include/hpp/fcl/narrowphase/gjk.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -565,7 +565,8 @@ struct HPP_FCL_DLLAPI EPA {
565565
/// @brief Add a new face to the polytope.
566566
/// This function sets the `ignore` flag to `true` if the origin does not
567567
/// project inside the face.
568-
SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex);
568+
SimplexFace* newFace(size_t id_a, size_t id_b, size_t id_vertex,
569+
bool force = false);
569570

570571
/// @brief Find the best polytope face to split
571572
SimplexFace* findClosestFace();

src/narrowphase/gjk.cpp

+33-7
Original file line numberDiff line numberDiff line change
@@ -1683,7 +1683,8 @@ bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a,
16831683
return false;
16841684
}
16851685

1686-
EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) {
1686+
EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c,
1687+
bool force) {
16871688
if (stock.root != nullptr) {
16881689
SimplexFace* face = stock.root;
16891690
stock.remove(face);
@@ -1718,7 +1719,26 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) {
17181719
face->ignore = true;
17191720
}
17201721

1721-
if (face->d >= -tolerance)
1722+
// For the initialization of EPA, we need to force the addition of the
1723+
// face. This is because the origin can lie outside the initial
1724+
// tetrahedron. This is expected. GJK can converge in two different ways:
1725+
// either by enclosing the origin with a simplex, or by converging
1726+
// sufficiently close to the origin.
1727+
// The thing is, "sufficiently close to the origin" depends on the
1728+
// tolerance of GJK. So in this case, GJK **cannot** guarantee that the
1729+
// shapes are indeed in collision. If it turns out they are not in
1730+
// collision, the origin will lie outside of the Minkowski difference and
1731+
// thus outside the initial tetrahedron. But EPA is ultimately a
1732+
// projection algorithm, so it will work fine!
1733+
//
1734+
// Actually, the `NonConvex` status is badly named. There should not be
1735+
// such a status! Again, if the origin lies outside the Minkowski
1736+
// difference, EPA will work fine, and will find the right (signed)
1737+
// distance between the shapes as well as the right normal. This is a
1738+
// very nice mathematical property, making GJK + EPA a **very** robust set
1739+
// of algorithms. :)
1740+
// TODO(louis): remove the `NonConvex` status.
1741+
if (face->d >= -tolerance || force)
17221742
return face;
17231743
else
17241744
status = NonConvex;
@@ -1786,10 +1806,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
17861806
sv_store[num_vertices++] = *simplex.vertex[i];
17871807
}
17881808

1789-
SimplexFace* tetrahedron[] = {newFace(0, 1, 2), //
1790-
newFace(1, 0, 3), //
1791-
newFace(2, 1, 3), //
1792-
newFace(0, 2, 3)};
1809+
SimplexFace* tetrahedron[] = {newFace(0, 1, 2, true), //
1810+
newFace(1, 0, 3, true), //
1811+
newFace(2, 1, 3, true), //
1812+
newFace(0, 2, 3, true)};
17931813

17941814
if (hull.count == 4) {
17951815
// set the face connectivity
@@ -2050,7 +2070,13 @@ void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
20502070
Vec3f& w1, Vec3f& normal) const {
20512071
details::getClosestPoints(result, w0, w1);
20522072
if ((w0 - w1).norm() > Eigen::NumTraits<FCL_REAL>::dummy_precision()) {
2053-
normal = (w0 - w1).normalized();
2073+
if (this->depth >= 0) {
2074+
// The shapes are in collision.
2075+
normal = (w0 - w1).normalized();
2076+
} else {
2077+
// The shapes are not in collision.
2078+
normal = (w1 - w0).normalized();
2079+
}
20542080
} else {
20552081
normal = this->normal;
20562082
}

0 commit comments

Comments
 (0)