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