bool SATCollide(SimBody *body1, SimBody *body2, float2 &N, f32 &t)
{
SimBody &a = *body1;
SimBody &b = *body2;
if(a.vertices.size() < 2 && b.vertices.size() < 2) return false;
Mat22 OA = Mat22::RotationMatrix(a.rotation_in_rads);
Mat22 OB = Mat22::RotationMatrix(b.rotation_in_rads);
Mat22 OB_T = OB.Transpose();
Mat22 xOrient = OA * OB_T;
float2 xOffset = (a.position - b.position) * OB_T;
const u32 MAX_SEPERATING_AXIS = 16;
float2 xAxis[MAX_SEPERATING_AXIS];
f32 taxis[MAX_SEPERATING_AXIS];
u32 axisCount = 0;
for(u32 i=0;i<a.seperatingAxis.size();++i)
{
xAxis[axisCount] = a.seperatingAxis[i] * xOrient;
if(!IntervalIntersect(a.vertices, b.vertices, xAxis[axisCount],
xOffset, xOrient, taxis[axisCount], t))
{
return false;
}
++axisCount;
};
for(u32 i=0;i<b.seperatingAxis.size();++i)
{
xAxis[axisCount] = b.seperatingAxis[i];
if(!IntervalIntersect(a.vertices, b.vertices, xAxis[axisCount],
xOffset, xOrient, taxis[axisCount], t))
{
return false;
}
++axisCount;
};
if(!GetMinimumTranslationVector(xAxis, taxis, axisCount, N, t))
{
return false;
}
f32 D = N.dot(xOffset);
N = D < 0.0f ? -N : N;
N = N * OB;
return true;
};
bool PolyColl::Collide(const Vector* A, int Anum,
const Vector* B, int Bnum,
const Vector& xOffset)
{
if (!A || !B) return false;
// All the separation axes
Vector xAxis[64]; // note : a maximum of 32 vertices per poly is supported
int iNumAxes=0;
// test separation axes of A
for(int j = Anum-1, i = 0; i < Anum; j = i, i ++)
{
Vector E0 = A[j];
Vector E1 = A[i];
Vector E = E1 - E0;
xAxis[iNumAxes] = Vector(-E.y, E.x);
if (!IntervalIntersect( A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset))
{
return false;
}
iNumAxes++;
}
// test separation axes of B
for(int j = Bnum-1, i = 0; i < Bnum; j = i, i ++)
{
Vector E0 = B[j];
Vector E1 = B[i];
Vector E = E1 - E0;
xAxis[iNumAxes] = Vector(-E.y, E.x);
if (!IntervalIntersect( A, Anum,
B, Bnum,
xAxis[iNumAxes],
xOffset))
{
return false;
}
iNumAxes++;
}
return true;
}
size_t collide(BoxShape &box1, BoxShape &box2, float dt, Contact *pxContacts, size_t numMaxContacts)
{
dt;
Point2f xAxis [4];
float fDepth[4];
xAxis[0] = box1.GetDir(0);
if (!IntervalIntersect(xAxis[0], box1, box2, fDepth[0])) return 0;
xAxis[1] = box1.GetDir(1);
if (!IntervalIntersect(xAxis[1], box1, box2, fDepth[1])) return 0;
xAxis[2] = box2.GetDir(0);
if (!IntervalIntersect(xAxis[2], box1, box2, fDepth[2])) return 0;
xAxis[3] = box2.GetDir(1);
if (!IntervalIntersect(xAxis[3], box1, box2, fDepth[3])) return 0;
float dcoll;
Point2f Ncoll;
if (!FindCollisionPlane(xAxis, fDepth, Ncoll, dcoll)) return 0;
Point2f D = box1.body.getPosition() - box2.body.getPosition();
if (D * Ncoll < 0.0f)
Ncoll *= -1.0f;
Point2f xPoints0[4];
Point2f xPoints1[4];
size_t numPoints0;
size_t numPoints1;
numPoints0 = box1.FindSupportPoints( Ncoll, xPoints0);
numPoints1 = box2.FindSupportPoints(-Ncoll, xPoints1);
if (!ConvertSupportPointsToContacts(box1, box2, Ncoll, xPoints0, numPoints0, xPoints1, numPoints1, pxContacts, numMaxContacts))
return 0;
return numMaxContacts;
}
