Engine-Pong/Game/Physics2D/PhysicsMath.cs

using System;
using System.Collections.Generic;
using System.Diagnostics.CodeAnalysis;

using Microsoft.Xna.Framework;

namespace Syntriax.Engine.Physics2D;

public record Line(Vector2 From, Vector2 To)
{
    public Vector2 Direction => Vector2.Normalize(To - From);
    public float Length => (From - To).Length();
    public float LengthSquared => (From - To).LengthSquared();
}

public record LineEquation(float Slope, float OffsetY);
public record Triangle(Vector2 A, Vector2 B, Vector2 C);
public record Circle(Vector2 Position, float Radius);
public record AABB(Vector2 LowerBoundary, Vector2 UpperBoundary);

public static class PhysicsMath
{
    public static Vector2 Scale(this Vector2 original, Vector2 scale)
        => new Vector2(original.X * scale.X, original.Y * scale.Y);

    public static Vector2 ClosestPointTo(this Line line, Vector2 point)
    {
        // Convert edge points to vectors
        var edgeVector = new Vector2(line.To.X - line.From.X, line.To.Y - line.From.Y);
        var pointVector = new Vector2(point.X - line.From.X, point.Y - line.From.Y);

        // Calculate the projection of pointVector onto edgeVector
        float t = (pointVector.X * edgeVector.X + pointVector.Y * edgeVector.Y) / (edgeVector.X * edgeVector.X + edgeVector.Y * edgeVector.Y);

        // Clamp t to the range [0, 1] to ensure the closest point is on the edge
        t = Math.Max(0, Math.Min(1, t));

        // Calculate the closest point on the edge
        float closestX = line.From.X + t * edgeVector.X;
        float closestY = line.From.Y + t * edgeVector.Y;

        return new Vector2((float)closestX, (float)closestY);
    }

    public static float GetArea(this Triangle triangle)
    {
        return Math.Abs((triangle.A.X * (triangle.B.Y - triangle.C.Y) +
                         triangle.B.X * (triangle.C.Y - triangle.A.Y) +
                         triangle.C.X * (triangle.A.Y - triangle.B.Y)) * .5f);
    }

    public static Circle ToCircumCircle(this Triangle triangle)
    {
        Vector2 midAB = (triangle.A + triangle.B) / 2;
        Vector2 midBC = (triangle.B + triangle.C) / 2;

        float slopeAB = (triangle.B.Y - triangle.A.Y) / (triangle.B.X - triangle.A.X);
        float slopeBC = (triangle.C.Y - triangle.B.Y) / (triangle.C.X - triangle.B.X);

        Vector2 center;
        if (Math.Abs(slopeAB - slopeBC) > float.Epsilon)
        {
            float x = (slopeAB * slopeBC * (triangle.A.Y - triangle.C.Y) + slopeBC * (triangle.A.X + triangle.B.X) - slopeAB * (triangle.B.X + triangle.C.X)) / (2 * (slopeBC - slopeAB));
            float y = -(x - (triangle.A.X + triangle.B.X) / 2) / slopeAB + (triangle.A.Y + triangle.B.Y) / 2;
            center = new Vector2((float)x, (float)y);
        }
        else
            center = (midAB + midBC) * .5f;

        return new(center, Vector2.Distance(center, triangle.A));
    }

    public static Triangle ToSuperTriangle(IList<Vector2> vertices)
    {
        float minX = float.MaxValue, minY = float.MaxValue;
        float maxX = float.MinValue, maxY = float.MinValue;

        foreach (Vector2 point in vertices)
        {
            minX = Math.Min(minX, point.X);
            minY = Math.Min(minY, point.Y);
            maxX = Math.Max(maxX, point.X);
            maxY = Math.Max(maxY, point.Y);
        }

        float dx = maxX - minX;
        float dy = maxY - minY;
        float deltaMax = Math.Max(dx, dy);
        float midX = (minX + maxX) / 2;
        float midY = (minY + maxY) / 2;

        Vector2 p1 = new Vector2((float)midX - 20f * (float)deltaMax, (float)midY - (float)deltaMax);
        Vector2 p2 = new Vector2((float)midX, (float)midY + 20 * (float)deltaMax);
        Vector2 p3 = new Vector2((float)midX + 20 * (float)deltaMax, (float)midY - (float)deltaMax);

        return new Triangle(p1, p2, p3);
    }

