313 lines
14 KiB
C#
313 lines
14 KiB
C#
using System;
|
||
using System.Diagnostics;
|
||
using System.Numerics;
|
||
|
||
namespace ThreatSource.Utils
|
||
{
|
||
/// <summary>
|
||
/// 运动算法静态类，提供各种运动计算方法
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 该类包含了导弹运动相关的各种算法，包括：
|
||
/// - 弹道计算
|
||
/// - 运动学计算
|
||
/// - 制导算法
|
||
/// - 扰动计算
|
||
/// 所有方法都是静态的，可以直接调用。
|
||
/// </remarks>
|
||
public static class MotionAlgorithm
|
||
{
|
||
/// <summary>
|
||
/// 计算抛物线弹道最佳发射方向（选择较小的仰角）
|
||
/// </summary>
|
||
/// <param name="startPos">发射位置坐标</param>
|
||
/// <param name="targetPos">目标位置坐标</param>
|
||
/// <param name="initialSpeed">发射初速度，单位：米/秒</param>
|
||
/// <returns>包含最佳发射方向和初始速度向量的元组，如果无解则返回null</returns>
|
||
/// <remarks>
|
||
/// 该方法使用弹道方程计算两个可能的发射角度，并选择较小的仰角作为最佳发射方向。
|
||
/// 计算考虑了重力影响，但未考虑空气阻力。
|
||
/// </remarks>
|
||
public static (Orientation? orientation, Vector3D? velocity) CalculateBestLaunchOrientation(Vector3D startPos, Vector3D targetPos, double initialSpeed)
|
||
{
|
||
// 计算水平距离
|
||
double dx = targetPos.X - startPos.X;
|
||
double dz = targetPos.Z - startPos.Z;
|
||
double horizontalDistance = Math.Sqrt(dx * dx + dz * dz);
|
||
|
||
// 计算高度差
|
||
double dy = targetPos.Y - startPos.Y;
|
||
|
||
double[]? angles = CalculateLaunchAngles(initialSpeed, horizontalDistance, dy);
|
||
|
||
if (angles == null)
|
||
{
|
||
Debug.WriteLine("无法计算发射角度");
|
||
return (null, null);
|
||
}
|
||
|
||
double bestAngle = Math.Min(angles[0], angles[1]);
|
||
double azimuth = Math.Atan2(dz, dx);
|
||
// 计算初始速度分量
|
||
double vx = initialSpeed * Math.Cos(bestAngle) * Math.Cos(azimuth);
|
||
double vy = initialSpeed * Math.Sin(bestAngle);
|
||
double vz = initialSpeed * Math.Cos(bestAngle) * Math.Sin(azimuth);
|
||
|
||
// 返回方向和速度
|
||
return (new Orientation(bestAngle, azimuth, 0), new Vector3D(vx, vy, vz));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算抛物线弹道发射角度
|
||
/// </summary>
|
||
/// <param name="v0">初始速度，单位：米/秒</param>
|
||
/// <param name="x">目标水平距离，单位：米</param>
|
||
/// <param name="y">目标高度差，单位：米</param>
|
||
/// <param name="g">重力加速度，默认9.81米/秒²</param>
|
||
/// <returns>两个可能的发射角度（弧度），如果无解则返回null</returns>
|
||
/// <remarks>
|
||
/// 使用标准弹道方程计算发射角度，会返回两个解：
|
||
/// - 一个是低角度解（较小仰角）
|
||
/// - 一个是高角度解（较大仰角）
|
||
/// 如果目标距离超出武器射程，则返回null。
|
||
/// </remarks>
|
||
public static double[]? CalculateLaunchAngles(double v0, double x, double y, double g = 9.81)
|
||
{
|
||
double v0_2 = v0 * v0;
|
||
double v0_4 = v0_2 * v0_2;
|
||
|
||
double discriminant = v0_4 - g * (g * x * x + 2 * y * v0_2);
|
||
|
||
if (discriminant < 0)
|
||
{
|
||
Debug.WriteLine("无实数解 - 目标不可达");
|
||
return null;
|
||
}
|
||
|
||
double angle1 = Math.Atan((v0_2 + Math.Sqrt(discriminant)) / (g * x));
|
||
double angle2 = Math.Atan((v0_2 - Math.Sqrt(discriminant)) / (g * x));
|
||
|
||
Debug.WriteLine($"计算得到的两个角度: {angle1 * 180 / Math.PI:F2}° 和 {angle2 * 180 / Math.PI:F2}°");
|
||
|
||
return new[] { angle1, angle2 };
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用运动学定律计算导弹运动状态
|
||
/// </summary>
|
||
/// <param name="currentPosition">当前位置坐标</param>
|
||
/// <param name="currentVelocity">当前速度向量</param>
|
||
/// <param name="acceleration">加速度向量（包含重力加速度）</param>
|
||
/// <param name="deltaTime">时间步长，单位：秒</param>
|
||
/// <returns>包含新位置和新速度的元组</returns>
|
||
/// <remarks>
|
||
/// 该方法适用于无制导状态下的导弹运动计算，使用标准运动学方程：
|
||
/// - 位置更新：p = p0 + v0*t + 0.5*a*t^2
|
||
/// - 速度更新：v = v0 + a*t
|
||
/// </remarks>
|
||
public static (Vector3D newPosition, Vector3D newVelocity) CalculateBallisticMotion(
|
||
Vector3D currentPosition,
|
||
Vector3D currentVelocity,
|
||
Vector3D acceleration,
|
||
double deltaTime)
|
||
{
|
||
// 使用标准运动学方程
|
||
Vector3D newPosition = new(
|
||
currentPosition.X + currentVelocity.X * deltaTime + 0.5 * acceleration.X * deltaTime * deltaTime,
|
||
currentPosition.Y + currentVelocity.Y * deltaTime + 0.5 * acceleration.Y * deltaTime * deltaTime,
|
||
currentPosition.Z + currentVelocity.Z * deltaTime + 0.5 * acceleration.Z * deltaTime * deltaTime
|
||
);
|
||
|
||
Vector3D newVelocity = new(
|
||
currentVelocity.X + acceleration.X * deltaTime,
|
||
currentVelocity.Y + acceleration.Y * deltaTime,
|
||
currentVelocity.Z + acceleration.Z * deltaTime
|
||
);
|
||
|
||
return (newPosition, newVelocity);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用四阶龙格库塔法计算导弹运动状态
|
||
/// </summary>
|
||
/// <param name="deltaTime">时间步长，单位：秒</param>
|
||
/// <param name="position">当前位置坐标</param>
|
||
/// <param name="velocity">当前速度向量</param>
|
||
/// <param name="acceleration">当前加速度向量</param>
|
||
/// <returns>包含新位置和新速度的元组</returns>
|
||
/// <remarks>
|
||
/// 四阶龙格库塔法提供了更高精度的数值解：
|
||
/// - 适用于有制导状态下的导弹运动计算
|
||
/// - 考虑了加速度随时间的变化
|
||
/// - 比简单欧拉法具有更高的精度
|
||
/// </remarks>
|
||
public static (Vector3D newPosition, Vector3D newVelocity) RungeKutta4(double deltaTime, Vector3D position, Vector3D velocity, Vector3D acceleration)
|
||
{
|
||
// 定义一个局部函数来计算加速度
|
||
Vector3D AccelerationFunction(Vector3D pos, Vector3D vel)
|
||
{
|
||
// 这里可以添加更复杂的加速度计算，比如考虑空气阻力等
|
||
return acceleration;
|
||
}
|
||
|
||
// 第一步
|
||
Vector3D k1v = AccelerationFunction(position, velocity) * deltaTime;
|
||
Vector3D k1r = velocity * deltaTime;
|
||
|
||
// 第二步
|
||
Vector3D k2v = AccelerationFunction(position + k1r * 0.5, velocity + k1v * 0.5) * deltaTime;
|
||
Vector3D k2r = (velocity + k1v * 0.5) * deltaTime;
|
||
|
||
// 第三步
|
||
Vector3D k3v = AccelerationFunction(position + k2r * 0.5, velocity + k2v * 0.5) * deltaTime;
|
||
Vector3D k3r = (velocity + k2v * 0.5) * deltaTime;
|
||
|
||
// 第四步
|
||
Vector3D k4v = AccelerationFunction(position + k3r, velocity + k3v) * deltaTime;
|
||
Vector3D k4r = (velocity + k3v) * deltaTime;
|
||
|
||
// 计算新的位置和速度
|
||
Vector3D newPosition = position + (k1r + k2r * 2 + k3r * 2 + k4r) / 6;
|
||
Vector3D newVelocity = velocity + (k1v + k2v * 2 + k3v * 2 + k4v) / 6;
|
||
|
||
return (newPosition, newVelocity);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算比例导引加速度
|
||
/// </summary>
|
||
/// <param name="proportionalNavigationCoefficient">比例导引系数</param>
|
||
/// <param name="missilePosition">导弹当前位置</param>
|
||
/// <param name="missileVelocity">导弹当前速度</param>
|
||
/// <param name="targetPosition">目标当前位置</param>
|
||
/// <param name="targetVelocity">目标当前速度</param>
|
||
/// <returns>比例导引产生的加速度向量</returns>
|
||
/// <remarks>
|
||
/// 使用比例导引法计算制导加速度：
|
||
/// - 计算动态预测时间
|
||
/// - 预测目标未来位置
|
||
/// - 计算视线角速率
|
||
/// - 根据比例导引公式计算所需加速度
|
||
/// 加速度方向垂直于导弹速度方向。
|
||
/// </remarks>
|
||
public static Vector3D CalculateProportionalNavigation(double proportionalNavigationCoefficient,
|
||
Vector3D missilePosition, Vector3D missileVelocity,
|
||
Vector3D targetPosition, Vector3D targetVelocity)
|
||
{
|
||
// 计算导弹到目标的距离
|
||
Vector3D r = targetPosition - missilePosition;
|
||
double distance = r.Magnitude();
|
||
|
||
// 根据距离和相对速度动态计算预测时间
|
||
Vector3D relativeVelocity = targetVelocity - missileVelocity;
|
||
double closingVelocity = -Vector3D.DotProduct(relativeVelocity, r.Normalize());
|
||
|
||
// 预测时间计算考虑：
|
||
// 1. 距离因素：距离越远，预测时间适当增加
|
||
// 2. 接近速度因素：接近速度越大，预测时间适当减小
|
||
// 3. 最小和最大限制：确保预测时间在合理范围内
|
||
double predictionTime = Math.Clamp(
|
||
distance / (closingVelocity + 1e-6) * 0.1, // 基础预测时间（取时程的10%）
|
||
0.05, // 最小预测时间：50ms
|
||
0.5 // 最大预测时间：500ms
|
||
);
|
||
|
||
// 预测目标位置
|
||
Vector3D predictedTargetPosition = targetPosition + targetVelocity * predictionTime;
|
||
|
||
// 计算视线矢量和视线变化率
|
||
Vector3D LOS = (predictedTargetPosition - missilePosition).Normalize();
|
||
Vector3D v = targetVelocity - missileVelocity;
|
||
Vector3D LOSRate = (v - (LOS * Vector3D.DotProduct(v, LOS))) / distance;
|
||
|
||
// 计算制导加速度
|
||
Vector3D acceleration = Vector3D.CrossProduct(
|
||
Vector3D.CrossProduct(LOS, LOSRate),
|
||
missileVelocity.Normalize()
|
||
) * proportionalNavigationCoefficient * missileVelocity.Magnitude();
|
||
|
||
return acceleration;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将角速度转换为加速度
|
||
/// </summary>
|
||
/// <param name="angularRate">角速度</param>
|
||
/// <param name="yawEffectiveness">偏航舵效</param>
|
||
/// <param name="pitchEffectiveness">俯仰舵效</param>
|
||
/// <returns>加速度</returns>
|
||
public static Vector3D ConvertAngularRateToAcceleration(Vector3D angularRate, double yawEffectiveness, double pitchEffectiveness)
|
||
{
|
||
return new Vector3D(
|
||
0,
|
||
angularRate.Y * yawEffectiveness,
|
||
angularRate.Z * pitchEffectiveness
|
||
);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为向量添加高斯噪声
|
||
/// </summary>
|
||
/// <param name="vector">原始向量</param>
|
||
/// <returns>添加高斯噪声后的向量</returns>
|
||
/// <remarks>
|
||
/// 使用Box-Muller变换生成高斯随机数：
|
||
/// - 为向量的每个分量添加独立的高斯噪声
|
||
/// - 噪声强度由标准差控制（默认0.1）
|
||
/// - 用于模拟传感器误差和环境扰动
|
||
/// </remarks>
|
||
public static Vector3D AddRandomPerturbation(Vector3D vector)
|
||
{
|
||
Random random = new();
|
||
// 添加高斯噪声
|
||
double sigma = 0.1; // 扰动标准差
|
||
// 使用Box-Muller变换来生成高斯随机数
|
||
double u1 = random.NextDouble(); // 生成[0, 1)之间的随机数
|
||
double u2 = random.NextDouble(); // 生成[0, 1)之间的随机数
|
||
double r = Math.Sqrt(-2.0 * Math.Log(u1));
|
||
double theta = 2.0 * Math.PI * u2;
|
||
double gaussianX = r * Math.Cos(theta);
|
||
double gaussianY = r * Math.Sin(theta);
|
||
// 由于我们需要三个高斯随机数，我们将再次生成
|
||
u1 = random.NextDouble(); // 生成[0, 1)之间的随机数
|
||
u2 = random.NextDouble(); // 生成[0, 1)之间的随机数
|
||
r = Math.Sqrt(-2.0 * Math.Log(u1));
|
||
theta = 2.0 * Math.PI * u2;
|
||
double gaussianZ = r * Math.Cos(theta);
|
||
// 将高斯随机数转换为指定标准差的高斯随机数
|
||
gaussianX *= sigma;
|
||
gaussianY *= sigma;
|
||
gaussianZ *= sigma;
|
||
return new Vector3D(
|
||
vector.X + gaussianX,
|
||
vector.Y + gaussianY,
|
||
vector.Z + gaussianZ
|
||
);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据风速和风向计算风速向量
|
||
/// </summary>
|
||
/// <param name="windSpeed">风速，单位：米/秒</param>
|
||
/// <param name="windDirection">风向，0-360度，0为北方，顺时针方向</param>
|
||
/// <returns>风速向量，单位：米/秒</returns>
|
||
/// <remarks>
|
||
/// 将风速和风向转换为三维风速向量
|
||
/// 风向是0-360度，0为北方，顺时针方向
|
||
/// 在坐标系中，北方对应+Z，东方对应+X
|
||
/// </remarks>
|
||
public static Vector3D CalculateWindVector(double windSpeed, double windDirection)
|
||
{
|
||
// 风向是0-360度，0为北方，顺时针方向
|
||
double windDirectionRad = windDirection * Math.PI / 180.0;
|
||
|
||
// 在水平面上分解风向
|
||
// 北方对应+Z，东方对应+X
|
||
double windX = windSpeed * Math.Sin(windDirectionRad); // 东西分量
|
||
double windZ = windSpeed * Math.Cos(windDirectionRad); // 南北分量
|
||
|
||
return new Vector3D(windX, 0, windZ);
|
||
}
|
||
}
|
||
}
|