public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal- v1).sqrMagnitude > 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)
{
// Ahead of v2
if ((cLocal- v2).sqrMagnitude > 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
{
float separation = Vector2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}