Skip to content

Commit 81b3d02

Browse files
committed
GJK/EPA: fix epsilon in asserts
1 parent 0d162a5 commit 81b3d02

File tree

2 files changed

+12
-13
lines changed

2 files changed

+12
-13
lines changed

include/hpp/fcl/narrowphase/narrowphase.h

+6-6
Original file line numberDiff line numberDiff line change
@@ -199,8 +199,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
199199
}
200200
HPP_FCL_COMPILER_DIAGNOSTIC_POP
201201

202-
static constexpr FCL_REAL dummy_precision =
203-
std::numeric_limits<FCL_REAL>::epsilon() * 100;
202+
const FCL_REAL dummy_precision =
203+
3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
204204
HPP_FCL_UNUSED_VARIABLE(dummy_precision);
205205
switch (gjk.status) {
206206
case details::GJK::DidNotRun:
@@ -410,8 +410,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
410410
// In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus
411411
// it can safely be normalized.
412412
distance = gjk.distance;
413-
static constexpr FCL_REAL dummy_precision =
414-
std::numeric_limits<FCL_REAL>::epsilon() * 100;
413+
const FCL_REAL dummy_precision =
414+
3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
415415
HPP_FCL_UNUSED_VARIABLE(dummy_precision);
416416
HPP_FCL_ASSERT(
417417
gjk.ray.norm() > gjk.getTolerance() + dummy_precision,
@@ -441,8 +441,8 @@ struct HPP_FCL_DLLAPI GJKSolver {
441441
FCL_REAL& distance, Vec3f& p1,
442442
Vec3f& p2,
443443
Vec3f& normal) const {
444-
static constexpr FCL_REAL dummy_precision =
445-
std::numeric_limits<FCL_REAL>::epsilon() * 100;
444+
const FCL_REAL dummy_precision =
445+
3 * std::sqrt(std::numeric_limits<FCL_REAL>::epsilon());
446446
HPP_FCL_UNUSED_VARIABLE(dummy_precision);
447447
HPP_FCL_ASSERT(gjk.distance <= gjk.getTolerance() + dummy_precision,
448448
"The distance should be lower than GJK's tolerance.",

src/narrowphase/gjk.cpp

+6-7
Original file line numberDiff line numberDiff line change
@@ -1120,10 +1120,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) {
11201120
const Vec3f a_cross_b = A.cross(B);
11211121
const Vec3f a_cross_c = A.cross(C);
11221122

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()));
11271125
HPP_FCL_UNUSED_VARIABLE(dummy_precision);
11281126

11291127
#define REGION_INSIDE() \
@@ -1747,7 +1745,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
17471745
// the support we just added is in the direction of the normal of
17481746
// the closest_face. Therefore, the support point will **always**
17491747
// 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()) &&
17511750
"The support is not in the right direction.");
17521751
//
17531752
// 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,
19211920
// recursive nature of `expand`, it is safer to go through the first case.
19221921
// This is because `expand` can potentially loop indefinitly if the
19231922
// 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()));
19261925
const SimplexVertex& vf = sv_store[f->vertex_id[e]];
19271926
if (f->n.dot(w.w - vf.w) < dummy_precision) {
19281927
// case 1: the support point is "below" `f`.

0 commit comments

Comments
 (0)