CounterDroneBackend/src/CounterDrone.Core/Algorithms/Kinematics.cs

using System;
using CounterDrone.Core.Models;

namespace CounterDrone.Core.Algorithms
{
    /// <summary>通用运动学 / 几何 / 大气扩散工具</summary>
    public static class Kinematics
    {
        /// <summary>Pasquill 稳定度等级</summary>
        public enum StabilityClass { A, B, C, D, E, F }
        /// <summary>风向 → 单位速度矢量 (X, 0, Z)，右手系 Y-up</summary>
        public static (float X, float Y, float Z) WindToVector(WindDirection dir, float speed)
        {
            var angle = dir switch
            {
                WindDirection.N => 0f,
                WindDirection.NE => 45f,
                WindDirection.E => 90f,
                WindDirection.SE => 135f,
                WindDirection.S => 180f,
                WindDirection.SW => 225f,
                WindDirection.W => 270f,
                WindDirection.NW => 315f,
                _ => 0f,
            };
            var rad = angle * (float)Math.PI / 180f;
            return ((float)Math.Sin(rad) * speed, 0, (float)Math.Cos(rad) * speed);
        }

        /// <summary>给定水平距离和初速，求解抛物线发射角（逆问题）</summary>
        /// <param name="range">水平距离 (m)，必须 > 0</param>
        /// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
        /// <param name="heightDiff">目标相对高度 targetY - startY (m)</param>
        /// <returns>发射角 (rad)，取低弹道解</returns>
        public static float CalculateLaunchAngle(float range, float muzzleVelocity, float heightDiff)
        {
            if (range <= 0)
                throw new ArgumentException($"range 必须 > 0，实际: {range}", nameof(range));
            if (muzzleVelocity <= 0)
                throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));

