Commit 8ba35428 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Clean EPA code.

parent aa160842
......@@ -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
{
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment