77 lines
3.0 KiB
C#
77 lines
3.0 KiB
C#
using System.Collections.Generic;
|
||
using CounterDrone.Core.Algorithms;
|
||
using CounterDrone.Core.Models;
|
||
using Xunit;
|
||
|
||
namespace CounterDrone.Core.Tests
|
||
{
|
||
public class InterceptCalculatorTests
|
||
{
|
||
private static List<Waypoint> FlatRoute(float length, float speedMs)
|
||
=> new() {
|
||
new() { PosX = 0, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
|
||
new() { PosX = length, PosY = 0, PosZ = 0, Speed = speedMs * 3.6f },
|
||
};
|
||
|
||
[Fact]
|
||
public void NoHeightDiff_vs100_vd1()
|
||
{
|
||
// 航路 0→20, vd=1, unit@(10,0,0), vs=100, R=0
|
||
// (10-A)² + 0.25·g²·A⁴ = 10000·A²
|
||
// 近似解 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);
|
||
|
||
Assert.True(arc > 0.05f && arc < 0.15f, $"arc={arc:F4}");
|
||
}
|
||
|
||
[Fact]
|
||
public void NoHeightDiff_vs100_vd1_R2()
|
||
{
|
||
// 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);
|
||
|
||
Assert.True(arc > 1.5f && arc < 3f, $"arc={arc:F3}");
|
||
}
|
||
|
||
[Fact]
|
||
public void NoHeightDiff_vs50_vd1_UnitFar()
|
||
{
|
||
// 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);
|
||
|
||
Assert.True(arc > 0.3f && arc < 1.5f, $"arc={arc:F4}");
|
||
}
|
||
|
||
[Fact]
|
||
public void UpwardBranch_ShellGoingUp()
|
||
{
|
||
// Y=500, unit Y=0, vs=800, vd=55.6(200km/h), R=5, route 0→10000
|
||
// 核实求出的 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);
|
||
|
||
Assert.True(arc > 0, $"arc={arc:F0}");
|
||
var (rx, ry, rz) = RouteGeometry.PositionAt(route, arc);
|
||
float d = MathF.Sqrt((rx - 5000) * (rx - 5000) + (rz - 50) * (rz - 50));
|
||
float cosTheta = d / (800 * ts);
|
||
float sinTheta = (ry - 0 + 0.5f * 9.81f * ts * ts) / (800 * ts);
|
||
// 上升段: vy > 0，即 vs*sinθ - g*ts > 0
|
||
float vy = 800 * sinTheta - 9.81f * ts;
|
||
Assert.True(vy > 0, $"vy={vy:F1} should be >0 (upward branch), θ={MathF.Atan2(sinTheta, cosTheta)*180/MathF.PI:F1}°");
|
||
}
|
||
|
||
private static List<Waypoint> StraightRoute(float x0, float x1, float alt, float speed)
|
||
=> new() {
|
||
new() { PosX = x0, PosY = alt, PosZ = 0, Speed = speed },
|
||
new() { PosX = x1, PosY = alt, PosZ = 0, Speed = speed },
|
||
};
|
||
}
|
||
}
|