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