tian/CounterDroneBackend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: InterceptCalculator 返回发射角; FireEvent +LaunchAngle 字段; 重新集成到 planner

Browse Source
This commit is contained in:
tian 2026-06-17 08:13:20 +08:00
parent 663afbe639
commit 3fd8433276
5 changed files with 52 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/CounterDrone.Core/Algorithms/AlgorithmTypes.cs
View File

@ -107,6 +107,7 @@ namespace CounterDrone.Core.Algorithms
public int PlatformIndex;
public float TargetX, TargetY, TargetZ;
public float MuzzleVelocity;
public float LaunchAngle;
}
// ═══════════════════════════════════════════════

16
src/CounterDrone.Core/Algorithms/DefensePlanner.cs
View File

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

53
src/CounterDrone.Core/Algorithms/InterceptCalculator.cs
View File

@ -9,16 +9,16 @@ namespace CounterDrone.Core.Algorithms
{
/// <summary>空基拦截：平抛(θ=0°)，下落时间固定。
/// 方程: (A-d)/vd = R + tf + |A - vp*tf - ux|/vp, tf = √(2Δy/g)</summary>
public static (float InterceptArc, float FlightTime) ComputeHorizontal(
public static (float InterceptArc, float FlightTime, float LaunchAngle) ComputeHorizontal(
IReadOnlyList<Waypoint> 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);
}
/// <summary>求解拦截弧长：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R</summary>
public static (float InterceptArc, float ShellTime) Compute(
/// <summary>地基拦截：D² + (Δy + ½g·ts²)² = vs²·ts²，ts = (A-d)/vd - R</summary>
public static (float InterceptArc, float ShellTime, float LaunchAngle) 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);
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<Waypoint> 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<Waypoint> 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));
}
}
}

4
test/unit/CounterDrone.Core.Tests/InterceptCalculatorAirTests.cs
View File

@ -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)

8
test/unit/CounterDrone.Core.Tests/InterceptCalculatorTests.cs
View File

@ -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);