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