diff --git a/src/CounterDrone.Core/Algorithms/Kinematics.cs b/src/CounterDrone.Core/Algorithms/Kinematics.cs
index 8198ee2..7aa8733 100644
--- a/src/CounterDrone.Core/Algorithms/Kinematics.cs
+++ b/src/CounterDrone.Core/Algorithms/Kinematics.cs
@@ -38,19 +38,11 @@ namespace CounterDrone.Core.Algorithms
throw new ArgumentException($"range 必须 > 0，实际: {range}", nameof(range));
if (muzzleVelocity <= 0)
throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));
-
- float v2 = muzzleVelocity * muzzleVelocity;
- float g = 9.81f;
- // 轨迹方程: heightDiff = R·tanθ - gR²/(2v²cos²θ)
- // 令 T = tanθ, a = gR²/(2v²): aT² - RT + (a + heightDiff) = 0
- float a = 0.5f * g * range * range / v2;
- float discriminant = range * range - 4f * a * (heightDiff + a);
- if (discriminant < 0)
+ var angle = ParabolicMotion.SolveAngle(range, heightDiff, muzzleVelocity, TrajectoryPreference.Low);
+ if (!angle.HasValue)
throw new ArgumentException(
$"当前参数无法命中目标: range={range}, heightDiff={heightDiff}, v₀={muzzleVelocity}");
- // 低弹道解（取较小 tanθ）
- float tanTheta = (range - (float)Math.Sqrt(discriminant)) / (2f * a);
- return (float)Math.Atan(tanTheta);
+ return angle.Value;
}
/// 弹道飞行时间（秒）。水平距离 / 水平分速
@@ -85,25 +77,12 @@ namespace CounterDrone.Core.Algorithms
throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));
if (launchAngle <= -Math.PI / 2 || launchAngle >= Math.PI / 2)
throw new ArgumentException($"launchAngle 必须在 (-π/2, π/2) 内，实际: {launchAngle}", nameof(launchAngle));
-
- float g = 9.81f;
- float cosA = (float)Math.Cos(launchAngle);
- float sinA = (float)Math.Sin(launchAngle);
-
- // 垂直运动: ½g·t² - v·sinθ·t + heightDiff = 0
- float vy = muzzleVelocity * sinA;
- float disc = vy * vy - 2f * g * heightDiff;
- if (disc < 0)
+ var motion = new ParabolicMotion(muzzleVelocity, launchAngle);
+ var result = motion.ComputeRange(heightDiff, TrajectoryPreference.Nearest);
+ if (!result.HasValue)
throw new ArgumentException(
$"当前参数无法达到目标高度: heightDiff={heightDiff}, v₀={muzzleVelocity}, θ={launchAngle * 180 / Math.PI:F1}°");
-
- // 取较小正时间（低弹道或唯一解）
- float t1 = (vy - (float)Math.Sqrt(disc)) / g;
- float t2 = (vy + (float)Math.Sqrt(disc)) / g;
- float timeOfFlight = t1 > 0 ? t1 : t2;
- float range = muzzleVelocity * cosA * timeOfFlight;
-
- return (range, timeOfFlight);
+ return result.Value;
}
/// 计算弹道顶点（最大高度和到达时间），相对发射点
diff --git a/src/CounterDrone.Core/Algorithms/ParabolicMotion.cs b/src/CounterDrone.Core/Algorithms/ParabolicMotion.cs
new file mode 100644
index 0000000..d0556a6
--- /dev/null
+++ b/src/CounterDrone.Core/Algorithms/ParabolicMotion.cs
@@ -0,0 +1,109 @@
+using System;
+
+namespace CounterDrone.Core.Algorithms
+{
+ /// 轨迹偏好（多解选择）
+ public enum TrajectoryPreference
+ {
+ High, // 高抛（较大仰角 / 较晚到达目标高度）
+ Low, // 低抛（较小仰角 / 较早到达目标高度）
+ Nearest, // 最短飞行时间（最小正时间）
+ Farthest // 最长飞行时间（最大正时间）
+ }
+
+ /// 抛物线运动学核心模块
+ public class ParabolicMotion
+ {
+ public readonly float V0, Theta, Y0, X0;
+ public readonly float Vx, Vy;
+ public const float G = 9.81f;
+
+ public ParabolicMotion(float v0, float theta, float y0 = 0, float x0 = 0)
+ {
+ V0 = v0; Theta = theta; Y0 = y0; X0 = x0;
+ Vx = v0 * (float)Math.Cos(theta);
+ Vy = v0 * (float)Math.Sin(theta);
+ }
+
+ /// 任意时刻 t 的状态
+ public (float x, float y, float vx, float vy) GetState(float t)
+ {
+ return (X0 + Vx * t, Y0 + Vy * t - 0.5f * G * t * t, Vx, Vy - G * t);
+ }
+
+ /// 到达目标高度 yTarget 的所有正时间解（0/1/2 个）
+ public float[] GetFlightTimes(float yTarget)
+ {
+ float a = 0.5f * G;
+ float b = -Vy;
+ float c = yTarget - Y0;
+ float d = b * b - 4f * a * c;
+ if (d < 0) return Array.Empty();
+
+ float sqrtD = (float)Math.Sqrt(d);
+ float t1 = (-b - sqrtD) / (2f * a);
+ float t2 = (-b + sqrtD) / (2f * a);
+
+ if (t1 > 0 && t2 > 0)
+ return t1 < t2 ? new[] { t1, t2 } : new[] { t2, t1 };
+ if (t1 > 0) return new[] { t1 };
+ if (t2 > 0) return new[] { t2 };
+ return Array.Empty();
+ }
+
+ /// 根据偏好选择到达目标高度的飞行时间
+ public float? GetFlightTime(float yTarget, TrajectoryPreference pref)
+ {
+ var ts = GetFlightTimes(yTarget);
+ if (ts.Length == 0) return null;
+ if (ts.Length == 1) return ts[0];
+ return pref switch
+ {
+ TrajectoryPreference.Nearest or TrajectoryPreference.Low => ts[0],
+ TrajectoryPreference.Farthest or TrajectoryPreference.High => ts[1],
+ _ => ts[0]
+ };
+ }
+
+ /// 根据偏好获取水平和飞行时间
+ public (float range, float timeOfFlight)? ComputeRange(float yTarget, TrajectoryPreference pref)
+ {
+ float? t = GetFlightTime(yTarget, pref);
+ if (!t.HasValue) return null;
+ return (Vx * t.Value, t.Value);
+ }
+
+ // ═══ 逆问题：求解瞄准角度 ═══
+
+ /// 已知水平距离、高度差、初速，求解两个可能的发射角 (rad)
+ public static (float theta1, float theta2)? SolveAngles(float range, float heightDiff, float v0)
+ {
+ if (range <= 0 || v0 <= 0) return null;
+ float v2 = v0 * v0;
+ float A = (G * range * range) / (2f * v2);
+ float B = -range;
+ float C = heightDiff + A;
+ float d = B * B - 4f * A * C;
+ if (d < 0 || A == 0) return null;
+ float sqrtD = (float)Math.Sqrt(d);
+ float p1 = (-B + sqrtD) / (2f * A);
+ float p2 = (-B - sqrtD) / (2f * A);
+ return ((float)Math.Atan(p1), (float)Math.Atan(p2));
+ }
+
+ /// 根据偏好选择一个瞄准角度
+ public static float? SolveAngle(float range, float heightDiff, float v0, TrajectoryPreference pref)
+ {
+ var result = SolveAngles(range, heightDiff, v0);
+ if (!result.HasValue) return null;
+ var (a1, a2) = result.Value;
+ float high = Math.Max(a1, a2);
+ float low = Math.Min(a1, a2);
+ return pref switch
+ {
+ TrajectoryPreference.High => high,
+ TrajectoryPreference.Low or TrajectoryPreference.Nearest or _ => low
+ };
+ }
+ }
+}
diff --git a/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs b/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
index 2cfa464..8a5725b 100644
--- a/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
+++ b/test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
@@ -176,8 +176,8 @@ namespace CounterDrone.Core.Tests
[Fact]
public void ComputeParabolicRange_ThrowsOnUnreachableHeight()
{
- // 300m/s 45° 不可能达到 5000m 高度的目标
float angle45 = (float)(System.Math.PI / 4);
+ // 300m/s 45° 不可能达到 5000m 高
Assert.Throws(() => Kinematics.ComputeParabolicRange(300f, angle45, 5000f));
}
@@ -256,16 +256,11 @@ namespace CounterDrone.Core.Tests
[Fact]
public void ComputeParabolicRange_ElevatedTarget_VerifiableByPosition()
{
- // v₀=800, θ=10°, 目标在发射点上方 500m
+ // Farthest=下行段, 弹在目标距离命中
float v0 = 800f, heightDiff = 500f;
float angle = 10f * (float)System.Math.PI / 180f;
-
var (range, time) = Kinematics.ComputeParabolicRange(v0, angle, heightDiff);
-
- // 用 ParabolicPosition 验证：在 time 时刻应到达 (range, heightDiff)
var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, time);
- // X 方向（azimuth=0 → cos(0) → +Z 方向）
- // 实际水平距离 = sqrt(x²+z²) = z（因为 azimuth=0）
float actualDist = (float)System.Math.Sqrt(x * x + z * z);
Assert.True(System.Math.Abs(actualDist - range) < 5f,
$"水平距离={actualDist:F1} expected={range:F1}");
@@ -276,19 +271,14 @@ namespace CounterDrone.Core.Tests
[Fact]
public void ComputeParabolicRange_DepressedTarget_VerifiableByPosition()
{
- // v₀=500, θ=5°, 目标在发射点下方 200m
+ // 俯射只有唯一解
float v0 = 500f, heightDiff = -200f;
float angle = 5f * (float)System.Math.PI / 180f;
-
var (range, time) = Kinematics.ComputeParabolicRange(v0, angle, heightDiff);
-
- // 验证到达时间时位置
var (x, y, z) = Kinematics.ParabolicPosition(0, 0, 0, angle, 0, v0, time);
float actualDist = (float)System.Math.Sqrt(x * x + z * z);
- Assert.True(System.Math.Abs(actualDist - range) < 5f,
- $"水平距离={actualDist:F1} expected={range:F1}");
- Assert.True(System.Math.Abs(y - heightDiff) < 5f,
- $"高度={y:F1} expected={heightDiff:F1}");
+ Assert.True(System.Math.Abs(actualDist - range) < 5f);
+ Assert.True(System.Math.Abs(y - heightDiff) < 5f);
}
[Fact]