@@ -1120,10 +1120,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
1120
1120
const Vec3f a_cross_b = A.cross (B);
1121
1121
const Vec3f a_cross_c = A.cross (C);
1122
1122
1123
- // dummy_precision is 1e-14 if FCL_REAL is double
1124
- // 1e-5 if FCL_REAL is float
1125
- const FCL_REAL dummy_precision (100 *
1126
- std::numeric_limits<FCL_REAL>::epsilon ());
1123
+ const FCL_REAL dummy_precision (
1124
+ 3 * std::sqrt (std::numeric_limits<FCL_REAL>::epsilon ()));
1127
1125
HPP_FCL_UNUSED_VARIABLE (dummy_precision);
1128
1126
1129
1127
#define REGION_INSIDE () \
@@ -1747,7 +1745,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
1747
1745
// the support we just added is in the direction of the normal of
1748
1746
// the closest_face. Therefore, the support point will **always**
1749
1747
// lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0.
1750
- assert (closest_face->n .dot (w.w ) > 0 &&
1748
+ assert (closest_face->n .dot (w.w ) >
1749
+ -3 * std::sqrt (std::numeric_limits<FCL_REAL>::epsilon ()) &&
1751
1750
" The support is not in the right direction." );
1752
1751
//
1753
1752
// 1) First check: `fdist` (see below) is an upper bound of how much
@@ -1921,8 +1920,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e,
1921
1920
// recursive nature of `expand`, it is safer to go through the first case.
1922
1921
// This is because `expand` can potentially loop indefinitly if the
1923
1922
// Minkowski difference is very flat (hence the check above).
1924
- const FCL_REAL dummy_precision (100 *
1925
- std::numeric_limits<FCL_REAL>::epsilon ());
1923
+ const FCL_REAL dummy_precision (
1924
+ 3 * std::sqrt (std:: numeric_limits<FCL_REAL>::epsilon () ));
1926
1925
const SimplexVertex& vf = sv_store[f->vertex_id [e]];
1927
1926
if (f->n .dot (w.w - vf.w ) < dummy_precision) {
1928
1927
// case 1: the support point is "below" `f`.
0 commit comments