cirnogodot/Scripts/Utils/MathFunctions.cs

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;
    }
}