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