// 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 struct Notch
DeepestNotch(int count, cpVect *verts, int hullCount, cpVect *hullVerts, int first, cpFloat tol)
{
struct Notch notch = {};
int j = Next(first, count);
for(int i=0; i<hullCount; i++){
cpVect a = hullVerts[i];
cpVect b = hullVerts[Next(i, hullCount)];
// TODO use a cross check instead?
cpVect n = cpvnormalize(cpvrperp(cpvsub(a, b)));
cpFloat d = cpvdot(n, a);
cpVect v = verts[j];
while(!cpveql(v, b)){
cpFloat depth = cpvdot(n, v) - d;
if(depth > notch.d){
notch.d = depth;
notch.i = j;
notch.v = v;
notch.n = n;
}
j = Next(j, count);
v = verts[j];
}
j = Next(j, count);
}
return notch;
}
cpSegmentShape *
cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r)
{
seg->a = a;
seg->b = b;
seg->n = cpvrperp(cpvnormalize(cpvsub(b, a)));
seg->r = r;
seg->a_tangent = cpvzero;
seg->b_tangent = cpvzero;
cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body, cpSegmentShapeMassInfo(0.0f, a, b, r));
return seg;
}
static void
SetVerts(cpPolyShape *poly, int count, const cpVect *verts)
{
poly->count = count;
if(count <= CP_POLY_SHAPE_INLINE_ALLOC){
poly->planes = poly->_planes;
} else {
poly->planes = (struct cpSplittingPlane *)cpcalloc(2*count, sizeof(struct cpSplittingPlane));
}
for(int i=0; i<count; i++){
cpVect a = verts[(i - 1 + count)%count];
cpVect b = verts[i];
cpVect n = cpvnormalize(cpvrperp(cpvsub(b, a)));
poly->planes[i + count].v0 = b;
poly->planes[i + count].n = n;
}
}