            float v2 = muzzleVelocity * muzzleVelocity;
            float g = 9.81f;
            // 轨迹方程: heightDiff = R·tanθ - gR²/(2v²cos²θ)
            // 令 T = tanθ, a = gR²/(2v²):  aT² - RT + (a + heightDiff) = 0
            float a = 0.5f * g * range * range / v2;
            float discriminant = range * range - 4f * a * (heightDiff + a);
            if (discriminant < 0)
                throw new ArgumentException(
                    $"当前参数无法命中目标: range={range}, heightDiff={heightDiff}, v₀={muzzleVelocity}");
            // 低弹道解（取较小 tanθ）
            float tanTheta = (range - (float)Math.Sqrt(discriminant)) / (2f * a);
            return (float)Math.Atan(tanTheta);
        }

        /// <summary>弹道飞行时间（秒）。水平距离 / 水平分速</summary>
        public static float ParabolicTimeOfFlight(float range, float launchAngle, float muzzleVelocity)
        {
            if (range <= 0)
                throw new ArgumentException($"range 必须 > 0，实际: {range}", nameof(range));
            if (muzzleVelocity <= 0)
                throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));

            float cosAngle = (float)Math.Cos(launchAngle);
            return range / (muzzleVelocity * cosAngle);
        }

        /// <summary>抛物线飞行时间：给定水平距离和目标高度差，计算弹道时间</summary>
        /// <remarks>等价于先调用 CalculateLaunchAngle 再调用 ParabolicTimeOfFlight</remarks>
        public static float ParabolicShellTime(float horizontalDist, float heightDiff, float muzzleVelocity)
        {
            float angle = CalculateLaunchAngle(horizontalDist, muzzleVelocity, heightDiff);
            return ParabolicTimeOfFlight(horizontalDist, angle, muzzleVelocity);
        }

        /// <summary>给定初速和发射角，计算到达指定相对高度的水平射程和飞行时间（正问题）</summary>
        /// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
        /// <param name="launchAngle">发射角 (rad)，必须 ∈ (0, π/2)</param>
        /// <param name="heightDiff">目标相对高度 targetY - startY (m)</param>
        /// <returns>(水平射程 m, 飞行时间 s)，取下行段解</returns>
        public static (float range, float timeOfFlight) ComputeParabolicRange(
            float muzzleVelocity, float launchAngle, float heightDiff)
        {
            if (muzzleVelocity <= 0)
                throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));
            if (launchAngle <= -Math.PI / 2 || launchAngle >= Math.PI / 2)
                throw new ArgumentException($"launchAngle 必须在 (-π/2, π/2) 内，实际: {launchAngle}", nameof(launchAngle));

            float g = 9.81f;
            float cosA = (float)Math.Cos(launchAngle);
            float tanA = (float)Math.Tan(launchAngle);

            // 轨迹方程: heightDiff = R·tanθ - gR²/(2v²cos²θ)
            // 整理: (g/(2v²cos²θ))·R² - tanθ·R + heightDiff = 0
            float a = g / (2f * muzzleVelocity * muzzleVelocity * cosA * cosA);
            float disc = tanA * tanA - 4f * a * heightDiff;

            if (disc < 0)
                throw new ArgumentException(
                    $"当前参数无法达到目标高度: heightDiff={heightDiff}, v₀={muzzleVelocity}, θ={launchAngle * 180 / Math.PI:F1}°");

            // 下行段解（较大根），+√disc 给出弹道下降分支与目标高度的交点
            float range = (tanA + (float)Math.Sqrt(disc)) / (2f * a);
            float timeOfFlight = range / (muzzleVelocity * cosA);

            return (range, timeOfFlight);
        }

        /// <summary>计算弹道顶点（最大高度和到达时间），相对发射点</summary>
        /// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
        /// <param name="launchAngle">发射角 (rad)，必须 ∈ [0, π/2)</param>
        /// <returns>(顶点高度 m, 到达顶点时间 s)，高度是相对发射点的增量</returns>
        public static (float apexHeight, float apexTime) ParabolicApex(
            float muzzleVelocity, float launchAngle)
        {
            if (muzzleVelocity <= 0)
                throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));
            if (launchAngle <= -Math.PI / 2 || launchAngle >= Math.PI / 2)
                throw new ArgumentException($"launchAngle 必须在 (-π/2, π/2) 内，实际: {launchAngle}", nameof(launchAngle));

            float g = 9.81f;
            float sinA = (float)Math.Sin(launchAngle);
            if (sinA <= 0) return (0, 0); // 水平或俯射，无上升顶点
            float vy = muzzleVelocity * sinA;
            float apexTime = vy / g;
            float apexHeight = vy * vy / (2f * g);
            return (apexHeight, apexTime);
        }

        /// <summary>给定初速、发射角和方位角，计算任意时刻的抛物线位置（正问题核心）</summary>
        /// <param name="launchAngle">发射角 (rad)，水平面以上为正</param>
        /// <param name="azimuth">方位角 (rad)，0=N(+Z), π/2=E(+X)</param>
        public static (float X, float Y, float Z) ParabolicPosition(
            float startX, float startY, float startZ,
            float launchAngle, float azimuth,
            float muzzleVelocity, float time)
        {
            var cosA = (float)Math.Cos(launchAngle);
            var sinA = (float)Math.Sin(launchAngle);
            var g = 9.81f;

            var horizontalDist = muzzleVelocity * cosA * time;
            var x = startX + horizontalDist * (float)Math.Sin(azimuth);
            var z = startZ + horizontalDist * (float)Math.Cos(azimuth);
            var y = startY + muzzleVelocity * sinA * time - 0.5f * g * time * time;

            return (x, y, z);
        }

        /// <summary>空投弹药位置：继承载机速度 + 重力下落</summary>
        /// <param name="startX,startY,startZ">投放点坐标</param>
        /// <param name="carrierVelX,carrierVelY,carrierVelZ">载机速度矢量 (m/s)</param>
        /// <param name="time">经过时间 (s)</param>
        public static (float X, float Y, float Z) AirDropPosition(
            float startX, float startY, float startZ,
            float carrierVelX, float carrierVelY, float carrierVelZ,
            float time)
        {
            const float g = 9.81f;
            var x = startX + carrierVelX * time;
            var y = startY + carrierVelY * time - 0.5f * g * time * time;
            var z = startZ + carrierVelZ * time;
            return (x, y, z);
        }

        /// <summary>空投降落时间：从释放高度下落到目标高度所需时间</summary>
        /// <param name="releaseAlt">释放高度 (m)</param>
        /// <param name="targetAlt">目标高度 (m)</param>
        /// <param name="carrierVelY">载机垂直速度 (m/s)，正=向上，默认 0（平飞）</param>
        public static float AirDropFallTime(float releaseAlt, float targetAlt, float carrierVelY = 0f)
        {
            const float g = 9.81f;
            var dy = releaseAlt - targetAlt;
            if (dy <= 0) return 0f;
            return ((float)Math.Sqrt(carrierVelY * carrierVelY + 2f * g * dy) - carrierVelY) / g;
        }

        /// <summary>两点间方向的速度矢量：normalize(to - from) × speed</summary>
        public static (float X, float Y, float Z) DirectionVelocity(
            float fromX, float fromY, float fromZ,
            float toX, float toY, float toZ,
            float speed)
        {
            float dx = toX - fromX;
            float dy = toY - fromY;
            float dz = toZ - fromZ;
            float dist = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
            if (dist < 0.01f) return (0, 0, 0);
            float s = speed / dist;
            return (dx * s, dy * s, dz * s);
        }

        /// <summary>云团覆盖间隔：无人机穿越单个云团的时间 = 云团直径 / 无人机速度</summary>
        /// <param name="cloudDiameter">云团有效直径 m</param>
        /// <param name="targetSpeedKmh">目标速度 km/h</param>
        /// <param name="minInterval">硬件最小间隔 s</param>
        public static float CloudCoverInterval(float cloudDiameter, float targetSpeedKmh, float minInterval = 0.1f)
        {
            float speedMs = targetSpeedKmh / 3.6f;
            if (speedMs <= 0.1f) return minInterval;
            return Math.Max(minInterval, cloudDiameter / speedMs);
        }

        /// <summary>点到矩形距离（简化判定）</summary>
        public static float Distance2D(float x1, float z1, float x2, float z2)
        {
            var dx = x2 - x1;
            var dz = z2 - z1;
            return (float)Math.Sqrt(dx * dx + dz * dz);
        }

        /// <summary>匀速直线运动从一点到另一点的飞行时间（秒）。
        /// 物理模型：无人机/平台沿直线匀速飞行，时间 = 距离 / 速度。
        /// speedKmh 为 0 时抛异常（速度必须 > 0，由调用方保证）。</summary>
        /// <param name="speedKmh">速度 km/h（与 TypicalSpeed 单位一致）</param>
        public static float TravelTime(float fromX, float fromZ, float toX, float toZ, float speedKmh)
        {
            if (speedKmh <= 0)
                throw new ArgumentException("速度必须 > 0", nameof(speedKmh));
            float dist = Distance2D(fromX, fromZ, toX, toZ);
            return dist / (speedKmh / 3.6f);
        }

        /// <summary>点到三维点距离</summary>
        public static float Distance3D(float x1, float y1, float z1, float x2, float y2, float z2)
        {
            var dx = x2 - x1;
            var dy = y2 - y1;
            var dz = z2 - z1;
            return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
        }

        /// <summary>根据天气+风速确定 Pasquill 稳定度</summary>
        public static StabilityClass GetStabilityClass(WeatherType weather, float windSpeed)
        {
            return weather switch
            {
                WeatherType.Sunny when windSpeed <= 5 => StabilityClass.A,
                WeatherType.Sunny => StabilityClass.B,
                WeatherType.Overcast => StabilityClass.D,
                WeatherType.Rain => StabilityClass.D,
                WeatherType.Fog => StabilityClass.E,
                WeatherType.Night => StabilityClass.F,
                _ => StabilityClass.D,
            };
        }

        /// <summary>Pasquill-Gifford 水平扩散系数 σy(m)，x 为扩散距离(m)</summary>
        public static float SigmaY(StabilityClass cls, float x)
        {
            var coeff = cls switch
            {
                StabilityClass.A => 0.22f, StabilityClass.B => 0.16f,
                StabilityClass.C => 0.11f, StabilityClass.D => 0.08f,
                StabilityClass.E => 0.06f, StabilityClass.F => 0.04f,
                _ => 0.08f,
            };
            return coeff * x / (float)Math.Sqrt(1f + 0.0001f * x);
        }

        /// <summary>Pasquill-Gifford 垂直扩散系数 σz(m)，x 为扩散距离(m)</summary>
        public static float SigmaZ(StabilityClass cls, float x)
        {
            return cls switch
            {
                StabilityClass.A => 0.20f * x,
                StabilityClass.B => 0.12f * x,
                StabilityClass.C => 0.08f * x / (float)Math.Sqrt(1f + 0.0002f * x),
                StabilityClass.D => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
                StabilityClass.E => 0.03f * x / (float)Math.Sqrt(1f + 0.0003f * x),
                StabilityClass.F => 0.016f * x / (float)Math.Sqrt(1f + 0.0003f * x),
                _ => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
            };
        }

        /// <summary>高斯烟团中心浓度 C_max = Q / [(2π)^(3/2) σy² σz]</summary>
        public static float GaussianPeakConcentration(float sourceStrength, float sigmaY, float sigmaZ)
        {
            var denom = (float)Math.Pow(2f * (float)Math.PI, 1.5f) * sigmaY * sigmaY * sigmaZ;
            return denom > 0.001f ? sourceStrength / denom : 0f;
        }
        public static bool PointInPolygon(float px, float pz, ReadOnlySpan<(float X, float Z)> vertices)
        {
            if (vertices.Length < 3) return false;
            bool inside = false;
            int j = vertices.Length - 1;
            for (int i = 0; i < vertices.Length; i++)
            {
                var xi = vertices[i].X; var zi = vertices[i].Z;
                var xj = vertices[j].X; var zj = vertices[j].Z;
                if ((zi > pz) != (zj > pz) && px < (xj - xi) * (pz - zi) / (zj - zi) + xi)
                    inside = !inside;
                j = i;
            }
            return inside;
        }
    }
}