feat: InterceptCalculator — 抛物线与直线联立方程求解拦截点

src/CounterDrone.Core/Algorithms/InterceptCalculator.cs

@@ -0,0 +1,77 @@
+using System;
+using System.Collections.Generic;
+using CounterDrone.Core.Models;
+
+namespace CounterDrone.Core.Algorithms
+{
+    /// <summary>拦截点计算——炮弹抛物线与无人机直线的联立方程求解</summary>
+    public static class InterceptCalculator
+    {
+        /// <summary>求解拦截弧长：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R</summary>
+        public static (float InterceptArc, float ShellTime) Compute(
+            IReadOnlyList<Waypoint> route, float detectArc, float droneSpeedKmh,
+            float reactionTime, Vector3 fireUnitPos, float muzzleVelocity)
+        {
+            if (route == null || route.Count < 2 || droneSpeedKmh <= 0 || muzzleVelocity <= 0)
+                return (0, 0);
+
+            float totalArc = RouteGeometry.TotalLength(route);
+            if (detectArc >= totalArc) return (0, 0);
+
+            float vd = droneSpeedKmh / 3.6f;
+            float vs2 = muzzleVelocity * muzzleVelocity;
+            float g = 9.81f;
+            float uy = fireUnitPos.Y;
+
+            float F(float a)
+            {
+                float ts = (a - detectArc) / vd - reactionTime;
+                if (ts <= 0) return float.MaxValue;
+                var (px, py, pz) = RouteGeometry.PositionAt(route, a);
+                float dx = px - fireUnitPos.X, dz = pz - fireUnitPos.Z;
+                float D2 = dx * dx + dz * dz;
+                float dy = py - uy;
+                float term = dy + 0.5f * g * ts * ts;
+                return D2 + term * term - vs2 * ts * ts;
+            }
+
+            // 从探测点向后搜索，找 F 变负的点
+            float lo = detectArc + Math.Max(0.001f, vd * reactionTime + 0.001f);
+            if (lo >= totalArc) return (0, 0);
+
+            float fLo = F(lo);
+            if (fLo >= float.MaxValue - 1) return (0, 0);
+
+            // 步进搜索下降段
+            float step = Math.Max(1f, (totalArc - lo) / 100f);
+            float hi = lo + step;
+            float fHi = 0;
+            while (hi <= totalArc)
+            {
+                fHi = F(hi);
+                if (fHi >= float.MaxValue - 1 || fHi <= 0) break;
+                lo = hi; fLo = fHi;
+                hi += step;
+            }
+            if (hi > totalArc || fHi >= float.MaxValue - 1) return (0, 0);
+            if (fHi > 0 && fLo > 0) return (0, 0); // 同号无解
+
+            // 二分精确求解
+            for (int i = 0; i < 30; i++)
+            {
+                float mid = (lo + hi) / 2f;
+                float fMid = F(mid);
+                if (MathF.Abs(fMid) < 0.001f || hi - lo < 0.001f)
+                {
+                    float ts = (mid - detectArc) / vd - reactionTime;
+                    return (mid, ts);
+                }
+                if (fLo <= 0 && fMid >= 0 || fLo >= 0 && fMid <= 0) { hi = mid; fHi = fMid; }
+                else { lo = mid; fLo = fMid; }
+            }
+
+            float finalTs = ((lo + hi) / 2f - detectArc) / vd - reactionTime;
+            return ((lo + hi) / 2f, finalTs);
+        }
+    }
+}

test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs

@@ -0,0 +1,76 @@
+using System.Collections.Generic;
+using CounterDrone.Core.Algorithms;
+using CounterDrone.Core.Models;
+using Xunit;
+
+namespace CounterDrone.Core.Tests
+{
+    public class InterceptCalculatorTests
+    {
+        private static List<Waypoint> FlatRoute(float length, float speedMs)
+            => new() {
+                new() { PosX = 0, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
+                new() { PosX = length, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
+            };
+
+        [Fact]
+        public void NoHeightDiff_vs100_vd1()
+        {
+            // 航路 0→20, vd=1, unit@(10,0,0), vs=100, R=0
+            // (10-A)² + 0.25·g²·A⁴ = 10000·A²
+            // 近似解 A≈0.099 (重力项可忽略)
+            var route = FlatRoute(20, 1);
+            var unit = new Vector3(10, 0, 0);
+            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 100);
+
+            Assert.True(arc > 0.05f && arc < 0.15f, $"arc={arc:F4}");
+        }
+
+        [Fact]
+        public void NoHeightDiff_vs100_vd1_R2()
+        {
+            // R=2s, ts=A-2, 方程: (10-A)²+0.25g²(A-2)⁴ = 10000(A-2)²
+            var route = FlatRoute(20, 1);
+            var unit = new Vector3(10, 0, 0);
+            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 2, unit, 100);
+
+            Assert.True(arc > 1.5f && arc < 3f, $"arc={arc:F3}");
+        }
+
+        [Fact]
+        public void NoHeightDiff_vs50_vd1_UnitFar()
+        {
+            // unit@(50,0,0), vs=50, vd=1, R=0, route 0→100
+            var route = FlatRoute(100, 1);
+            var unit = new Vector3(50, 0, 0);
+            var (arc, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 50);
+
+            Assert.True(arc > 0.3f && arc < 1.5f, $"arc={arc:F4}");
+        }
+
+        [Fact]
+        public void UpwardBranch_ShellGoingUp()
+        {
+            // Y=500, unit Y=0, vs=800, vd=55.6(200km/h), R=5, route 0→10000
+            // 核实求出的 ts 对应上升段
+            var route = StraightRoute(0, 10000, 500, 200);
+            var unit = new Vector3(5000, 0, 50);
+            var (arc, ts) = InterceptCalculator.Compute(route, 0, 200, 5, unit, 800);
+
+            Assert.True(arc > 0, $"arc={arc:F0}");
+            var (rx, ry, rz) = RouteGeometry.PositionAt(route, arc);
+            float d = MathF.Sqrt((rx - 5000) * (rx - 5000) + (rz - 50) * (rz - 50));
+            float cosTheta = d / (800 * ts);
+            float sinTheta = (ry - 0 + 0.5f * 9.81f * ts * ts) / (800 * ts);
+            // 上升段: vy > 0，即 vs*sinθ - g*ts > 0
+            float vy = 800 * sinTheta - 9.81f * ts;
+            Assert.True(vy > 0, $"vy={vy:F1} should be >0 (upward branch), θ={MathF.Atan2(sinTheta, cosTheta)*180/MathF.PI:F1}°");
+        }
+
+        private static List<Waypoint> StraightRoute(float x0, float x1, float alt, float speed)
+            => new() {
+                new() { PosX = x0, PosY = alt, PosZ = 0, Speed = speed },
+                new() { PosX = x1, PosY = alt, PosZ = 0, Speed = speed },
+            };
+    }
+}