public static void PolygonToPolygon(Manifold m, Body a, Body b)
{
Polygon shapeA = a.shape as Polygon;
Polygon shapeB = b.shape as Polygon;
m.contactCount = 0;
//Check for separating axis on A's normals
int faceA;
float penetrationA = FindLeastPenetrationAxis(shapeA, shapeB, out faceA);
if (penetrationA >= 0)
{
return;
}
//Check for separating axis on B's normals
int faceB;
float penetrationB = FindLeastPenetrationAxis(shapeB, shapeA, out faceB);
if (penetrationB >= 0)
{
return;
}
int referenceIndex;
//Make sure we always point from A to B for consistent results
bool flip;
Polygon refPoly, incPoly;
if (Mathf.BiasGreaterThan(penetrationA, penetrationB))
{
refPoly = shapeA;
incPoly = shapeB;
referenceIndex = faceA;
flip = false;
}
else
{
refPoly = shapeB;
incPoly = shapeA;
referenceIndex = faceB;
flip = true;
}
Vec2[] incidentFace;
GetIncidentFace(out incidentFace, refPoly, incPoly, referenceIndex);
//Set up vertices
Vec2 v1 = refPoly.vertices[referenceIndex];
Vec2 v2 = refPoly.vertices[(referenceIndex + 1) % refPoly.VertexCount];
//Transform to world space
v1 = refPoly.transform.LocalToWorldPosition(v1);
v2 = refPoly.transform.LocalToWorldPosition(v2);
Vec2 sidePlaneNormal = v2 - v1;
sidePlaneNormal.Normalize();
Vec2 refFaceNormal = new Vec2(sidePlaneNormal.y, -sidePlaneNormal.x);
float refC = Vec2.Dot(refFaceNormal, v1);
float negSide = -Vec2.Dot(sidePlaneNormal, v1);
float posSide = Vec2.Dot(sidePlaneNormal, v2);
// Due to floating point error, possible to not have required points
if (Clip(-sidePlaneNormal, negSide, ref incidentFace) < 2)
{
return;
}
if (Clip(sidePlaneNormal, posSide, ref incidentFace) < 2)
{
return;
}
m.normal = flip ? -refFaceNormal : refFaceNormal;
Contact[] contacts = new Contact[2];
// Keep points behind reference face
int cp = 0; // clipped points behind reference face
for (int i = 0; i < 2; i++)
{
float separation = Vec2.Dot(refFaceNormal, incidentFace[i]) - refC;
if (separation <= 0.0f)
{
contacts[cp] = new Contact(incidentFace[i]);
contacts[cp].penetration = -separation;
++cp;
}
}
m.contactCount = cp;
m.Update(cp, contacts);
}
