Monogame-Extended/source/MonoGame.Extended/Collisions/GeometricPrimitives.cs

using System;
using System.Runtime.CompilerServices;
using Microsoft.Xna.Framework;

namespace MonoGame.Extended.Collisions;

internal static class GeometricPrimitives
{
    #region Projection Utilities (for SAT)

    /// <summary>
    /// Projects a set of vertices onto an axis and returns the minium and maximum extents.
    /// </summary>
    /// <param name="vertices">The vertices to project.</param>
    /// <param name="axis">The axis to project onto.  Does not need to be normalized.</param>
    /// <param name="min">
    /// When this method returns, contains the minimum projection value.
    /// </param>
    /// <param name="max">
    /// When this method returns, contains the maximum projection value.
    /// </param>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static void ProjectOntoAxis(ReadOnlySpan<Vector2> vertices, Vector2 axis, out float min, out float max)
    {
        min = float.MaxValue;
        max = float.MinValue;

        for (int i = 0; i < vertices.Length; i++)
        {
            float projection = Vector2.Dot(vertices[i], axis);
            if (projection < min) min = projection;
            if (projection > max) max = projection;
        }
    }

    /// <summary>
    /// Projects a single point onto an axis.
    /// </summary>
    /// <param name="point">The point to project.</param>
    /// <param name="axis">The axis to project onto. Does not need to be normalized.</param>
    /// <returns>The scalar projection value.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static float ProjectPointOntoAxis(Vector2 point, Vector2 axis)
    {
        return Vector2.Dot(point, axis);
    }

    /// <summary>
    /// Projects an OBB onto an axis and returns the minimum and maximum extents.
    /// </summary>
    /// <param name="obb">The oriented bounding rectangle to project.</param>
    /// <param name="axis">The axis to project onto. Does not need to be normalized.</param>
    /// <param name="min">
    /// When this method returns, contains the minimum projection value.
    /// </param>
    /// <param name="max">
    /// When this method returns, contains the maximum projection value.
    /// </param>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static void ProjectOBBOntoAxis(in OrientedBoundingRectangle obb, Vector2 axis, out float min, out float max)
    {
        // Project center onto axis
        float centerProjection = Vector2.Dot(obb.Center, axis);

        // Project half-extents onto axis
        float extentProjection = MathF.Abs(Vector2.Dot(obb.AxisX * obb.HalfExtents.X, axis))
                               + MathF.Abs(Vector2.Dot(obb.AxisY * obb.HalfExtents.Y, axis));

        min = centerProjection - extentProjection;
        max = centerProjection + extentProjection;
    }

    /// <summary>
    /// Tests if two 1D intervals overlap.
    /// </summary>
    /// <param name="min1">Minimum of first interval.</param>
    /// <param name="max1">Maximum of first interval.</param>
    /// <param name="min2">Minimum of second intervale.</param>
    /// <param name="max2">Maximum of second interval.</param>
    /// <returns><see langword="true"/>if the intervals overlap; otherwise, <see langword="false"/>.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static bool IntervalsOverlap(float min1, float max1, float min2, float max2)
    {
        return !(max1 < min2 || max2 < min1);
    }

    #endregion

    #region Distance Calculations

    /// <summary>
    /// Computes the squared distance between points.
    /// </summary>
    /// <param name="a">The first point.</param>
    /// <param name="b">The second point.</param>
    /// <returns>The squared Euclidean distance between points.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static float DistanceSquared(Vector2 a, Vector2 b)
    {
        return Vector2.DistanceSquared(a, b);
    }

    /// <summary>
    /// Computes the squared distances from a point to a line segment.
    /// </summary>
    /// <param name="point">The point.</param>
    /// <param name="segmentStart">The segment start point.</param>
    /// <param name="segmentEnd">The segment end point.</param>
    /// <returns>The squared distance from point to the closest point on the segment.</returns>
    /// <remarks>
    /// More efficient than computing actual distance when only comparison is needed. <br/>
    /// Used on capsule intersection tests (Phase 3).
    /// </remarks>
    internal static float DistanceSquaredPointToSegment(Vector2 point, Vector2 segmentStart, Vector2 segmentEnd)
    {
        Vector2 ab = segmentEnd - segmentStart;
        Vector2 ap = point - segmentStart;

        float t = Vector2.Dot(ap, ab);
        if (t <= 0.0f)
        {
            // point projects before segment start
            return Vector2.DistanceSquared(point, segmentStart);
        }

        float denom = Vector2.Dot(ab, ab);
        if (t >= denom)
        {
            // Pont projects beyond segment
            return Vector2.DistanceSquared(point, segmentEnd);
        }

        // Point projects on segment interior
        t /= denom;
        Vector2 projection = segmentStart + t * ab;
        return Vector2.DistanceSquared(point, projection);
    }

    #endregion

    #region Closest Point Queries

    /// <summary>
    /// Finds the closest point on an axis-aligned bounding rectangle to a given point.
    /// </summary>
    /// <param name="rect">The AABB.</param>
    /// <param name="point">The query point.</param>
    /// <returns>The closest point on or inside the rectangle.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static Vector2 ClosestPointOnAABB(in BoundingRectangle rect, Vector2 point)
    {
        return new Vector2(
            Math.Clamp(point.X, rect.Min.X, rect.Max.X),
            Math.Clamp(point.Y, rect.Min.Y, rect.Max.Y)
        );
    }

    /// <summary>
    /// Finds the closest point on an oriented bounding rectangle to a given point.
    /// </summary>
    /// <param name="obb">The OBB.</param>
    /// <param name="point">The query point.</param>
    /// <returns>The closets point on or inside the rectangle.</returns>
    internal static Vector2 ClosestPointOnOBB(in OrientedBoundingRectangle obb, Vector2 point)
    {
        // Transform point into OBB's local coordinate system
        Vector2 d = point - obb.Center;

        // Project onto local axes and clamp to extents
        float distX = Vector2.Dot(d, obb.AxisX);
        float distY = Vector2.Dot(d, obb.AxisY);

        distX = Math.Clamp(distX, -obb.HalfExtents.X, obb.HalfExtents.X);
        distY = Math.Clamp(distY, -obb.HalfExtents.Y, obb.HalfExtents.Y);

        // Transform back to world space
        return obb.Center + distX * obb.AxisX + distY * obb.AxisY;
    }

    #endregion

    #region Rotation and Perpendicular Vectors

    /// <summary>
    /// Computes the perpendicular vector o the given 2D vector, rotated 90 degrees counter-clockwise.
    /// </summary>
    /// <param name="vector">The input vector.</param>
    /// <returns>The perpendicular vector (-vector.Y, vector.X).</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static Vector2 Perpendicular(Vector2 vector)
    {
        return new Vector2(-vector.Y, vector.X);
    }

    /// <summary>
    /// Normalizes a vector.  Returns zero vector if input length is zero.
    /// </summary>
    /// <param name="vector">The vector to normalize.</param>
    /// <returns>The normalized vector, or zero if input length is zero.</returns>
    [MethodImpl(MethodImplOptions.AggressiveInlining)]
    internal static Vector2 SafeNormalize(Vector2 vector)
    {
        float lengthSquared = vector.LengthSquared();

        if(lengthSquared < MathExtended.Epsilon * MathExtended.Epsilon)
        {
            return Vector2.Zero;
        }

        return vector / MathF.Sqrt(lengthSquared);
    }

    #endregion
}