Monogame-Extended/source/MonoGame.Extended/Math/IntersectionTests.cs

using System;
using Microsoft.Xna.Framework;

namespace MonoGame.Extended;

/// <summary>
/// Provides methods for testing geometric intersections between shapes.
/// </summary>
internal static class IntersectionTests
{
    /// <summary>
    /// Determines whether two circles intersect.
    /// </summary>
    /// <param name="a">The first circle.</param>
    /// <param name="b">The second circle.</param>
    /// <returns>
    /// <c>true</c> if the circles intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Two circles intersect when the distance between their centers is less than or equal to the sum of their radii.
    /// <para>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 4.3.1: Bounding Spheres - Sphere-Sphere Intersection
    /// </para>
    /// </remarks>
    public static bool CircleCircle(in Circle a, in Circle b)
    {
        float distSq = Vector2.DistanceSquared(a.Center, b.Center);
        float radiiSum = a.Radius + b.Radius;
        return distSq <= radiiSum * radiiSum;
    }

    /// <summary>
    /// Determines whether a circle and an ellipse intersect.
    /// </summary>
    /// <param name="circle">The circle to test.</param>
    /// <param name="ellipse">The ellipse to test.</param>
    /// <returns>
    /// <c>true</c> if the circle and ellipse intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// This method uses an approximation that provides reasonable accuracy for most collision detection scenarios.
    /// </remarks>
    public static bool CircleEllipse(in Circle circle, in Ellipse ellipse)
    {
        Vector2 closestPoint = ellipse.ClosestPointTo(circle.Center);
        float distSq = Vector2.DistanceSquared(closestPoint, circle.Center);
        return distSq <= circle.Radius * circle.Radius;
    }

    /// <summary>
    /// Determines whether a circle and an axis-aligned rectangle intersect.
    /// </summary>
    /// <param name="circle">The circle to test.</param>
    /// <param name="rectangle">The axis-aligned rectangle to test.</param>
    /// <returns>
    /// <c>true</c> if the circle and rectangle intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.2.5: Testing Sphere against AABB
    /// </remarks>
    public static bool CircleRectangleF(in Circle circle, in RectangleF rectangle)
    {
        float distSq = rectangle.DistanceToSquared(circle.Center);
        return distSq <= circle.Radius * circle.Radius;
    }

    /// <summary>
    /// Determines whether a circle and a ray intersect.
    /// </summary>
    /// <param name="circle">The circle to test.</param>
    /// <param name="ray">The ray to test.</param>
    /// <returns>
    /// <c>true</c> if the ray intersects the circle; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.2: Intersecting Ray or Segment Against Sphere
    /// </remarks>
    public static bool CircleRay(in Circle circle, in Ray ray)
    {
        Vector2 m = ray.Origin - circle.Center;
        float c = Vector2.Dot(m, m) - circle.Radius * circle.Radius;

        // If ray origin is inside circle, there is definitely an intersection
        if (c <= 0.0f)
        {
            return true;
        }

        float b = Vector2.Dot(m, ray.Direction);

        // Exit early if ray origin is outside circle and ray is pointing away
        // from circle
        if (b > 0.0f)
        {
            return false;
        }

        float discriminant = b * b - c;

        // A negative discriminant corresponds to ray missing circle
        if (discriminant < 0.0f)
        {
            return false;
        }

        return true;
    }

    /// <summary>
    /// Determines whether a circle and a line segment intersect.
    /// </summary>
    /// <param name="circle">The circle to test.</param>
    /// <param name="lineSegment">The line segment to test.</param>
    /// <returns>
    /// <c>true</c> if the circle and line segment intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.1.2: Closest Point on Line Segment to Point<br/>
    /// Chapter 5.1.2.1: Distance of Point to Segment
    /// </remarks>
    public static bool CirceLineSegment(in Circle circle, in LineSegment lineSegment)
    {
        float distSq = lineSegment.DistanceSquaredTo(circle.Center);
        return distSq <= circle.Radius * circle.Radius;
    }

