cirnogodot/Scripts/Utils/MathFunctions.cs

using System;
using Godot;

namespace Cirno.Scripts.Utils;

public static class MathFunctions
{
    public static Vector2? PredictInterceptPosition(Vector2 shooterPos, Vector2 targetPos, Vector2 targetVel, float projectileSpeed)
    {
        Vector2 displacement = targetPos - shooterPos;
        float a = targetVel.LengthSquared() - projectileSpeed * projectileSpeed;
        float b = 2 * displacement.Dot(targetVel);
        float c = displacement.LengthSquared();

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

        if (discriminant < 0 || Mathf.Abs(a) < 0.001f)
            return null; // No solution or projectile too slow

        float sqrtDisc = Mathf.Sqrt(discriminant);
        float t1 = (-b - sqrtDisc) / (2 * a);
        float t2 = (-b + sqrtDisc) / (2 * a);

        float t = Mathf.Min(t1, t2);
        if (t < 0)
            t = Mathf.Max(t1, t2);

        if (t < 0)
            return null; // No valid positive time

        return targetPos + targetVel * t;
    }
    
    // Critically damped spring, based on Game Programming Gems 4 Chapter 1.10. https://archive.org/details/game-programming-gems-4/page/95/mode/2up
    // Returns a 2-tuple of [next_position, next_velocity].
    public static Tuple<float, float> SmoothDamp(float current, float target, float currentVelocity, float smoothTime, float maxSpeed, float delta)
    {
        smoothTime = MathF.Max(smoothTime, 0.0001f);
        var omega = 2.0f / smoothTime;

        var x = omega * delta;
        var xExp = 1.0f / (1.0f + x + 0.48f * x * x + 0.235f * x * x * x);
        var change = current - target;
        var originalTarget = target;
        
        // Clamp max speed
        var maxChange = maxSpeed * smoothTime;
        change = Math.Clamp(change, -maxChange, maxChange);
        target = current - change;
        
        var temp = (currentVelocity + omega * change) * delta;
        currentVelocity = (currentVelocity - omega * temp) * xExp;
        var output = target + (change + temp) * xExp;
        
        // Prevent Overshooting
        if ((originalTarget - current > 0.0) == (output > originalTarget))
        {
            output = originalTarget;
            currentVelocity = (output - originalTarget) / delta;
        }
        
        return new Tuple<float, float>(output, currentVelocity);
    }
    
    // --- Helpers ---
    private static float Cross(Vector2 a, Vector2 b) => a.X * b.Y - a.Y * b.X;

    // --- Point → infinite line ---
    public static float DistancePointToLine(Vector2 p, Vector2 a, Vector2 b)
    {
        Vector2 ab = b - a;
        float len = ab.Length();
        if (len == 0f)
            throw new ArgumentException("Line points must not be identical.", nameof(b));

        // |(b - a) x (p - a)| / |b - a|
        return Mathf.Abs(Cross(ab, p - a)) / len;
    }

    // --- Point → line segment ---
    public static float DistancePointToSegment(Vector2 p, Vector2 a, Vector2 b)
    {
        Vector2 ab = b - a;
        float abLenSq = ab.LengthSquared();

        // Degenerate segment → distance to the single endpoint
        if (abLenSq == 0f)
            return (p - a).Length();

        // Project p onto the line, normalize to [0,1], then clamp to segment
        float t = (p - a).Dot(ab) / abLenSq;
        t = Mathf.Clamp(t, 0f, 1f);

        Vector2 closest = a + t * ab;

        // Replacement for Vector2.Distance: length of the difference vector
        return (p - closest).Length();
    }
}