tian
276bb7adfa
Kinematics: ComputeParabolicRange 垂直运动直接算时间 DefensePlanner: cloudGen 先算再基于此算角度，消除风偏导致的不一致 DefaultLaneDivider: 宽+深双维度判断拆分 DroneEntity/RoutePlan: 支持 LateralAxis/LongitudinalAxis MunitionEntity: _arrivesDescending 修正 测试: 237 通过，Jet(Skip:高弹道)
299 lines
14 KiB
C#
299 lines
14 KiB
C#
using System;
|
||
using CounterDrone.Core.Models;
|
||
|
||
namespace CounterDrone.Core.Algorithms
|
||
{
|
||
/// <summary>通用运动学 / 几何 / 大气扩散工具</summary>
|
||
public static class Kinematics
|
||
{
|
||
/// <summary>Pasquill 稳定度等级</summary>
|
||
public enum StabilityClass { A, B, C, D, E, F }
|
||
/// <summary>风向 → 单位速度矢量 (X, 0, Z)，右手系 Y-up</summary>
|
||
public static (float X, float Y, float Z) WindToVector(WindDirection dir, float speed)
|
||
{
|
||
var angle = dir switch
|
||
{
|
||
WindDirection.N => 0f,
|
||
WindDirection.NE => 45f,
|
||
WindDirection.E => 90f,
|
||
WindDirection.SE => 135f,
|
||
WindDirection.S => 180f,
|
||
WindDirection.SW => 225f,
|
||
WindDirection.W => 270f,
|
||
WindDirection.NW => 315f,
|
||
_ => 0f,
|
||
};
|
||
var rad = angle * (float)Math.PI / 180f;
|
||
return ((float)Math.Sin(rad) * speed, 0, (float)Math.Cos(rad) * speed);
|
||
}
|
||
|
||
/// <summary>给定水平距离和初速，求解抛物线发射角（逆问题）</summary>
|
||
/// <param name="range">水平距离 (m)，必须 > 0</param>
|
||
/// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
|
||
/// <param name="heightDiff">目标相对高度 targetY - startY (m)</param>
|
||
/// <returns>发射角 (rad)，取低弹道解</returns>
|
||
public static float CalculateLaunchAngle(float range, float muzzleVelocity, float heightDiff)
|
||
{
|
||
if (range <= 0)
|
||
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)
|
||
throw new ArgumentException(
|
||
$"当前参数无法命中目标: range={range}, heightDiff={heightDiff}, v₀={muzzleVelocity}");
|
||
// 低弹道解（取较小 tanθ）
|
||
float tanTheta = (range - (float)Math.Sqrt(discriminant)) / (2f * a);
|
||
return (float)Math.Atan(tanTheta);
|
||
}
|
||
|
||
/// <summary>弹道飞行时间（秒）。水平距离 / 水平分速</summary>
|
||
public static float ParabolicTimeOfFlight(float range, float launchAngle, float muzzleVelocity)
|
||
{
|
||
if (range <= 0)
|
||
throw new ArgumentException($"range 必须 > 0，实际: {range}", nameof(range));
|
||
if (muzzleVelocity <= 0)
|
||
throw new ArgumentException($"muzzleVelocity 必须 > 0，实际: {muzzleVelocity}", nameof(muzzleVelocity));
|
||
|
||
float cosAngle = (float)Math.Cos(launchAngle);
|
||
return range / (muzzleVelocity * cosAngle);
|
||
}
|
||
|
||
/// <summary>抛物线飞行时间：给定水平距离和目标高度差，计算弹道时间</summary>
|
||
/// <remarks>等价于先调用 CalculateLaunchAngle 再调用 ParabolicTimeOfFlight</remarks>
|
||
public static float ParabolicShellTime(float horizontalDist, float heightDiff, float muzzleVelocity)
|
||
{
|
||
float angle = CalculateLaunchAngle(horizontalDist, muzzleVelocity, heightDiff);
|
||
return ParabolicTimeOfFlight(horizontalDist, angle, muzzleVelocity);
|
||
}
|
||
|
||
/// <summary>给定初速和发射角，计算到达指定相对高度的水平射程和飞行时间（正问题）</summary>
|
||
/// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
|
||
/// <param name="launchAngle">发射角 (rad)，必须 ∈ (0, π/2)</param>
|
||
/// <param name="heightDiff">目标相对高度 targetY - startY (m)</param>
|
||
/// <returns>(水平射程 m, 飞行时间 s)，取下行段解</returns>
|
||
public static (float range, float timeOfFlight) ComputeParabolicRange(
|
||
float muzzleVelocity, float launchAngle, float heightDiff)
|
||
{
|
||
if (muzzleVelocity <= 0)
|
||
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)
|
||
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);
|
||
}
|
||
|
||
/// <summary>计算弹道顶点（最大高度和到达时间），相对发射点</summary>
|
||
/// <param name="muzzleVelocity">初速 (m/s)，必须 > 0</param>
|
||
/// <param name="launchAngle">发射角 (rad)，必须 ∈ [0, π/2)</param>
|
||
/// <returns>(顶点高度 m, 到达顶点时间 s)，高度是相对发射点的增量</returns>
|
||
public static (float apexHeight, float apexTime) ParabolicApex(
|
||
float muzzleVelocity, float launchAngle)
|
||
{
|
||
if (muzzleVelocity <= 0)
|
||
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 sinA = (float)Math.Sin(launchAngle);
|
||
if (sinA <= 0) return (0, 0); // 水平或俯射，无上升顶点
|
||
float vy = muzzleVelocity * sinA;
|
||
float apexTime = vy / g;
|
||
float apexHeight = vy * vy / (2f * g);
|
||
return (apexHeight, apexTime);
|
||
}
|
||
|
||
/// <summary>给定初速、发射角和方位角，计算任意时刻的抛物线位置（正问题核心）</summary>
|
||
/// <param name="launchAngle">发射角 (rad)，水平面以上为正</param>
|
||
/// <param name="azimuth">方位角 (rad)，0=N(+Z), π/2=E(+X)</param>
|
||
public static (float X, float Y, float Z) ParabolicPosition(
|
||
float startX, float startY, float startZ,
|
||
float launchAngle, float azimuth,
|
||
float muzzleVelocity, float time)
|
||
{
|
||
var cosA = (float)Math.Cos(launchAngle);
|
||
var sinA = (float)Math.Sin(launchAngle);
|
||
var g = 9.81f;
|
||
|
||
var horizontalDist = muzzleVelocity * cosA * time;
|
||
var x = startX + horizontalDist * (float)Math.Sin(azimuth);
|
||
var z = startZ + horizontalDist * (float)Math.Cos(azimuth);
|
||
var y = startY + muzzleVelocity * sinA * time - 0.5f * g * time * time;
|
||
|
||
return (x, y, z);
|
||
}
|
||
|
||
/// <summary>空投弹药位置：继承载机速度 + 重力下落</summary>
|
||
/// <param name="startX,startY,startZ">投放点坐标</param>
|
||
/// <param name="carrierVelX,carrierVelY,carrierVelZ">载机速度矢量 (m/s)</param>
|
||
/// <param name="time">经过时间 (s)</param>
|
||
public static (float X, float Y, float Z) AirDropPosition(
|
||
float startX, float startY, float startZ,
|
||
float carrierVelX, float carrierVelY, float carrierVelZ,
|
||
float time)
|
||
{
|
||
const float g = 9.81f;
|
||
var x = startX + carrierVelX * time;
|
||
var y = startY + carrierVelY * time - 0.5f * g * time * time;
|
||
var z = startZ + carrierVelZ * time;
|
||
return (x, y, z);
|
||
}
|
||
|
||
/// <summary>空投降落时间：从释放高度下落到目标高度所需时间</summary>
|
||
/// <param name="releaseAlt">释放高度 (m)</param>
|
||
/// <param name="targetAlt">目标高度 (m)</param>
|
||
/// <param name="carrierVelY">载机垂直速度 (m/s)，正=向上，默认 0（平飞）</param>
|
||
public static float AirDropFallTime(float releaseAlt, float targetAlt, float carrierVelY = 0f)
|
||
{
|
||
const float g = 9.81f;
|
||
var dy = releaseAlt - targetAlt;
|
||
if (dy <= 0) return 0f;
|
||
return ((float)Math.Sqrt(carrierVelY * carrierVelY + 2f * g * dy) - carrierVelY) / g;
|
||
}
|
||
|
||
/// <summary>两点间方向的速度矢量：normalize(to - from) × speed</summary>
|
||
public static (float X, float Y, float Z) DirectionVelocity(
|
||
float fromX, float fromY, float fromZ,
|
||
float toX, float toY, float toZ,
|
||
float speed)
|
||
{
|
||
float dx = toX - fromX;
|
||
float dy = toY - fromY;
|
||
float dz = toZ - fromZ;
|
||
float dist = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
if (dist < 0.01f) return (0, 0, 0);
|
||
float s = speed / dist;
|
||
return (dx * s, dy * s, dz * s);
|
||
}
|
||
|
||
/// <summary>云团覆盖间隔：无人机穿越单个云团的时间 = 云团直径 / 无人机速度</summary>
|
||
/// <param name="cloudDiameter">云团有效直径 m</param>
|
||
/// <param name="targetSpeedKmh">目标速度 km/h</param>
|
||
/// <param name="minInterval">硬件最小间隔 s</param>
|
||
public static float CloudCoverInterval(float cloudDiameter, float targetSpeedKmh, float minInterval = 0.1f)
|
||
{
|
||
float speedMs = targetSpeedKmh / 3.6f;
|
||
if (speedMs <= 0.1f) return minInterval;
|
||
return Math.Max(minInterval, cloudDiameter / speedMs);
|
||
}
|
||
|
||
/// <summary>点到矩形距离（简化判定）</summary>
|
||
public static float Distance2D(float x1, float z1, float x2, float z2)
|
||
{
|
||
var dx = x2 - x1;
|
||
var dz = z2 - z1;
|
||
return (float)Math.Sqrt(dx * dx + dz * dz);
|
||
}
|
||
|
||
/// <summary>匀速直线运动从一点到另一点的飞行时间（秒）。
|
||
/// 物理模型：无人机/平台沿直线匀速飞行，时间 = 距离 / 速度。
|
||
/// speedKmh 为 0 时抛异常（速度必须 > 0，由调用方保证）。</summary>
|
||
/// <param name="speedKmh">速度 km/h（与 TypicalSpeed 单位一致）</param>
|
||
public static float TravelTime(float fromX, float fromZ, float toX, float toZ, float speedKmh)
|
||
{
|
||
if (speedKmh <= 0)
|
||
throw new ArgumentException("速度必须 > 0", nameof(speedKmh));
|
||
float dist = Distance2D(fromX, fromZ, toX, toZ);
|
||
return dist / (speedKmh / 3.6f);
|
||
}
|
||
|
||
/// <summary>点到三维点距离</summary>
|
||
public static float Distance3D(float x1, float y1, float z1, float x2, float y2, float z2)
|
||
{
|
||
var dx = x2 - x1;
|
||
var dy = y2 - y1;
|
||
var dz = z2 - z1;
|
||
return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
/// <summary>根据天气+风速确定 Pasquill 稳定度</summary>
|
||
public static StabilityClass GetStabilityClass(WeatherType weather, float windSpeed)
|
||
{
|
||
return weather switch
|
||
{
|
||
WeatherType.Sunny when windSpeed <= 5 => StabilityClass.A,
|
||
WeatherType.Sunny => StabilityClass.B,
|
||
WeatherType.Overcast => StabilityClass.D,
|
||
WeatherType.Rain => StabilityClass.D,
|
||
WeatherType.Fog => StabilityClass.E,
|
||
WeatherType.Night => StabilityClass.F,
|
||
_ => StabilityClass.D,
|
||
};
|
||
}
|
||
|
||
/// <summary>Pasquill-Gifford 水平扩散系数 σy(m)，x 为扩散距离(m)</summary>
|
||
public static float SigmaY(StabilityClass cls, float x)
|
||
{
|
||
var coeff = cls switch
|
||
{
|
||
StabilityClass.A => 0.22f, StabilityClass.B => 0.16f,
|
||
StabilityClass.C => 0.11f, StabilityClass.D => 0.08f,
|
||
StabilityClass.E => 0.06f, StabilityClass.F => 0.04f,
|
||
_ => 0.08f,
|
||
};
|
||
return coeff * x / (float)Math.Sqrt(1f + 0.0001f * x);
|
||
}
|
||
|
||
/// <summary>Pasquill-Gifford 垂直扩散系数 σz(m)，x 为扩散距离(m)</summary>
|
||
public static float SigmaZ(StabilityClass cls, float x)
|
||
{
|
||
return cls switch
|
||
{
|
||
StabilityClass.A => 0.20f * x,
|
||
StabilityClass.B => 0.12f * x,
|
||
StabilityClass.C => 0.08f * x / (float)Math.Sqrt(1f + 0.0002f * x),
|
||
StabilityClass.D => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
|
||
StabilityClass.E => 0.03f * x / (float)Math.Sqrt(1f + 0.0003f * x),
|
||
StabilityClass.F => 0.016f * x / (float)Math.Sqrt(1f + 0.0003f * x),
|
||
_ => 0.06f * x / (float)Math.Sqrt(1f + 0.0015f * x),
|
||
};
|
||
}
|
||
|
||
/// <summary>高斯烟团中心浓度 C_max = Q / [(2π)^(3/2) σy² σz]</summary>
|
||
public static float GaussianPeakConcentration(float sourceStrength, float sigmaY, float sigmaZ)
|
||
{
|
||
var denom = (float)Math.Pow(2f * (float)Math.PI, 1.5f) * sigmaY * sigmaY * sigmaZ;
|
||
return denom > 0.001f ? sourceStrength / denom : 0f;
|
||
}
|
||
public static bool PointInPolygon(float px, float pz, ReadOnlySpan<(float X, float Z)> vertices)
|
||
{
|
||
if (vertices.Length < 3) return false;
|
||
bool inside = false;
|
||
int j = vertices.Length - 1;
|
||
for (int i = 0; i < vertices.Length; i++)
|
||
{
|
||
var xi = vertices[i].X; var zi = vertices[i].Z;
|
||
var xj = vertices[j].X; var zj = vertices[j].Z;
|
||
if ((zi > pz) != (zj > pz) && px < (xj - xi) * (pz - zi) / (zj - zi) + xi)
|
||
inside = !inside;
|
||
j = i;
|
||
}
|
||
return inside;
|
||
}
|
||
}
|
||
}
|