    /// <summary>
    /// Determines whether two ellipses intersect.
    /// </summary>
    /// <param name="a">The first ellipse.</param>
    /// <param name="b">The second ellipse.</param>
    /// <returns>
    /// <c>true</c> if the ellipses intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// This method uses an approximation that provides reasonable accuracy for most collision detection scenarios.
    /// </remarks>
    public static bool EllipseEllipse(in Ellipse a, in Ellipse b)
    {
        Vector2 closestOnB = b.ClosestPointTo(a.Center);
        if (a.Contains(closestOnB))
        {
            return true;
        }

        Vector2 closestOnA = a.ClosestPointTo(b.Center);
        return b.Contains(closestOnA);

    }

    /// <summary>
    /// Determines whether an ellipse and an axis-aligned rectangle intersect.
    /// </summary>
    /// <param name="ellipse">The ellipse to test.</param>
    /// <param name="rectangle">The axis-aligned rectangle to test.</param>
    /// <returns>
    /// <c>true</c> if the ellipse and rectangle intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.2.5: Testing Sphere Against AABB<br/>
    /// (adapted for ellipses)
    /// </remarks>
    public static bool EllipseRectangleF(in Ellipse ellipse, in RectangleF rectangle)
    {
        // Find the closest point on the rectangle to the ellipse center
        // Clamp ellipse center to the rectangle bounds (analogous to sphere-AABB test)
        float closestX = Math.Clamp(ellipse.Center.X, rectangle.Left, rectangle.Right);
        float closestY = Math.Clamp(ellipse.Center.Y, rectangle.Top, rectangle.Bottom);
        Vector2 closestPoint = new Vector2(closestX, closestY);

        return ellipse.Contains(closestPoint);

    }

    /// <summary>
    /// Determines whether an ellipse and a ray intersect.
    /// </summary>
    /// <param name="ellipse">The ellipse to test.</param>
    /// <param name="ray">The ray to test.</param>
    /// <returns>
    /// <c>true</c> if the ray intersects the ellipse; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.2: Intersecting Ray or Segment Against Sphere
    /// </remarks>
    public static bool EllipseRay(in Ellipse ellipse, in Ray ray)
    {
        // Transform the ray into ellipse local space where it's a unit circle
        // This makes the intersection test much simpler

        // 1. Translate ray origin relative to ellipse center
        Vector2 localOrigin = ray.Origin - ellipse.Center;

        // 2. Rotate ray by -ellipse.Rotation to align with axes
        float cos = MathF.Cos(-ellipse.Rotation);
        float sin = MathF.Sin(-ellipse.Rotation);

        Vector2 rotatedOrigin = new Vector2(
            localOrigin.X * cos - localOrigin.Y * sin,
            localOrigin.X * sin + localOrigin.Y * cos
        );

        Vector2 rotatedDirection = new Vector2(
            ray.Direction.X * cos - ray.Direction.Y * sin,
            ray.Direction.X * sin + ray.Direction.Y * cos
        );

        // 3. Scale to transform ellipse into unit circle
        Vector2 scaledOrigin = new Vector2(
            rotatedOrigin.X / ellipse.RadiusX,
            rotatedOrigin.Y / ellipse.RadiusY
        );

        Vector2 scaledDirection = new Vector2(
            rotatedDirection.X / ellipse.RadiusX,
            rotatedDirection.Y / ellipse.RadiusY
        );

        // 4. Normalize the scaled directions
        float dirLength = scaledDirection.Length();
        if (dirLength < 0.0001f)
        {
            // Degenerate ray
            return false;
        }

        scaledDirection /= dirLength;

        // 5. Perform ray-circle intersection in unit circle space
        // Ray equation: P(t) = origin + t * direction
        // Circle equation: x^2 + y^2 = 1
        // Substitute and solve quadratic: (o + td)·(o + td) = 1

        float a = Vector2.Dot(scaledDirection, scaledDirection);
        float b = 2.0f * Vector2.Dot(scaledOrigin, scaledDirection);
        float c = Vector2.Dot(scaledOrigin, scaledOrigin) - 1.0f;

        float discriminant = b * b - 4 * a * c;

        // No intersection if discriminant is negative
        return discriminant >= 0.0f;
    }

