@@ -1683,7 +1683,8 @@ bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a,
1683
1683
return false ;
1684
1684
}
1685
1685
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) {
1687
1688
if (stock.root != nullptr ) {
1688
1689
SimplexFace* face = stock.root ;
1689
1690
stock.remove (face);
@@ -1718,7 +1719,26 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) {
1718
1719
face->ignore = true ;
1719
1720
}
1720
1721
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)
1722
1742
return face;
1723
1743
else
1724
1744
status = NonConvex;
@@ -1786,10 +1806,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
1786
1806
sv_store[num_vertices++] = *simplex.vertex [i];
1787
1807
}
1788
1808
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 )};
1793
1813
1794
1814
if (hull.count == 4 ) {
1795
1815
// set the face connectivity
@@ -2050,7 +2070,13 @@ void EPA::getWitnessPointsAndNormal(const MinkowskiDiff& shape, Vec3f& w0,
2050
2070
Vec3f& w1, Vec3f& normal ) const {
2051
2071
details::getClosestPoints (result, w0, w1);
2052
2072
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
+ }
2054
2080
} else {
2055
2081
normal = this ->normal ;
2056
2082
}
0 commit comments