// Find the closest points on the surface of two overlapping shapes using the EPA algorithm.
// EPA is called from GJK when two shapes overlap.
// This is moderately expensive step! Avoid it by adding radii to your shapes so their inner polygons won't overlap.
static struct ClosestPoints
EPA(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const struct MinkowskiPoint v2)
{
// TODO: allocate a NxM array here and do an in place convex hull reduction in EPARecurse
struct MinkowskiPoint hull[3] = {v0, v1, v2};
return EPARecurse(ctx, 3, hull, 1);
}
// Recursive implementation of the EPA loop.
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
// Find the closest segment hull[i] and hull[i + 1] to (0, 0)
for(int j=0, i=count-1; j<count; i=j, j++) {
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist) {
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
// Check if there is a point on the minkowski difference beyond this edge.
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
if(CheckArea(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab))) && iteration < MAX_EPA_ITERATIONS) {
// Rebuild the convex hull by inserting p.
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
int count2 = 1;
hull2[0] = p;
for(int i=0; i<count; i++) {
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
if(CheckArea(cpvsub(h2, h0), cpvadd(cpvsub(h1, h0), cpvsub(h1, h2)))) {
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
// Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
for(int j=0, i=count-1; j<count; i=j, j++){
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist){
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS){
int count2 = 1;
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
hull2[0] = p;
for(int i=0; i<count; i++){
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
// TODO: Should this be changed to an area2x check?
if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f){
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}