Skip to content

Commit 9ae51ed

Browse files
committed
core: add assert to epa fallback to check that GJK has found the origin
1 parent 2fffb74 commit 9ae51ed

File tree

2 files changed

+4
-0
lines changed

2 files changed

+4
-0
lines changed

include/hpp/fcl/narrowphase/gjk.h

+3
Original file line numberDiff line numberDiff line change
@@ -254,6 +254,9 @@ struct HPP_FCL_DLLAPI GJK {
254254
/// @brief Get GJK number of iterations.
255255
inline size_t getIterations() { return iterations; }
256256

257+
/// @brief Get GJK tolerance.
258+
inline FCL_REAL getTolerance() { return tolerance; }
259+
257260
private:
258261
SimplexV store_v[4];
259262
SimplexV* free_v[4];

src/narrowphase/gjk.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -1621,6 +1621,7 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) {
16211621
// the origin.
16221622
status = FallBack;
16231623
// TODO: define a better normal
1624+
assert(simplex.rank == 1 && simplex.vertex[0]->w.isZero(gjk.getTolerance()));
16241625
normal = -guess;
16251626
FCL_REAL nl = normal.norm();
16261627
if (nl > 0)

0 commit comments

Comments
 (0)