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