// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
public static ClosestPoints ClosestPointsNew(MinkowskiPoint v0, MinkowskiPoint v1)
{
// Find the closest p(t) on the minkowski difference to (0, 0)
float t = cpCollision.ClosestT(v0.ab, v1.ab);
cpVect p = cpCollision.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 = cpCollision.LerpT(v0.a, v1.a, t);
cpVect pb = cpCollision.LerpT(v0.b, v1.b, t);
ulong 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 = cpVect.cpvsub(v1.ab, v0.ab);
cpVect n = cpVect.cpvnormalize(cpVect.cpvrperp(delta));
float d = cpVect.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.
ClosestPoints points = new ClosestPoints(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.
float d2 = cpVect.cpvlength(p);
cpVect n2 = cpVect.cpvmult(p, 1.0f / (d2 + float.MinValue));
ClosestPoints points = new ClosestPoints(pa, pb, n2, d2, (ulong)id);
return(points);
}
}
// 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.
public static ClosestPoints EPARecurse(ref SupportContext ctx, int count, MinkowskiPoint[] hull, int iteration)
{
int mini = 0;
float minDist = cp.Infinity;
//int count = hull.Length;
// TODO: precalculate this when building the hull and save a step.
for (int j = 0, i = count - 1; j < count; i = j, j++)
{
float d = ClosestDist(hull[i].ab, hull[j].ab);
if (d < minDist)
{
minDist = d;
mini = i;
}
}
MinkowskiPoint v0 = hull[mini];
MinkowskiPoint v1 = hull[(mini + 1) % count];
cp.AssertSoft(!cpVect.cpveql(v0.ab, v1.ab), string.Format("Internal Error: EPA vertexes are the same ({0} and {1})", mini, (mini + 1) % count));
MinkowskiPoint p = MinkowskiPoint.Support(ref ctx,
cpVect.cpvperp(cpVect.cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
public static MinkowskiPoint MinkowskiPointNew(SupportPoint a, SupportPoint b)
{
var point = new MinkowskiPoint(a.p, b.p,
cpVect.cpvsub(b.p, a.p),
((a.id & 0xFF) << 8 | (b.id & 0xFF)));
return(point);
}
public static MinkowskiPoint MinkowskiPointNew(SupportPoint a, SupportPoint b)
{
var point = new MinkowskiPoint(a.p, b.p,
cpVect.cpvsub(b.p, a.p),
((a.id & 0xFF) << 8 | (b.id & 0xFF)));
return point;
}
// Calculate the closest points on two shapes given the closest edge on their minkowski difference to (0, 0)
public static ClosestPoints ClosestPointsNew(MinkowskiPoint v0, MinkowskiPoint v1)
{
// Find the closest p(t) on the minkowski difference to (0, 0)
float t = cpCollision.ClosestT(v0.ab, v1.ab);
cpVect p = cpCollision.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 = cpCollision.LerpT(v0.a, v1.a, t);
cpVect pb = cpCollision.LerpT(v0.b, v1.b, t);
ulong 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 = cpVect.cpvsub(v1.ab, v0.ab);
cpVect n = cpVect.cpvnormalize(cpVect.cpvrperp(delta));
float d = cpVect.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.
ClosestPoints points = new ClosestPoints(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.
float d2 = cpVect.cpvlength(p);
cpVect n2 = cpVect.cpvmult(p, 1.0f / (d2 + float.MinValue));
ClosestPoints points = new ClosestPoints(pa, pb, n2, d2, (ulong)id);
return points;
}
}
// 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.
public static ClosestPoints EPARecurse(ref SupportContext ctx, int count, MinkowskiPoint[] hull, int iteration)
{
int mini = 0;
float minDist = cp.Infinity;
//int count = hull.Length;
// TODO: precalculate this when building the hull and save a step.
for (int j = 0, i = count - 1; j < count; i = j, j++)
{
float d = ClosestDist(hull[i].ab, hull[j].ab);
if (d < minDist)
{
minDist = d;
mini = i;
}
}
MinkowskiPoint v0 = hull[mini];
MinkowskiPoint v1 = hull[(mini + 1) % count];
cp.AssertSoft(!cpVect.cpveql(v0.ab, v1.ab), string.Format("Internal Error: EPA vertexes are the same ({0} and {1})", mini, (mini + 1) % count));
MinkowskiPoint p = MinkowskiPoint.Support(ref ctx,
cpVect.cpvperp(cpVect.cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
// 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.
public static ClosestPoints EPA(ref SupportContext ctx, ref MinkowskiPoint v0, ref MinkowskiPoint v1, ref MinkowskiPoint v2)
{
// TODO: allocate a NxM array here and do an in place convex hull reduction in EPARecurse
MinkowskiPoint[] hull = new MinkowskiPoint[] { v0, v1, v2 };
return EPARecurse(ref ctx, 3, hull, 1);
}
