diff --git a/src/Collision/NarrowPhase.cs b/src/Collision/NarrowPhase.cs index ddad6e92..71978756 100644 --- a/src/Collision/NarrowPhase.cs +++ b/src/Collision/NarrowPhase.cs @@ -13,23 +13,41 @@ namespace MoonWorks.Collision public static bool TestCollision(IShape2D shapeA, Transform2D transformA, IShape2D shapeB, Transform2D transformB) { - if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.Rotation == 0 && transformB.Rotation == 0) + // If we can use a fast path check, let's do that! + if (shapeA is Rectangle rectangleA && shapeB is Rectangle rectangleB && transformA.IsAxisAligned && transformB.IsAxisAligned) { return TestRectangleOverlap(rectangleA, transformA, rectangleB, transformB); } - else if (shapeA is Point && shapeB is Rectangle && transformB.Rotation == 0) + else if (shapeA is Point && shapeB is Rectangle && transformB.IsAxisAligned) { return TestPointRectangleOverlap((Point) shapeA, transformA, (Rectangle) shapeB, transformB); } - else if (shapeA is Rectangle && shapeB is Point && transformA.Rotation == 0) + else if (shapeA is Rectangle && shapeB is Point && transformA.IsAxisAligned) { return TestPointRectangleOverlap((Point) shapeB, transformB, (Rectangle) shapeA, transformA); } - else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.Scale.X == transformA.Scale.Y && transformB.Scale.X == transformB.Scale.Y) + else if (shapeA is Rectangle && shapeB is Circle && transformA.IsAxisAligned && transformB.IsUniformScale) + { + return TestCircleRectangleOverlap((Circle) shapeB, transformB, (Rectangle) shapeA, transformA); + } + else if (shapeA is Circle && shapeB is Rectangle && transformA.IsUniformScale && transformB.IsAxisAligned) + { + return TestCircleRectangleOverlap((Circle) shapeA, transformA, (Rectangle) shapeB, transformB); + } + else if (shapeA is Circle && shapeB is Point && transformA.IsUniformScale) + { + return TestCirclePointOverlap((Circle) shapeA, transformA, (Point) shapeB, transformB); + } + else if (shapeA is Point && shapeB is Circle && transformB.IsUniformScale) + { + return TestCirclePointOverlap((Circle) shapeB, transformB, (Point) shapeA, transformA); + } + else if (shapeA is Circle circleA && shapeB is Circle circleB && transformA.IsUniformScale && transformB.IsUniformScale) { return TestCircleOverlap(circleA, transformA, circleB, transformB); } + // Sad, we can't do a fast path optimization. Time for a simplex reduction. return FindCollisionSimplex(shapeA, transformA, shapeB, transformB).Item1; } @@ -49,6 +67,36 @@ namespace MoonWorks.Collision return transformedPoint.X > AABB.Left && transformedPoint.X < AABB.Right && transformedPoint.Y < AABB.Bottom && transformedPoint.Y > AABB.Top; } + public static bool TestCirclePointOverlap(Circle circle, Transform2D circleTransform, Point point, Transform2D pointTransform) + { + var circleCenter = circleTransform.Position; + var circleRadius = circle.Radius * circleTransform.Scale.X; + + var distanceX = circleCenter.X - pointTransform.Position.X; + var distanceY = circleCenter.Y - pointTransform.Position.Y; + + return (distanceX * distanceX) + (distanceY * distanceY) < (circleRadius * circleRadius); + } + + /// + /// NOTE: The rectangle must be axis aligned, and the scaling of the circle must be uniform. + /// + public static bool TestCircleRectangleOverlap(Circle circle, Transform2D circleTransform, Rectangle rectangle, Transform2D rectangleTransform) + { + var circleCenter = circleTransform.Position; + var circleRadius = circle.Radius * circleTransform.Scale.X; + var AABB = rectangle.TransformedAABB(rectangleTransform); + + var closestX = Math.MathHelper.Clamp(circleCenter.X, AABB.Left, AABB.Right); + var closestY = Math.MathHelper.Clamp(circleCenter.Y, AABB.Top, AABB.Bottom); + + var distanceX = circleCenter.X - closestX; + var distanceY = circleCenter.Y - closestY; + + var distanceSquared = (distanceX * distanceX) + (distanceY * distanceY); + return distanceSquared < (circleRadius * circleRadius); + } + public static bool TestCircleOverlap(Circle circleA, Transform2D transformA, Circle circleB, Transform2D transformB) { var radiusA = circleA.Radius * transformA.Scale.X; diff --git a/src/Math/Transform2D.cs b/src/Math/Transform2D.cs index 2a62c680..c9fcb582 100644 --- a/src/Math/Transform2D.cs +++ b/src/Math/Transform2D.cs @@ -8,6 +8,17 @@ namespace MoonWorks.Math public Matrix3x2 TransformMatrix { get; } + public bool IsAxisAligned => Rotation % MathHelper.PiOver2 == 0; + public bool IsUniformScale => Scale.X == Scale.Y; + + public Transform2D() + { + Position = Vector2.Zero; + Rotation = 0; + Scale = Vector2.One; + TransformMatrix = CreateTransformMatrix(Position, Rotation, Scale); + } + public Transform2D(Vector2 position) { Position = position;