From 3fd8433276ce61191e316c14ef8e76c0e4cc44f1 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Wed, 17 Jun 2026 08:13:20 +0800
Subject: [PATCH] =?UTF-8?q?feat:=20InterceptCalculator=20=E8=BF=94?=
=?UTF-8?q?=E5=9B=9E=E5=8F=91=E5=B0=84=E8=A7=92;=20FireEvent=20+LaunchAngl?=
=?UTF-8?q?e=20=E5=AD=97=E6=AE=B5;=20=E9=87=8D=E6=96=B0=E9=9B=86=E6=88=90?=
=?UTF-8?q?=E5=88=B0=20planner?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
.../Algorithms/AlgorithmTypes.cs | 1 +
.../Algorithms/DefensePlanner.cs | 16 +++++-
.../Algorithms/InterceptCalculator.cs | 53 +++++++++++--------
.../InterceptCalculatorAirTests.cs | 4 +-
.../InterceptCalculatorTests.cs | 8 +--
5 files changed, 52 insertions(+), 30 deletions(-)
diff --git a/src/CounterDrone.Core/Algorithms/AlgorithmTypes.cs b/src/CounterDrone.Core/Algorithms/AlgorithmTypes.cs
index 518d959..3585f1b 100644
--- a/src/CounterDrone.Core/Algorithms/AlgorithmTypes.cs
+++ b/src/CounterDrone.Core/Algorithms/AlgorithmTypes.cs
@@ -107,6 +107,7 @@ namespace CounterDrone.Core.Algorithms
public int PlatformIndex;
public float TargetX, TargetY, TargetZ;
public float MuzzleVelocity;
+ public float LaunchAngle;
}
// ═══════════════════════════════════════════════
diff --git a/src/CounterDrone.Core/Algorithms/DefensePlanner.cs b/src/CounterDrone.Core/Algorithms/DefensePlanner.cs
index 5456b75..928ee6b 100644
--- a/src/CounterDrone.Core/Algorithms/DefensePlanner.cs
+++ b/src/CounterDrone.Core/Algorithms/DefensePlanner.cs
@@ -366,7 +366,13 @@ namespace CounterDrone.Core.Algorithms
if (dist > maxRange) return null;
float shellTime = dist / unit.MuzzleVelocity;
- if (!HasInterceptWindow(threat, midArc, expansionTime, shellTime)) return null;
+
+ // 用 InterceptCalculator 验证可行性 + 获取发射角
+ float speedKph = GetDroneSpeedKph(threat);
+ var (interceptArc, _, launchAngle) = InterceptCalculator.Compute(
+ threat.Waypoints, threat.DetectArc, speedKph,
+ _config.ReactionTime + expansionTime, unit.Position, unit.MuzzleVelocity);
+ if (interceptArc <= 0) return null;
float interceptTime = threat.ArrivalTime;
@@ -402,6 +408,14 @@ namespace CounterDrone.Core.Algorithms
if (deliveryTime > interceptTime) return null;
if (!HasInterceptWindow(threat, midArc, expansionTime, deliveryTime)) return null;
+ // 空基也用 InterceptCalculator 验证
+ float speedKph = GetDroneSpeedKph(threat);
+ var (icArc, _, _) = InterceptCalculator.ComputeHorizontal(
+ threat.Waypoints, threat.DetectArc, speedKph,
+ _config.ReactionTime + expansionTime, unit.Position, unit.CruiseSpeed,
+ unit.ReleaseAltitude, (float)threat.Target.TypicalAltitude);
+ if (icArc <= 0) return null;
+
float avgSpeed = GetDroneSpeedMs(threat);
float neededExposure = _damageModel.RequiredExposureSeconds((TargetType)threat.Target.TargetType, (PowerType)threat.Target.PowerType, ammoType);
float actualExposure = effectiveR * 2f / avgSpeed;
diff --git a/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs b/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
index ed8581b..76a63b6 100644
--- a/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
+++ b/src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
@@ -9,16 +9,16 @@ namespace CounterDrone.Core.Algorithms
{
/// 空基拦截：平抛(θ=0°)，下落时间固定。
/// 方程: (A-d)/vd = R + tf + |A - vp*tf - ux|/vp, tf = √(2Δy/g)
- public static (float InterceptArc, float FlightTime) ComputeHorizontal(
+ public static (float InterceptArc, float FlightTime, float LaunchAngle) ComputeHorizontal(
IReadOnlyList route, float detectArc, float droneSpeedKmh,
float reactionTime, Vector3 platformPos, float cruiseSpeed,
float releaseAlt, float targetAlt)
{
if (route == null || route.Count < 2 || droneSpeedKmh <= 0 || cruiseSpeed <= 0)
- return (0, 0);
+ return (0, 0, 0);
float totalArc = RouteGeometry.TotalLength(route);
- if (detectArc >= totalArc) return (0, 0);
+ if (detectArc >= totalArc) return (0, 0, 0);
float vd = droneSpeedKmh / 3.6f;
float tf = releaseAlt > targetAlt ? MathF.Sqrt(2f * (releaseAlt - targetAlt) / 9.81f) : 0;
@@ -28,43 +28,42 @@ namespace CounterDrone.Core.Algorithms
float d = detectArc;
float vp_tf = vp * tf;
- // 情况1: A >= ux + vp*tf（拦截点在释放点前方，平台向前飞）
if (vp > vd)
{
float A1 = (delay - tf - ux / vp) * vd * vp / (vp - vd);
if (A1 >= ux + vp_tf && A1 > d && A1 <= totalArc)
- return (A1, (A1 - d) / vd - delay);
+ return (A1, (A1 - d) / vd - delay, 0);
}
- // 情况2: A < ux + vp*tf（拦截点在释放点后方，平台向后飞）
float A2 = (delay + ux / vp + tf) * vd * vp / (vp + vd);
if (A2 < ux + vp_tf && A2 > d && A2 <= totalArc)
- return (A2, (A2 - d) / vd - delay);
+ return (A2, (A2 - d) / vd - delay, 0);
- return (0, 0);
+ return (0, 0, 0);
}
- /// 求解拦截弧长：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R
- public static (float InterceptArc, float ShellTime) Compute(
+ /// 地基拦截：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R
+ public static (float InterceptArc, float ShellTime, float LaunchAngle) 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);
+ return (0, 0, 0);
float totalArc = RouteGeometry.TotalLength(route);
- if (detectArc >= totalArc) return (0, 0);
+ if (detectArc >= totalArc) return (0, 0, 0);
float vd = droneSpeedKmh / 3.6f;
float vs2 = muzzleVelocity * muzzleVelocity;
float g = 9.81f;
float uy = fireUnitPos.Y;
+ IReadOnlyList r = route;
float F(float a)
{
float ts = (a - detectArc) / vd - reactionTime;
if (ts <= 0) return float.MaxValue;
- var (px, py, pz) = RouteGeometry.PositionAt(route, a);
+ var (px, py, pz) = RouteGeometry.PositionAt(r, a);
float dx = px - fireUnitPos.X, dz = pz - fireUnitPos.Z;
float D2 = dx * dx + dz * dz;
float dy = py - uy;
@@ -72,14 +71,22 @@ namespace CounterDrone.Core.Algorithms
return D2 + term * term - vs2 * ts * ts;
}
- // 从探测点向后搜索，找 F 变负的点
+ static float Angle(Vector3 unit, float arc, float ts, float vs, IReadOnlyList wps)
+ {
+ var (px, py, pz) = RouteGeometry.PositionAt(wps, arc);
+ float D = MathF.Sqrt((px - unit.X) * (px - unit.X) + (pz - unit.Z) * (pz - unit.Z));
+ float dy = py - unit.Y;
+ float sinA = (dy + 0.5f * 9.81f * ts * ts) / (vs * ts);
+ float cosA = D / (vs * ts);
+ return MathF.Atan2(sinA, cosA);
+ }
+
float lo = detectArc + Math.Max(0.001f, vd * reactionTime + 0.001f);
- if (lo >= totalArc) return (0, 0);
+ if (lo >= totalArc) return (0, 0, 0);
float fLo = F(lo);
- if (fLo >= float.MaxValue - 1) return (0, 0);
+ if (fLo >= float.MaxValue - 1) return (0, 0, 0);
- // 步进搜索下降段
float step = (totalArc - lo) / 100f;
if (step < 0.001f) step = 0.001f;
float hi = lo + step;
@@ -91,10 +98,9 @@ namespace CounterDrone.Core.Algorithms
lo = hi; fLo = fHi;
hi += step;
}
- if (hi > totalArc || fHi >= float.MaxValue - 1) return (0, 0);
- if (fHi > 0 && fLo > 0) return (0, 0); // 同号无解
+ if (hi > totalArc || fHi >= float.MaxValue - 1) return (0, 0, 0);
+ if (fHi > 0 && fLo > 0) return (0, 0, 0);
- // 二分精确求解
for (int i = 0; i < 30; i++)
{
float mid = (lo + hi) / 2f;
@@ -102,14 +108,15 @@ namespace CounterDrone.Core.Algorithms
if (MathF.Abs(fMid) < 0.001f || hi - lo < 0.001f)
{
float ts = (mid - detectArc) / vd - reactionTime;
- return (mid, ts);
+ return (mid, ts, Angle(fireUnitPos, mid, ts, muzzleVelocity, r));
}
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);
+ float final = (lo + hi) / 2f;
+ float finalTs = (final - detectArc) / vd - reactionTime;
+ return (final, finalTs, Angle(fireUnitPos, final, finalTs, muzzleVelocity, r));
}
}
}
diff --git a/test/unit/CounterDrone.Core.Tests/InterceptCalculatorAirTests.cs b/test/unit/CounterDrone.Core.Tests/InterceptCalculatorAirTests.cs
index d84bc58..2794978 100644
--- a/test/unit/CounterDrone.Core.Tests/InterceptCalculatorAirTests.cs
+++ b/test/unit/CounterDrone.Core.Tests/InterceptCalculatorAirTests.cs
@@ -20,7 +20,7 @@ namespace CounterDrone.Core.Tests
// 平台(2000,1000,0), cruise=80, R+exp=35
var route = R(0, 20000, 500, 200);
var unit = new Vector3(2000, 1000, 0);
- var (arc, _) = InterceptCalculator.ComputeHorizontal(
+ var (arc, _, _) = InterceptCalculator.ComputeHorizontal(
route, 0, 200, 35, unit, 80, 1000, 500);
Assert.True(arc > 0, $"arc={arc:F0}");
@@ -32,7 +32,7 @@ namespace CounterDrone.Core.Tests
// 手算验证时间一致性
var route = R(0, 20000, 500, 200);
var unit = new Vector3(2000, 1000, 0);
- var (arc, _) = InterceptCalculator.ComputeHorizontal(
+ var (arc, _, _) = InterceptCalculator.ComputeHorizontal(
route, 0, 200, 35, unit, 80, 1000, 500);
if (arc > 0)
diff --git a/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs b/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
index 49054e3..966808f 100644
--- a/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
+++ b/test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
@@ -21,7 +21,7 @@ namespace CounterDrone.Core.Tests
// 近似解 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);
+ var (arc, _, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 100);
Assert.True(arc > 0.05f && arc < 0.15f, $"arc={arc:F4}");
}
@@ -32,7 +32,7 @@ namespace CounterDrone.Core.Tests
// 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);
+ var (arc, _, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 2, unit, 100);
Assert.True(arc > 1.5f && arc < 3f, $"arc={arc:F3}");
}
@@ -43,7 +43,7 @@ namespace CounterDrone.Core.Tests
// 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);
+ var (arc, _, _) = InterceptCalculator.Compute(route, 0, 1 * 3.6f, 0, unit, 50);
Assert.True(arc > 0.3f && arc < 1.5f, $"arc={arc:F4}");
}
@@ -55,7 +55,7 @@ namespace CounterDrone.Core.Tests
// 核实求出的 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);
+ 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);