// 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))); }