    /// <summary>
    /// Determines whether an ellipse and a line segment intersect.
    /// </summary>
    /// <param name="ellipse">The ellipse to test.</param>
    /// <param name="lineSegment">The line segment to test.</param>
    /// <returns>
    /// <c>true</c> if the ellipse and line segment intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.2: Intersecting Ray or Segment Against Sphere
    /// </remarks>
    public static bool EllipseLineSegment(in Ellipse ellipse, in LineSegment lineSegment)
    {
        // Transform the line segment into ellipse local space
        Vector2 p1 = lineSegment.Start;
        Vector2 p2 = lineSegment.End;
        Vector2 direction = p2 - p1;
        float segmentLength = direction.Length();

        if (segmentLength < 0.0001f)
        {
            // Degenerate segment
            // just test point containment
            return ellipse.Contains(p1);
        }

        direction /= segmentLength;

        // 1. Translate segment relative to ellipse center
        Vector2 localP1 = p1 - ellipse.Center;

        // 2. Rotate segment by -ellipse.Rotation
        float cos = MathF.Cos(-ellipse.Rotation);
        float sin = MathF.Sin(-ellipse.Rotation);

        Vector2 rotatedP1 = new Vector2(
            localP1.X * cos - localP1.Y * sin,
            localP1.X * sin + localP1.Y * cos
        );

        Vector2 rotatedDir = new Vector2(
            direction.X * cos - direction.Y * sin,
            direction.X * sin + direction.Y * cos
        );

        // 3. Scale to unit circle space
        Vector2 scaledOrigin = new Vector2(
            rotatedP1.X / ellipse.RadiusX,
            rotatedP1.Y / ellipse.RadiusY
        );

        Vector2 scaledDirection = new Vector2(
            rotatedDir.X / ellipse.RadiusX,
            rotatedDir.Y / ellipse.RadiusY
        );

        float scaledDirLength = scaledDirection.Length();
        if (scaledDirLength < 0.0001f)
        {
            return ellipse.Contains(p1);
        }

        scaledDirection /= scaledDirLength;

        // 4. Solve quadratic for ray-circle intersection
        float a = Vector2.Dot(scaledDirection, scaledDirection);
        float b = 2.0f * Vector2.Dot(scaledOrigin, scaledDirection);
        float c = Vector2.Dot(scaledOrigin, scaledOrigin) - 1.0f;

        float discriminant = b * b - 4 * a * c;

        if (discriminant < 0)
        {
            // no intersection
            return false;
        }

        // 5. Check if intersection points lie within segment bounds
        float sqrtDiscriminant = MathF.Sqrt(discriminant);
        float t1 = (-b - sqrtDiscriminant) / (2 * a);
        float t2 = (-b + sqrtDiscriminant) / (2 * a);

        // Scale t values back to original segment parameterization
        // Account for the scaling transformation
        float scaleRatio = segmentLength / scaledDirLength;
        t1 *= scaleRatio;
        t2 *= scaleRatio;

        // Check if either intersection point is within [0, segmentLength]
        return (t1 >= 0 && t1 <= segmentLength)
               || (t2 >= 0 && t2 <= segmentLength)
               || (t1 < 0 && t2 > segmentLength);
    }

    /// <summary>
    /// Determines whether two axis-aligned rectangles intersect.
    /// </summary>
    /// <param name="a">The first rectangle.</param>
    /// <param name="b">The second rectangle.</param>
    /// <returns>
    /// <c>true</c> if the rectangles intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 4.2.1: AABB-AABB Intersection
    /// </remarks>
    public static bool RectangleFRectangleF(in RectangleF a, in RectangleF b)
    {
        return a.Left < b.Right
               && b.Left < a.Right
               && a.Top < b.Bottom
               && b.Bottom < a.Top;
    }

