From 8ba354281a3ecfba63e3e5c4b6376ed7a12d319b Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 19 Feb 2020 17:21:26 +0100
Subject: [PATCH] Clean EPA code.
---
include/hpp/fcl/narrowphase/gjk.h | 1 -
src/narrowphase/gjk.cpp | 65 +++++++++++++------------------
2 files changed, 27 insertions(+), 39 deletions(-)
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 6e7d25ff..50848009 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -218,7 +218,6 @@ static const size_t EPA_MAX_ITERATIONS = 255;
/// @brief class for EPA algorithm
struct EPA
{
-private:
typedef GJK::SimplexV SimplexV;
struct SimplexF
{
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 9f0b94db..7880f193 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -1194,46 +1194,35 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = Valid;
for(; iterations < max_iterations; ++iterations)
{
- if(nextsv < max_vertex_num)
- {
- SimplexHorizon horizon;
- SimplexV* w = &sv_store[nextsv++];
- bool valid = true;
- best->pass = ++pass;
- // At the moment, SimplexF.n is always normalized. This could be revised in the future...
- gjk.getSupport(best->n, true, *w);
- FCL_REAL wdist = best->n.dot(w->w) - best->d;
- if(wdist > tolerance)
- {
- for(size_t j = 0; (j < 3) && valid; ++j)
- {
- valid &= expand(pass, w, best->f[j], best->e[j], horizon);
- }
-
-
- if(valid && horizon.nf >= 3)
- {
- // need to add the edge connectivity between first and last faces
- bind(horizon.ff, 2, horizon.cf, 1);
- hull.remove(best);
- stock.append(best);
- best = findBest();
- outer = *best;
- }
- else
- {
- status = InvalidHull; break;
- }
- }
- else
- {
- status = AccuracyReached; break;
- }
+ if (nextsv >= max_vertex_num) {
+ status = OutOfVertices;
+ break;
}
- else
- {
- status = OutOfVertices; break;
+
+ SimplexHorizon horizon;
+ SimplexV* w = &sv_store[nextsv++];
+ bool valid = true;
+ best->pass = ++pass;
+ // At the moment, SimplexF.n is always normalized. This could be revised in the future...
+ gjk.getSupport(best->n, true, *w);
+ FCL_REAL wdist = best->n.dot(w->w) - best->d;
+ if(wdist <= tolerance) {
+ status = AccuracyReached;
+ break;
+ }
+ for(size_t j = 0; (j < 3) && valid; ++j)
+ valid &= expand(pass, w, best->f[j], best->e[j], horizon);
+
+ if(!valid || horizon.nf < 3) {
+ status = InvalidHull;
+ break;
}
+ // need to add the edge connectivity between first and last faces
+ bind(horizon.ff, 2, horizon.cf, 1);
+ hull.remove(best);
+ stock.append(best);
+ best = findBest();
+ outer = *best;
}
normal = outer.n;
--
GitLab