    public static IList<Line> ToLines(this IList<Vector2> vertices)
    {
        List<Line> lines = new List<Line>(vertices.Count - 1);
        ToLines(vertices, lines);
        return lines;
    }

    public static void ToLines(this IList<Vector2> vertices, IList<Line> lines)
    {
        lines.Clear();
        for (int i = 0; i < vertices.Count - 1; i++)
            lines.Add(new(vertices[i], vertices[i + 1]));
        lines.Add(new(vertices[^1], vertices[0]));
    }

    public static bool ExistIn(Line lineToCheck, List<Vector2> vertices)
    {
        for (int i = 0; i < vertices.Count - 1; i++)
        {
            Vector2 vertexCurrent = vertices[i];
            Vector2 vertexNext = vertices[i];
            if (lineToCheck.From == vertexCurrent && lineToCheck.To == vertexNext) return true;
            if (lineToCheck.From == vertexNext && lineToCheck.To == vertexCurrent) return true;
        }

        Vector2 vertexFirst = vertices[0];
        Vector2 vertexLast = vertices[^1];
        if (lineToCheck.From == vertexFirst && lineToCheck.To == vertexLast) return true;
        if (lineToCheck.From == vertexLast && lineToCheck.To == vertexFirst) return true;
        return false;
    }

    public static bool LaysOn(this Vector2 point, Line line)
        => ApproximatelyEqualEpsilon(line.Resolve(point.X), point, float.Epsilon);

    public static LineEquation ToLineEquation(this Line line)
    {
        Vector2 slopeVector = line.To - line.From;
        float slope = slopeVector.Y / slopeVector.X;

        float yOffset = line.From.Y - (slope * line.From.X);

        return new LineEquation(slope, yOffset);
    }

    // Given three collinear points p, q, r, the function checks if 
    // point q lies on line segment 'pr' 
    public static bool OnSegment(Vector2 p, Vector2 q, Vector2 r)
    {
        if (q.X <= Math.Max(p.X, r.X) && q.X >= Math.Min(p.X, r.X) &&
            q.Y <= Math.Max(p.Y, r.Y) && q.Y >= Math.Min(p.Y, r.Y))
            return true;

        return false;
    }

    // To find orientation of ordered triplet (p, q, r). 
    // The function returns following values 
    // 0 --> p, q and r are collinear 
    // 1 --> Clockwise 
    // 2 --> Counterclockwise 
    public static int Orientation(Vector2 p, Vector2 q, Vector2 r)
    {
        // See https://www.geeksforgeeks.org/orientation-3-ordered-points/ 
        // for details of below formula. 
        float val = (q.Y - p.Y) * (r.X - q.X) -
                (q.X - p.X) * (r.Y - q.Y);

        if (val == 0) return 0; // collinear 

        return (val > 0) ? 1 : 2; // clock or counterclock wise 
    }

    public static float IntersectionParameterT(Vector2 p0, Vector2 p1, Vector2 q0, Vector2 q1)
        => ((q0.X - p0.X) * (p1.Y - p0.Y) - (q0.Y - p0.Y) * (p1.X - p0.X)) /
            ((q1.Y - q0.Y) * (p1.X - p0.X) - (q1.X - q0.X) * (p1.Y - p0.Y));

    public static float IntersectionParameterT(this Line l0, Line l1)
        => ((l1.From.X - l0.From.X) * (l0.To.Y - l0.From.Y) - (l1.From.Y - l0.From.Y) * (l0.To.X - l0.From.X)) /
            ((l1.To.Y - l1.From.Y) * (l0.To.X - l0.From.X) - (l1.To.X - l1.From.X) * (l0.To.Y - l0.From.Y));