    /// <summary>
    /// Determines whether an axis-aligned rectangle and a ray intersect.
    /// </summary>
    /// <param name="rectangle">The axis-aligned rectangle to test.</param>
    /// <param name="ray">The ray to test.</param>
    /// <returns>
    /// <c>true</c> if the ray intersects the rectangle; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.3: Intersecting Ray or Segment Against Box
    /// </remarks>
    public static bool RectangleFRay(in RectangleF rectangle, in Ray ray)
    {
        float tMin = 0.0f;
        float tMax = float.MaxValue;

        // Test intersection with X slab
        if (MathF.Abs(ray.Direction.X) < MathExtended.Epsilon)
        {
            // Ray is parallel to slap.
            // No hit if origin not within slab
            if (ray.Origin.X < rectangle.Left || ray.Origin.X > rectangle.Right)
            {
                return false;
            }
        }
        else
        {
            // Compute intersection t value of ray with near and far plane of slab
            float ood = 1.0f / ray.Direction.X;
            float t1 = (rectangle.Left - ray.Origin.X) * ood;
            float t2 = (rectangle.Right - ray.Origin.X) * ood;

            // Make t1 be intersection with near plan, t2 with far plane
            if (t1 > t2)
            {
                (t1, t2) = (t2, t1);
            }

            // Compute the intersection of slab intersection intervals
            tMin = MathF.Max(t1, tMin);
            tMax = MathF.Min(t2, tMax);

            // Exit with no collision as soon as slab intersection becomes empty
            if (tMin > tMax)
            {
                return false;
            }
        }

        // Test intersection with Y slab
        if (MathF.Abs(ray.Direction.Y) < MathExtended.Epsilon)
        {
            // Ray is parallel to slab.
            // No hit if origin not within slab
            if (ray.Origin.Y < rectangle.Top || ray.Origin.Y > rectangle.Bottom)
            {
                return false;
            }
        }
        else
        {
            // Compute the intersection t value of ray with near and far plane of slab
            float ood = 1.0f / ray.Direction.Y;
            float t1 = (rectangle.Top - ray.Origin.Y) * ood;
            float t2 = (rectangle.Bottom - ray.Origin.Y) * ood;

            // Make t1 be intersection with near plan, t2 with far plan
            if (t1 > t2)
            {
                (t1, t2) = (t2, t1);
            }

            // Compute the intersection of slab intersection intervals
            tMin = MathF.Max(t1, tMin);
            tMax = MathF.Min(t2, tMax);

            // Exit with no collision as soon as slab intersection becomes empty
            if (tMin > tMax)
            {
                return false;
            }
        }

        // Ray intersects all slabs
        return true;
    }

    /// <summary>
    /// Determines whether an axis-aligned rectangle and a line segment intersect.
    /// </summary>
    /// <param name="rectangle">The axis-aligned rectangle to test.</param>
    /// <param name="lineSegment">The line segment to test.</param>
    /// <returns>
    /// <c>true</c> if the rectangle and line segment intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.3: Testing Segment Against AABB
    /// </remarks>
    public static bool RectangleFLineSegment(in RectangleF rectangle, in LineSegment lineSegment)
    {
        // Box center point
        Vector2 c = rectangle.Center;

        // Box half length extents
        Vector2 e = new Vector2(rectangle.Width, rectangle.Height) * 0.5f;

        // Segment midpoint
        Vector2 m = lineSegment.Center;

        // Segment half length vector
        Vector2 d = lineSegment.End - m;

        // Translate box and segment to origin
        m -= c;

        // Try world coordinate axes as separating axes
        float adx = MathF.Abs(d.X);
        if (MathF.Abs(m.X) > e.X + adx) return false;

        float ady = MathF.Abs(d.Y);
        if (MathF.Abs(m.Y) > e.Y + ady) return false;

        // Add in an epsilon term to counteract arithmetic errors when segment
        // is (near) parallel to coordinate axis
        adx += MathExtended.Epsilon;
        ady += MathExtended.Epsilon;

        // Try cross products of segment direction vector with coordinate axes
        // For 2D, we only have one cross product to test (the Z component of the 3D cross product)
        if (Math.Abs(m.X * d.Y - m.Y * d.X) > e.X * ady + e.Y * adx) return false;

        // No separating axis found, segment must be overlapping AABB
        return true;
    }

