/// <summary>
/// Discrete Box vs Box test. Very fast. Generates contact info. NOTE: ensure UpdateAxes is called for each BoxShape prior.
/// </summary>
/// <param name="A">BoxShape A.</param>
/// <param name="PA">BoxShape A's position.</param>
/// <param name="OA">BoxShape A's orientation.</param>
/// <param name="B">BoxShape B.</param>
/// <param name="PB">BoxShape B's position.</param>
/// <param name="OB">BoxShape B's orientation.</param>
/// <param name="normal">Normal of collision.</param>
/// <param name="t">Amount of penetration.</param>
/// <param name="CA">Contacts found on BoxShape A.</param>
/// <param name="CB">Contacts found on BoxShape B.</param>
/// <param name="NumContacts">Number of contacts found.</param>
/// <returns>True if collision exists.</returns>
public static bool BoxBoxTestContact(ref BoxShape A, ref JVector PA, ref JMatrix OA,
ref BoxShape B, ref JVector PB, ref JMatrix OB,
out JVector normal, out float t, out JVector[] CA, out JVector[] CB, out int NumContacts)
{
float overlap = 0;
normal = A.xAxis;
t = 0.0f;
// TODO in future to save a few bytes of garbage make these
// SA1, SA2 and SB1, SB2 (no arrays)
JVector[] SA = new JVector[2], SB = new JVector[2];
int NumSA = 0, NumSB = 0;
CA = new JVector[2]; CB = new JVector[2];
NumContacts = 0;
JVector T = PB - PA;
// cache scaled local axes
JVector Ax = A.halfSize.X * A.xAxis;
JVector Ay = A.halfSize.Y * A.yAxis;
JVector Bx = B.halfSize.X * B.xAxis;
JVector By = B.halfSize.Y * B.yAxis;
// axis to test
JVector L = A.xAxis;
float TL = Math.Abs(T * L);
float a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
float b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
// just set first overlap
overlap = TL - (b + a);
// axis to test
L = A.yAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
var o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = A.yAxis;
}
// axis to test
L = B.xAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = B.xAxis;
}
// axis to test
L = B.yAxis;
TL = Math.Abs(T * L);
a = Math.Abs((Ax) * L) + Math.Abs((Ay) * L);
b = Math.Abs((Bx) * L) + Math.Abs((By) * L);
if (TL > (a + b))
{
return(false);
}
// cache overlap
o = TL - (b + a);
if (o > overlap)
{
overlap = o;
normal = B.yAxis;
}
// make sure the polygons gets pushed away from each other.
if (normal * T < 0.0f)
{
normal = -normal;
}
t = overlap;
// now to find a contact point or edge!
var mn = -normal;
NumSA = A.FindSupportPoints(ref mn, ref PA, ref OA, out SA);
NumSB = B.FindSupportPoints(ref normal, ref PB, ref OB, out SB);
// now refine contact points from support points
// edge/vertex case
if (NumSA == 2 && NumSB == 1)
{
ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out CA[NumContacts]);
CB[NumContacts] = SB[0];
NumContacts++;
return(true);
}
// vertex/edge case
if (NumSA == 1 && NumSB == 2)
{
ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out CB[NumContacts]);
CA[NumContacts] = SA[0];
NumContacts++;
return(true);
}
// edge/edge case
if (NumSA == 2 && NumSB == 2)
{
// clip contacts
JVector perp = new JVector(-normal.Y, normal.X);
// project first 2 contacts to axis perpendicular to normal
float min0 = SA[0] * perp;
float max0 = min0;
float min1 = SB[0] * perp;
float max1 = min1;
// project next point from A
max0 = SA[1] * perp;
if (max0 < min0)
{
JMath.Swapf(ref min0, ref max0);
JVector.Swap(ref SA[0], ref SA[1]);
}
max1 = SB[1] * perp;
if (max1 < min1)
{
JMath.Swapf(ref min1, ref max1);
JVector.Swap(ref SB[0], ref SB[1]);
}
if (min0 > min1)
{
JVector Pseg;
ProjectPointOnLine(ref SA[0], ref SB[0], ref SB[1], out Pseg);
CA[NumContacts] = SA[0];
CB[NumContacts] = Pseg;
NumContacts++;
}
else
{
JVector Pseg;
ProjectPointOnLine(ref SB[0], ref SA[0], ref SA[1], out Pseg);
CA[NumContacts] = Pseg;
CB[NumContacts] = SB[0];
NumContacts++;
}
if (max0 < max1)
{
JVector Pseg;
ProjectPointOnLine(ref SA[1], ref SB[0], ref SB[1], out Pseg);
CA[NumContacts] = SA[1];
CB[NumContacts] = Pseg;
NumContacts++;
}
else
{
JVector Pseg;
ProjectPointOnLine(ref SB[1], ref SA[0], ref SA[1], out Pseg);
CA[NumContacts] = Pseg;
CB[NumContacts] = SB[1];
NumContacts++;
}
return(true);
}
// if all axes overlap collision exists
return(true);
}