diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 6e7d25ffc487085b533802b52cf1fb94c67c93cb..508480099c974ae6b0b9ee3f8007f2890d260ffe 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 9f0b94db7e2a185edb8bda396a9c2bf7b60bfe5a..7880f193933f32f55b44fd7ad077eaf979d6dfce 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;