tian
355b430828
- Kinematics.CalculateLaunchAngle: 解 tan(θ) 二次方程取平射解 - Kinematics.ParabolicShellTime: 精确飞行时间替代 dist/mv 近似 - MunitionEntity.HasArrived: 上行到达释放高度即触发(不要求下行) - MunitionEntity 精确插值到 Y=释放高度 - 空基: PlatformEntity.CommandFlyTo 读取 releaseAlt 算 driftDist - 空基: 投放点位置保留插值不传送到终点 - 单元测试: 13/13 Kinematics 通过(含45°标准公式验证)
250 lines
11 KiB
C#
250 lines
11 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)</param>
|
||
/// <param name="muzzleVelocity">初速 (m/s)</param>
|
||
/// <param name="releaseAltitude">释放高度 (m)，弹道顶点必须 ≥ 此值</param>
|
||
/// <returns>发射角 (rad)，取低弹道</returns>
|
||
/// <summary>弹道角度：使抛物线在(targetY - startY)高度经下行段精确经过距离 range 处</summary>
|
||
public static float CalculateLaunchAngle(float range, float muzzleVelocity, float releaseAltitude)
|
||
{
|
||
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);
|
||
}
|
||
|
||
/// <summary>弹道飞行时间（秒）</summary>
|
||
public static float ParabolicTimeOfFlight(float range, float launchAngle, float muzzleVelocity)
|
||
{
|
||
float cosAngle = (float)Math.Cos(launchAngle);
|
||
if (cosAngle < 0.001f) return range / Math.Max(1f, muzzleVelocity * 0.7f);
|
||
return range / (muzzleVelocity * cosAngle);
|
||
}
|
||
|
||
/// <summary>炮弹飞行时间：抛物线轨迹到目标点的时间（自动求解发射角，下行段命中）</summary>
|
||
public static float EstimatedShellTime(float horizontalDist, float muzzleVelocity)
|
||
{
|
||
return ParabolicShellTime(horizontalDist, 0f, muzzleVelocity);
|
||
}
|
||
|
||
/// <summary>抛物线飞行时间：解 tan(θ) 二次方程求到达 (horizontalDist, heightDiff) 的时间</summary>
|
||
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);
|
||
}
|
||
|
||
/// <summary>根据发射参数计算抛物线位置</summary>
|
||
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>点到三维点距离</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;
|
||
}
|
||
|
||
/// <summary>高斯烟团某点浓度 C(x,y,z) — 简化：相对中心的偏移</summary>
|
||
public static float GaussianConcentration(float q, float sx, float sy, float sz, float offsetY, float offsetZ)
|
||
{
|
||
var norm = q / ((float)Math.Pow(2f * (float)Math.PI, 1.5f) * sx * sy * sz);
|
||
var ey = (float)Math.Exp(-0.5f * offsetY * offsetY / (sy * sy));
|
||
var ez = (float)Math.Exp(-0.5f * offsetZ * offsetZ / (sz * sz));
|
||
var ezr = (float)Math.Exp(-0.5f * offsetZ * offsetZ / (sz * sz));
|
||
return norm * ey * (ez + ezr);
|
||
}
|
||
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;
|
||
}
|
||
}
|
||
}
|