    /// <summary>
    /// Determines whether two rays in 2D intersect.
    /// </summary>
    /// <param name="a">The first ray.</param>
    /// <param name="b">The second ray.</param>
    /// <returns>
    /// <c>true</c> if the rays intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Returns <c>false</c> for parallel or coincident rays.
    /// <para>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.1.9.1: 2D Segment Intersection
    /// </para>
    /// </remarks>
    public static bool RayRay(in Ray a, in Ray b)
    {
        Vector2 r = b.Origin - a.Origin;

        // Using 2D cross product: s = (r × b.Direction) / (a.Direction × b.Direction)
        // For 2D vectors: u × v = u.X * v.Y - u.Y * v.X

        float denom = a.Direction.X * b.Direction.Y - a.Direction.Y * b.Direction.X;

        // Parallel or coincident rays
        if (MathF.Abs(denom) < MathExtended.Epsilon)
        {
            return false;
        }

        float s = (r.X * b.Direction.Y - r.Y * b.Direction.X) / denom;
        float t = (r.X * a.Direction.Y - r.Y * a.Direction.X) / denom;

        // Rays intersect if both parameters are non-negative
        return s >= 0.0f & t >= 0.0f;
    }

    /// <summary>
    /// Determines whether a ray and a line segment in 2D intersect.
    /// </summary>
    /// <param name="ray">The ray to test.</param>
    /// <param name="lineSegment">The line segment to test.</param>
    /// <returns>
    /// <c>true</c> if the ray and line segment intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Returns <c>false</c> for parallel or coincident cases.
    /// <para>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.3.1: Intersecting Segment Against Plane
    /// </para>
    /// </remarks>
    public static bool RayLineSegment(in Ray ray, in LineSegment lineSegment)
    {
        Vector2 segmentDir = lineSegment.Direction;
        Vector2 r = lineSegment.Start - ray.Origin;

        // Using 2D cross produce to solve the system
        float denom = ray.Direction.X * segmentDir.Y - ray.Direction.Y * segmentDir.X;

        // Parallel or coincident
        if (MathF.Abs(denom) < MathExtended.Epsilon)
        {
            return false;
        }

        float s = (r.X * segmentDir.Y - r.Y * segmentDir.X) / denom;
        float t = (r.X * ray.Direction.Y - r.Y * ray.Direction.X) / denom;

        // Ray intersects segment if s >= 0 (ray constraint) and 0 <= t <= 1 (segment constraint)
        return s >= 0.0f && t >= 0.0f && t <= 1.0f;
    }

    /// <summary>
    /// Determines whether two line segments in 2D intersect.
    /// </summary>
    /// <param name="a">The first line segment.</param>
    /// <param name="b">The second line segment.</param>
    /// <returns>
    /// <c>true</c> if the line segments intersect; otherwise, <c>false</c>.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.1.9.1: 2D Segment Intersection
    /// </remarks>
    public static bool LineSegmentLineSegment(in LineSegment a, in LineSegment b)
    {
        // Compute signed areas for triangles formed by segment a and endpoints of segment b
        float a1 = SignedTriangleArea(a.Start, a.End, b.End);
        float a2 = SignedTriangleArea(a.Start, a.End, b.Start);

        // If both endpoints of b are on the same side of a, no intersection
        if (a1 * a2 > 0.0f) return false;

        // Compute signed areas for triangles formed by segment b and endpoints of segment a
        float a3 = SignedTriangleArea(b.Start, b.End, a.Start);
        float a4 = SignedTriangleArea(b.Start, b.End, a.End);

        // If both endpoints of a are on the same side of b, no intersection
        if (a3 * a4 > 0.0f) return false;

        // Segments intersect
        return true;
    }

    /// <summary>
    /// Computes twice the signed area of a triangle defined by three points.
    /// </summary>
    /// <param name="a">The first vertex of the triangle.</param>
    /// <param name="b">The second vertex of the triangle.</param>
    /// <param name="c">The third vertex of the triangle.</param>
    /// <returns>
    /// A positive value if the triangle vertices are ordered counterclockwise,
    /// a negative value if clockwise, or zero if the points are collinear.
    /// </returns>
    /// <remarks>
    /// Reference: Real-Time Collision Detection by Christer Ericson (2005)<br/>
    /// Chapter 5.1.9.1: 2D Segment Intersection
    /// </remarks>
    private static float SignedTriangleArea(Vector2 a, Vector2 b, Vector2 c)
    {
        return (a.X - c.X) * (b.Y - c.Y) - (a.Y - c.Y) * (b.X - c.X);
    }
}