public static bool PolygonvsPolygon(ref Manifold pM) { PolygonMesh A = (PolygonMesh)pM.BodyA.Mesh; PolygonMesh B = (PolygonMesh)pM.BodyB.Mesh; float penetrationA = FindAxisLeastPenetration(out int faceA, A, B); if (penetrationA >= 0) { return(false); } float penetrationB = FindAxisLeastPenetration(out int faceB, B, A); if (penetrationB >= 0) { return(false); } int referenceIndex; bool flip; PolygonMesh reference; PolygonMesh incident; if (penetrationA >= penetrationB * .95f + penetrationA * .01f) { reference = A; incident = B; referenceIndex = faceA; flip = false; } else { reference = B; incident = A; referenceIndex = faceB; flip = true; } FindIncidentFace(out Vector2[] incidentFace, reference, incident, referenceIndex); Vector2 v1 = reference.Vertices[referenceIndex]; referenceIndex = referenceIndex + 1 == reference.Vertices.Length ? 0 : referenceIndex + 1; Vector2 v2 = reference.Vertices[referenceIndex]; var rMat = reference.Body.GameObject.RotationMatrix; v1 = rMat * v1 + reference.Body.AABB.Center; v2 = rMat * v2 + reference.Body.AABB.Center; Vector2 sidePlaneNormal = v2 - v1; sidePlaneNormal.Normalize(); Vector2 refFaceNormal = new Vector2(sidePlaneNormal.Y, -sidePlaneNormal.X); float refC = Vector2.DotProduct(refFaceNormal, v1); float negSide = -Vector2.DotProduct(sidePlaneNormal, v1); float posSide = Vector2.DotProduct(sidePlaneNormal, v2); if (Clip(-sidePlaneNormal, negSide, ref incidentFace) < 2) { return(false); } if (Clip(sidePlaneNormal, posSide, ref incidentFace) < 2) { return(false); } pM.Normal = flip ? -refFaceNormal : refFaceNormal; float separation = Vector2.DotProduct(refFaceNormal, incidentFace[0]) - refC; List <Vector2> contacts = new List <Vector2>(); if (separation <= 0) { contacts.Add(incidentFace[0]); pM.Penetration = -separation; } separation = Vector2.DotProduct(refFaceNormal, incidentFace[1]) - refC; if (separation <= 0) { contacts.Add(incidentFace[1]); pM.Penetration += -separation; pM.Penetration /= contacts.Count; } pM.Contacts = contacts.ToArray(); return(pM.Contacts.Length > 0); }
public static bool CirclevsPolygon(ref Manifold pM) { ColliderComponent A = pM.BodyA; ColliderComponent B = pM.BodyB; CircleMesh c = (CircleMesh)A.Mesh; PolygonMesh p = (PolygonMesh)B.Mesh; var bMat = B.GameObject.RotationMatrix; Vector2 center = A.AABB.Center; center = bMat.Transpose() * (center - B.AABB.Center); float maxSeparation = float.MinValue; int faceNormal = 0; for (int i = 0; i < p.Vertices.Length; i++) { float sep = Vector2.DotProduct(p.Normals[i], center - p.Vertices[i]); if (sep > c.Radius) { return(false); } if (sep > maxSeparation) { maxSeparation = sep; faceNormal = i; } } Vector2 v1 = p.Vertices[faceNormal]; int nextFace = faceNormal + 1 < p.Vertices.Length ? faceNormal + 1 : 0; Vector2 v2 = p.Vertices[nextFace]; if (maxSeparation < .00005f) { pM.Normal = -(bMat * p.Normals[faceNormal]); pM.Contacts = new Vector2[] { pM.Normal *c.Radius + A.AABB.Center }; pM.Penetration = c.Radius; return(true); } float dot1 = Vector2.DotProduct(center - v1, v2 - v1); float dot2 = Vector2.DotProduct(center - v2, v1 - v2); pM.Penetration = c.Radius - maxSeparation; if (dot1 <= 0) { if (Vector2.DistanceSqrd(center, v1) > c.Radius * c.Radius) { return(false); } Vector2 n = v1 - center; n = bMat * n; n.Normalize(); pM.Normal = n; v1 = bMat * v1 + B.AABB.Center; pM.Contacts = new Vector2[] { v1 }; } else if (dot2 <= 0) { if (Vector2.DistanceSqrd(center, v2) > c.Radius * c.Radius) { return(false); } Vector2 n = v2 - center; n = bMat * n; n.Normalize(); pM.Normal = n; v2 = bMat * v2 + B.AABB.Center; pM.Contacts = new Vector2[] { v2 }; } else { Vector2 n = p.Normals[faceNormal]; if (Vector2.DotProduct(center - v1, n) > c.Radius) { return(false); } n = bMat * n; pM.Normal = -n; pM.Contacts = new Vector2[] { pM.Normal *c.Radius + A.AABB.Center }; } return(true); }
public static bool PolygonvsCircle(ref Manifold pM) { pM = new Manifold(pM.BodyB, pM.BodyA); return(CirclevsPolygon(ref pM)); }