    public static float GetT(this Line line, Vector2 point)
    {
        //     if (!point.LaysOn(line))
        //         throw new Exception("Point does not lay on Line");

        float fromX = MathF.Abs(line.From.X);
        float toX = MathF.Abs(line.To.X);
        float pointX = MathF.Abs(point.X);

        float min = MathF.Min(fromX, toX);
        float max = MathF.Max(fromX, toX) - min;

        pointX -= min;

        return pointX / max;
    }

    public static Vector2 Resolve(this Line line, float x)
    {
        LineEquation lineEquation = line.ToLineEquation();

        // y = mx + b
        float y = lineEquation.Slope * x + lineEquation.OffsetY;
        return new Vector2(x, y);
    }

    public static Vector2 IntersectionPoint(this Line l1, Line l2)
        => Vector2.Lerp(l1.From, l1.To, IntersectionParameterT(l1, l2));

    public static bool Intersects(this Line l1, Line l2)
    {
        int o1 = Orientation(l1.From, l1.To, l2.From);
        int o2 = Orientation(l1.From, l1.To, l2.To);
        int o3 = Orientation(l2.From, l2.To, l1.From);
        int o4 = Orientation(l2.From, l2.To, l1.To);

        if (o1 != o2 && o3 != o4)
            return true;

        if (o1 == 0 && OnSegment(l1.From, l2.From, l1.To)) return true;
        if (o2 == 0 && OnSegment(l1.From, l2.To, l1.To)) return true;
        if (o3 == 0 && OnSegment(l2.From, l1.From, l2.To)) return true;
        if (o4 == 0 && OnSegment(l2.From, l1.To, l2.To)) return true;

        return false;
    }

    public static bool Intersects(this Line l1, Line l2, [NotNullWhen(returnValue: true)] out Vector2? point)
    {
        point = null;

        bool result = Intersects(l1, l2);

        if (result)
            point = IntersectionPoint(l1, l2);

        return result;
    }

    public static bool Intersects(this Circle circle, Circle circleOther)
    {
        float distanceSquared = (circle.Position - circleOther.Position).LengthSquared();
        float radiusSumSquared = circle.Radius * circle.Radius + circleOther.Radius * circleOther.Radius;

        return distanceSquared < radiusSumSquared;
    }

    public static bool Inside(this Triangle triangle, Vector2 point)
    {
        float originalTriangleArea = GetArea(triangle);

        float pointTriangleArea1 = GetArea(new Triangle(point, triangle.B, triangle.C));
        float pointTriangleArea2 = GetArea(new Triangle(triangle.A, point, triangle.C));
        float pointTriangleArea3 = GetArea(new Triangle(triangle.A, triangle.B, point));

        float pointTriangleAreasSum = pointTriangleArea1 + pointTriangleArea2 + pointTriangleArea3;

        return originalTriangleArea >= pointTriangleAreasSum;
    }

    public static bool Inside(this AABB aabb, Vector2 point)
        => point.X >= aabb.LowerBoundary.X && point.X <= aabb.UpperBoundary.X &&
            point.Y >= aabb.LowerBoundary.Y && point.Y <= aabb.UpperBoundary.Y;

    public static bool Inside(this Circle circle, Vector2 point)
        => (circle.Position - point).LengthSquared() <= circle.Radius * circle.Radius;

    public static bool ApproximatelyEqualEpsilon(float a, float b, float epsilon)
    {
        if (a == b)
            return true;

        const float floatNormal = (1 << 23) * float.Epsilon;
        float absA = Math.Abs(a);
        float absB = Math.Abs(b);
        float diff = Math.Abs(a - b);

        if (a == 0.0f || b == 0.0f || diff < floatNormal)
            return diff < (epsilon * floatNormal);

        return diff / Math.Min((absA + absB), float.MaxValue) < epsilon;
    }
    public static bool ApproximatelyEqualEpsilon(Vector2 a, Vector2 b, float epsilon)
    {
        return ApproximatelyEqualEpsilon(a.X, b.X, epsilon) && ApproximatelyEqualEpsilon(a.Y, b.Y, epsilon);
    }
}