private static bool DoTheyCollide(ConvexPolygon shapeA, ConvexPolygon shapeB, Body bodyA,
Body bodyB, out Vector2 impactNormal, out float penetration)
{
Vector2[] pointsA = shapeA.GetAdjustedPointPositions(bodyA.Position, bodyA.Rotation);
Vector2[] pointsB = shapeB.GetAdjustedPointPositions(bodyB.Position, bodyB.Rotation);
return(DoTheyCollide(pointsA, pointsA, out impactNormal, out penetration));
}
private static bool DoTheyCollide(ConvexPolygon shapeA, Circular shapeB, Body bodyA,
Vector2 positionB, out Vector2 impactNormal, out float penetration)
{
impactNormal = Vector2.Zero;
penetration = 0f;
Vector2[] pointsA = shapeA.GetAdjustedPointPositions(bodyA.Position, bodyA.Rotation);
Vector2[] normals = CreateNormalUnitVectors(pointsA);
float minPenatration = float.MaxValue;
bool doTheyCollide = true;
for (int i = 0; i < 3; i++)
{
Vector2 radiusVector = normals[i] * shapeB.Radius;
Range extentRangeA = Range.FindExtent(normals[i], pointsA);
Range extentRangeB = Range.FindExtent(normals[i], positionB - radiusVector, positionB + radiusVector);
if (extentRangeA.Max < extentRangeB.Min || extentRangeB.Max < extentRangeB.Min)
{
doTheyCollide = false;
break;
}
float halfExtentLengthA = extentRangeA.Length / 2f;
float halfExtentLengthB = extentRangeB.Length / 2f;
float extentCenterDelta = (halfExtentLengthB + extentRangeB.Min) - (halfExtentLengthA + extentRangeA.Min);
float overlap = halfExtentLengthA + halfExtentLengthB - Math.Abs(extentCenterDelta);
if (overlap < minPenatration)
{
minPenatration = overlap;
impactNormal = normals[i];
}
}
if (doTheyCollide)
{
penetration = minPenatration;
}
return(doTheyCollide);
}
