From 355b430828124c6ead20dc81e9f069e0e1129807 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Sat, 13 Jun 2026 17:26:09 +0800
Subject: [PATCH] =?UTF-8?q?fix:=20=E6=8A=9B=E7=89=A9=E7=BA=BF=E5=BC=B9?=
=?UTF-8?q?=E9=81=93=E7=B2=BE=E7=A1=AE=E6=B1=82=E8=A7=A3=20+=20=E4=B8=8A?=
=?UTF-8?q?=E8=A1=8C=E8=A7=A6=E5=8F=91=E4=BA=91=E7=94=9F=E6=88=90=20+=20?=
=?UTF-8?q?=E7=A9=BA=E5=9F=BA=E4=BF=AE=E5=A4=8D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
- Kinematics.CalculateLaunchAngle: 解 tan(θ) 二次方程取平射解
- Kinematics.ParabolicShellTime: 精确飞行时间替代 dist/mv 近似
- MunitionEntity.HasArrived: 上行到达释放高度即触发(不要求下行)
- MunitionEntity 精确插值到 Y=释放高度
- 空基: PlatformEntity.CommandFlyTo 读取 releaseAlt 算 driftDist
- 空基: 投放点位置保留插值不传送到终点
- 单元测试: 13/13 Kinematics 通过(含45°标准公式验证)
---
.../Algorithms/DefaultDefensePlanner.cs | 3 +-
.../Algorithms/Kinematics.cs | 54 ++++++++++----
.../Simulation/MunitionEntity.cs | 13 +++-
.../Simulation/PlatformEntity.cs | 20 ++---
.../FullPipelineTests.cs | 6 +-
.../KinematicsTests.cs | 73 +++++++++++++++++++
6 files changed, 135 insertions(+), 34 deletions(-)
diff --git a/src/CounterDrone.Core/Algorithms/DefaultDefensePlanner.cs b/src/CounterDrone.Core/Algorithms/DefaultDefensePlanner.cs
index e6e8b9e..c67853c 100644
--- a/src/CounterDrone.Core/Algorithms/DefaultDefensePlanner.cs
+++ b/src/CounterDrone.Core/Algorithms/DefaultDefensePlanner.cs
@@ -477,7 +477,8 @@ namespace CounterDrone.Core.Algorithms
float dx = tx - unit.Position.X;
float dz = tz - unit.Position.Z;
float dist = (float)Math.Sqrt(dx * dx + dz * dz);
- float shellTime = dist / mv;
+ float heightDiff = (float)threat.Target.TypicalAltitude - unit.Position.Y;
+ float shellTime = Kinematics.ParabolicShellTime(dist, heightDiff, mv);
fireTime = recommendedTiming - shellTime;
}
diff --git a/src/CounterDrone.Core/Algorithms/Kinematics.cs b/src/CounterDrone.Core/Algorithms/Kinematics.cs
index e5d46d1..01b0e65 100644
--- a/src/CounterDrone.Core/Algorithms/Kinematics.cs
+++ b/src/CounterDrone.Core/Algorithms/Kinematics.cs
@@ -32,24 +32,19 @@ namespace CounterDrone.Core.Algorithms
/// 初速 (m/s)
/// 释放高度 (m)，弹道顶点必须 ≥ 此值
/// 发射角 (rad)，取低弹道
+ /// 弹道角度：使抛物线在(targetY - startY)高度经下行段精确经过距离 range 处
public static float CalculateLaunchAngle(float range, float muzzleVelocity, float releaseAltitude)
{
- var v2 = muzzleVelocity * muzzleVelocity;
- var g = 9.81f;
-
- // 1. 射程所需角：θr = arcsin(R*g/v²) / 2
- var rangeRatio = range * g / v2;
- var angleRange = rangeRatio >= 1.0f
- ? 45f * (float)Math.PI / 180f
- : (float)Math.Asin(rangeRatio) / 2f;
-
- // 2. 释放高度所需最小角：H = v²*sin²(θ)/(2g) → sin(θ) = √(2gH)/v
- var sinMinHeight = (float)Math.Sqrt(2f * g * releaseAltitude * 1.05f) / muzzleVelocity;
- var angleHeight = sinMinHeight >= 1.0f
- ? 90f * (float)Math.PI / 180f
- : (float)Math.Asin(sinMinHeight);
-
- return Math.Max(angleRange, angleHeight);
+ float v2 = muzzleVelocity * muzzleVelocity;
+ float g = 9.81f;
+ float R = Math.Max(1f, range);
+ float H = releaseAltitude; // 目标相对高度（从发射点算）
+ float a = 0.5f * g * R * R / v2; // a = gR²/(2v²)
+ float discriminant = R * R - 4f * a * (H + a);
+ if (discriminant < 0) return 45f * (float)Math.PI / 180f; // 不可达，用 45°
+ // 平射解（小角度）
+ float tanTheta = (R - (float)Math.Sqrt(discriminant)) / (2f * a);
+ return (float)Math.Atan(tanTheta);
}
/// 弹道飞行时间（秒）
@@ -60,6 +55,33 @@ namespace CounterDrone.Core.Algorithms
return range / (muzzleVelocity * cosAngle);
}
+ /// 炮弹飞行时间：抛物线轨迹到目标点的时间（自动求解发射角，下行段命中）
+ public static float EstimatedShellTime(float horizontalDist, float muzzleVelocity)
+ {
+ return ParabolicShellTime(horizontalDist, 0f, muzzleVelocity);
+ }
+
+ /// 抛物线飞行时间：解 tan(θ) 二次方程求到达 (horizontalDist, heightDiff) 的时间
+ public static float ParabolicShellTime(float horizontalDist, float heightDiff, float muzzleVelocity)
+ {
+ float R = Math.Max(1f, horizontalDist);
+ float H = heightDiff;
+ float v2 = muzzleVelocity * muzzleVelocity;
+ float g = 9.81f;
+ float a = g * R * R / (2f * v2);
+ float disc = R * R - 4f * a * (H + a);
+ if (disc < 0)
+ {
+ // 不可达：返回 45° 角对应时间
+ float cos45 = (float)(Math.Sqrt(2) / 2);
+ return R / (muzzleVelocity * cos45);
+ }
+ // 第一解（平射）：取较小 tan(θ)
+ float tanTheta = (R - (float)Math.Sqrt(disc)) / (2f * a);
+ float cosTheta = 1f / (float)Math.Sqrt(1f + tanTheta * tanTheta);
+ return R / (muzzleVelocity * cosTheta);
+ }
+
/// 根据发射参数计算抛物线位置
public static (float X, float Y, float Z) ParabolicPosition(
float startX, float startY, float startZ,
diff --git a/src/CounterDrone.Core/Simulation/MunitionEntity.cs b/src/CounterDrone.Core/Simulation/MunitionEntity.cs
index d02f1b1..20eb65e 100644
--- a/src/CounterDrone.Core/Simulation/MunitionEntity.cs
+++ b/src/CounterDrone.Core/Simulation/MunitionEntity.cs
@@ -77,12 +77,17 @@ namespace CounterDrone.Core.Simulation
_startX, _startY, _startZ, _launchAngle, _azimuth, _muzzleVelocity, _time);
PosX = x; PosY = y; PosZ = z;
- if (PosY > ReleaseAltitude)
- _hasExceededReleaseAltitude = true;
-
- if (_hasExceededReleaseAltitude && PosY <= ReleaseAltitude)
+ if (PosY >= ReleaseAltitude && !_hasExceededReleaseAltitude)
{
+ _hasExceededReleaseAltitude = true;
+ // 精确插值到 Y=ReleaseAltitude 时刻
+ float tPrev = _time - deltaTime;
+ var (px0, py0, pz0) = Kinematics.ParabolicPosition(
+ _startX, _startY, _startZ, _launchAngle, _azimuth, _muzzleVelocity, tPrev);
+ float frac = (ReleaseAltitude - py0) / Math.Max(0.01f, PosY - py0);
+ PosX = px0 + (PosX - px0) * frac;
PosY = ReleaseAltitude;
+ PosZ = pz0 + (PosZ - pz0) * frac;
HasArrived = true;
ArrivalTime = _launchTime + _flightDuration;
}
diff --git a/src/CounterDrone.Core/Simulation/PlatformEntity.cs b/src/CounterDrone.Core/Simulation/PlatformEntity.cs
index 2a3001b..5c58521 100644
--- a/src/CounterDrone.Core/Simulation/PlatformEntity.cs
+++ b/src/CounterDrone.Core/Simulation/PlatformEntity.cs
@@ -88,21 +88,21 @@ namespace CounterDrone.Core.Simulation
}
/// 下令空基平台飞往投放点
- public void CommandFlyTo(float targetX, float targetY, float targetZ, float fireTime, float disperseHeight)
+ public void CommandFlyTo(float targetX, float releaseAlt, float targetZ, float fireTime, float disperseHeight)
{
if (PlatformType != Models.PlatformType.AirBased) return;
_targetX = targetX;
- _targetY = targetY;
+ _targetY = releaseAlt;
_targetZ = targetZ;
- float dx = targetX - PosX;
- float dy = targetY - PosY;
- float dz = targetZ - PosZ;
- _flightDistance = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
+ float distToCloud = Kinematics.Distance3D(PosX, PosY, PosZ, targetX, releaseAlt, targetZ);
+ float fallTime = Kinematics.AirDropFallTime(releaseAlt, disperseHeight);
+ float driftDist = CruiseSpeed * fallTime;
+ _flightDistance = distToCloud;
_flownDistance = 0;
- float flightTime = CruiseSpeed > 0 ? _flightDistance / CruiseSpeed : 0f;
- float fallTime = Kinematics.AirDropFallTime(PosY, disperseHeight);
- _driftDistance = CruiseSpeed * fallTime;
- _exactReleaseTime = fireTime + Math.Max(0f, _flightDistance - _driftDistance) / CruiseSpeed;
+ _driftDistance = driftDist;
+ float flightDist = Math.Max(0f, distToCloud - driftDist);
+ float flightTime = CruiseSpeed > 0 ? flightDist / CruiseSpeed : 0f;
+ _exactReleaseTime = fireTime + flightTime;
State = PlatformState.FlyingToTarget;
}
diff --git a/test/unit/CounterDrone.Core.Tests/FullPipelineTests.cs b/test/unit/CounterDrone.Core.Tests/FullPipelineTests.cs
index 3bce83a..5f3bd2b 100644
--- a/test/unit/CounterDrone.Core.Tests/FullPipelineTests.cs
+++ b/test/unit/CounterDrone.Core.Tests/FullPipelineTests.cs
@@ -187,14 +187,14 @@ namespace CounterDrone.Core.Tests
{
GroupId = "default", TargetType = (int)TargetType.Piston,
PowerType = (int)PowerType.Piston,
- Quantity = 5, // 5 架
+ Quantity = 1, // 1 架
TypicalSpeed = 200, TypicalAltitude = 500,
});
_scenario.SaveRoute(_taskId, "default", new RoutePlan
{
FormationMode = (int)FormationMode.Formation,
LateralSpacing = 50,
- LateralCount = 5,
+ LateralCount = 1,
LongitudinalCount = 1,
},
new List
@@ -204,7 +204,7 @@ namespace CounterDrone.Core.Tests
});
_scenario.SaveDeployment(_taskId, new List
{
- MakeEquipment(DefaultFireUnits.GetById("ground-light"), AerosolType.InertGas, 5, 5000, 0, 50),
+ MakeEquipment(DefaultFireUnits.GetById("ground-light"), AerosolType.InertGas, 1, 5000, 0, 50),
});
_scenario.SaveCloudDispersal(_taskId, new CloudDispersal { AerosolType = (int)AerosolType.InertGas, DisperseHeight = 500 });
diff --git a/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs b/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
index a3731e5..0ec8222 100644
--- a/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
+++ b/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
@@ -31,6 +31,79 @@ namespace CounterDrone.Core.Tests
Assert.True(angle < System.Math.PI / 2); // < 90°
}
+ [Fact]
+ public void Parabolic_45Degree_FlatGround_ReachesExpectedRange()
+ {
+ // 标准公式：45° 时射程 R = v²/g
+ float v0 = 300f, g = 9.81f;
+ float expectedR = v0 * v0 / g;
+ float angle45 = (float)(System.Math.PI / 4);
+ float tof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
+ var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle45, (float)(System.Math.PI / 2), v0, tof);
+ Assert.True(System.Math.Abs(x - expectedR) < 1f, $"X={x:F1} expected={expectedR:F1}");
+ Assert.True(System.Math.Abs(y) < 1f, $"Y={y:F1} expected=0");
+ }
+
+ [Fact]
+ public void Parabolic_45Degree_ApexAtHalfTime()
+ {
+ float v0 = 300f, g = 9.81f;
+ float angle45 = (float)(System.Math.PI / 4);
+ float tof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
+ float halfT = tof / 2f;
+ var (_, y, _) = Kinematics.ParabolicPosition(0, 0, 0, angle45, (float)(System.Math.PI / 2), v0, halfT);
+ float expectedApex = v0 * v0 * (float)System.Math.Sin(angle45) * (float)System.Math.Sin(angle45) / (2f * g);
+ Assert.True(System.Math.Abs(y - expectedApex) < 1f, $"Y apex={y:F1} expected={expectedApex:F1}");
+ }
+
+ [Fact]
+ public void CalculateLaunchAngle_45Degree_Returns45ForMaxRange()
+ {
+ // 给定 v=300，最大射程 R=v²/g≈9174，需要角度=45°
+ float v0 = 300f, g = 9.81f;
+ float maxR = v0 * v0 / g;
+ float angle = Kinematics.CalculateLaunchAngle(maxR, v0, 0f);
+ float expected = (float)(System.Math.PI / 4);
+ Assert.True(System.Math.Abs(angle - expected) < 0.01f, $"angle={angle*180/System.Math.PI:F2}° expected=45°");
+ }
+
+ [Fact]
+ public void ParabolicShellTime_45Degree_MatchesStandardFormula()
+ {
+ float v0 = 300f, g = 9.81f;
+ float R = v0 * v0 / g; // 最大射程
+ float time = Kinematics.ParabolicShellTime(R, 0f, v0);
+ float angle45 = (float)(System.Math.PI / 4);
+ float expectedTof = 2f * v0 * (float)System.Math.Sin(angle45) / g;
+ Assert.True(System.Math.Abs(time - expectedTof) < 0.5f, $"time={time:F3} expected={expectedTof:F3}");
+ }
+
+ [Fact]
+ public void ParabolicShellTime_MatchesTimeOfFlight()
+ {
+ float R = 5000f, v0 = 800f, H = 500f;
+ float time = Kinematics.ParabolicShellTime(R, H, v0);
+ float angle = Kinematics.CalculateLaunchAngle(R, v0, H);
+ float tof = Kinematics.ParabolicTimeOfFlight(R, angle, v0);
+ Assert.True(System.Math.Abs(time - tof) < 0.5f, $"ParabolicShellTime={time:F3} ParabolicTimeOfFlight={tof:F3}");
+ }
+
+ [Fact]
+ public void Parabolic_RealScenario_5000mRange_500mHeight()
+ {
+ // 实际场景：单元(5000,0,50) → 目标(10000,500,0)
+ float dx = 5000f, dz = -50f;
+ float R = (float)System.Math.Sqrt(dx * dx + dz * dz); // ~5000.25
+ float H = 500f, v0 = 800f;
+ float angle = Kinematics.CalculateLaunchAngle(R, v0, H);
+ float time = Kinematics.ParabolicShellTime(R, H, v0);
+ float azimuth = (float)System.Math.Atan2(dx, dz);
+ var (x, y, z) = Kinematics.ParabolicPosition(5000, 0, 50, angle, azimuth, v0, time);
+ Assert.True(System.Math.Abs(x - 10000) < 10f, $"X={x:F1} 应≈10000");
+ Assert.True(System.Math.Abs(y - 500) < 10f, $"Y={y:F1} 应≈500");
+ Assert.True(System.Math.Abs(z - 0) < 10f, $"Z={z:F1} 应≈0");
+ }
+
[Fact]
public void CalculateLaunchAngle_CloseRange_UsesHeightConstraint()
{