/// <summary>
/// Collides the polygon and circle using the specified manifold
/// </summary>
/// <param name="manifold">The manifold</param>
/// <param name="polygon">The polygon</param>
/// <param name="xf1">The xf</param>
/// <param name="circle">The circle</param>
/// <param name="xf2">The xf</param>
public static void CollidePolygonAndCircle(ref Manifold manifold,
PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vec2 c = Math.Mul(xf2, circle.Position);
Vec2 cLocal = Math.MulT(xf1, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FltMax;
float radius = polygon.Radius + circle.Radius;
int vertexCount = polygon.VertexCount;
Vec2[] vertices = polygon.Vertices;
Vec2[] normals = polygon.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vec2.Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.FltEpsilon)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle.Position;
manifold.Points[0].Id.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vec2.Dot(cLocal - v1, v2 - v1);
float u2 = Vec2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle.Position;
manifold.Points[0].Id.Key = 0;
}
else if (u2 <= 0.0f)
{
if (Vec2.DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle.Position;
manifold.Points[0].Id.Key = 0;
}
else
{
Vec2 faceCenter = 0.5f * (v1 + v2);
float dot = Vec2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (dot > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle.Position;
manifold.Points[0].Id.Key = 0;
}
}