// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
static inline struct ClosestPoints
ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
{
// Find the closest p(t) on the minkowski difference to (0, 0)
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect p = LerpT(v0.ab, v1.ab, t);
// Interpolate the original support points using the same 't' value as above.
// This gives you the closest surface points in absolute coordinates. NEAT!
cpVect pa = LerpT(v0.a, v1.a, t);
cpVect pb = LerpT(v0.b, v1.b, t);
cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF);
// First try calculating the MSA from the minkowski difference edge.
// This gives us a nice, accurate MSA when the surfaces are close together.
cpVect delta = cpvsub(v1.ab, v0.ab);
cpVect n = cpvnormalize(cpvrperp(delta));
cpFloat d = cpvdot(n, p);
if(d <= 0.0f || (-1.0f < t && t < 1.0f)) {
// If the shapes are overlapping, or we have a regular vertex/edge collision, we are done.
struct ClosestPoints points = {pa, pb, n, d, id};
return points;
} else {
// Vertex/vertex collisions need special treatment since the MSA won't be shared with an axis of the minkowski difference.
cpFloat d2 = cpvlength(p);
cpVect n2 = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));
struct ClosestPoints points = {pa, pb, n2, d2, id};
return points;
}
}
static inline struct ClosestPoints
ClosestPointsNew(const struct MinkowskiPoint v0, const struct MinkowskiPoint v1)
{
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect p = LerpT(v0.ab, v1.ab, t);
cpVect pa = LerpT(v0.a, v1.a, t);
cpVect pb = LerpT(v0.b, v1.b, t);
cpCollisionID id = (v0.id & 0xFFFF)<<16 | (v1.id & 0xFFFF);
cpVect delta = cpvsub(v1.ab, v0.ab);
cpVect n = cpvnormalize(cpvperp(delta));
cpFloat d = -cpvdot(n, p);
if(d <= 0.0f || (0.0f < t && t < 1.0f)){
struct ClosestPoints points = {pa, pb, cpvneg(n), d, id};
return points;
} else {
cpFloat d2 = cpvlength(p);
cpVect n = cpvmult(p, 1.0f/(d2 + CPFLOAT_MIN));
struct ClosestPoints points = {pa, pb, n, d2, id};
return points;
}
}
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
static inline cpFloat
ClosestDist(const cpVect v0,const cpVect v1)
{
return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1)));
}
