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

refactor: 引入 ParabolicMotion 替代自研抛物线方程，修复 c 符号 bug

Browse Source 
新增 ParabolicMotion 模块：
- GetFlightTimes(yTarget) 返回所有正时间
- GetFlightTime(yTarget, pref) 按偏好选择
- SolveAngles(range, hd, v0) 返回两个角度
- 支持 TrajectoryPreference {High, Low, Nearest, Farthest}

Kinematics 委托给 ParabolicMotion：
- CalculateLaunchAngle → SolveAngle(Low)
- ComputeParabolicRange → ComputeRange(Nearest)

修复 AI 模块 bug：c = y0 - yTarget → c = yTarget - y0
This commit is contained in:
tian 2026-06-18 00:12:45 +08:00
parent 5909c86402
commit 4631049aad
3 changed files with 121 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
src/CounterDrone.Core/Algorithms/Kinematics.cs
View File

@ -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;
}
/// <summary>弹道飞行时间（秒）。水平距离 / 水平分速</summary>
@ -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;
}
/// <summary>计算弹道顶点（最大高度和到达时间），相对发射点</summary>

109
src/CounterDrone.Core/Algorithms/ParabolicMotion.cs Normal file
View File

@ -0,0 +1,109 @@
using System;
namespace CounterDrone.Core.Algorithms
{
/// <summary>轨迹偏好（多解选择）</summary>
public enum TrajectoryPreference
{
High, // 高抛（较大仰角 / 较晚到达目标高度）
Low, // 低抛（较小仰角 / 较早到达目标高度）
Nearest, // 最短飞行时间（最小正时间）
Farthest // 最长飞行时间（最大正时间）
}
/// <summary>抛物线运动学核心模块</summary>
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);
}
/// <summary>任意时刻 t 的状态</summary>
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);
}
/// <summary>到达目标高度 yTarget 的所有正时间解（0/1/2 个）</summary>
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>();
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<float>();
}
/// <summary>根据偏好选择到达目标高度的飞行时间</summary>
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]
};
}
/// <summary>根据偏好获取水平和飞行时间</summary>
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);
}
// ═══ 逆问题：求解瞄准角度 ═══
/// <summary>已知水平距离、高度差、初速，求解两个可能的发射角 (rad)</summary>
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));
}
/// <summary>根据偏好选择一个瞄准角度</summary>
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
};
}
}
}

20
test/unit/CounterDrone.Core.Tests/KinematicsTests.cs
View File

@ -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<ArgumentException>(() => 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]