From 2089ff6c7bf0dacbc337f6625134c494743ff5b2 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Tue, 16 Jun 2026 16:12:31 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20InterceptCalculator=20=E2=80=94=20?=
=?UTF-8?q?=E6=8A=9B=E7=89=A9=E7=BA=BF=E4=B8=8E=E7=9B=B4=E7=BA=BF=E8=81=94?=
=?UTF-8?q?=E7=AB=8B=E6=96=B9=E7=A8=8B=E6=B1=82=E8=A7=A3=E6=8B=A6=E6=88=AA?=
=?UTF-8?q?=E7=82=B9?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../Algorithms/InterceptCalculator.cs | 77 +++++++++++++++++++
.../InterceptCalculatorTests.cs | 76 ++++++++++++++++++
2 files changed, 153 insertions(+)
create mode 100644 src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
create mode 100644 test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
diff --git a/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs b/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
new file mode 100644
index 0000000..1afbb0a
--- /dev/null
+++ b/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
@@ -0,0 +1,77 @@
+using System;
+using System.Collections.Generic;
+using CounterDrone.Core.Models;
+
+namespace CounterDrone.Core.Algorithms
+{
+ /// 拦截点计算——炮弹抛物线与无人机直线的联立方程求解
+ public static class InterceptCalculator
+ {
+ /// 求解拦截弧长：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R
+ public static (float InterceptArc, float ShellTime) Compute(
+ IReadOnlyList 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);
+ }
+ }
+}
diff --git a/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs b/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
new file mode 100644
index 0000000..49054e3
--- /dev/null
+++ b/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 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 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 },
+ };
+ }
+}