208 lines
9.2 KiB
C#
208 lines
9.2 KiB
C#
using System;
|
||
using System.Numerics;
|
||
|
||
|
||
namespace ActiveProtect.Utility
|
||
{
|
||
public static class MotionAlgorithm
|
||
{
|
||
/// <summary>
|
||
/// 计算抛物线弹道最佳发射方向（选择较小的仰角）
|
||
/// </summary>
|
||
/// <param name="startPos">发射位置</param>
|
||
/// <param name="targetPos">目标位置</param>
|
||
/// <param name="initialSpeed">初始速度</param>
|
||
/// <returns>最佳发射方向，如果无解则返回null</returns>
|
||
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)
|
||
{
|
||
Console.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">初始速度(m/s)</param>
|
||
/// <param name="x">目标点x坐标(m)</param>
|
||
/// <param name="y">目标点y坐标(m)</param>
|
||
/// <param name="g">重力加速度(m/s²)</param>
|
||
/// <returns>两个可能的发射角度(弧度)，如果无解则返回null</returns>
|
||
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)
|
||
{
|
||
Console.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));
|
||
|
||
//Console.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>
|
||
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>
|
||
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>
|
||
public static Vector3D CalculateProportionalNavigation(double proportionalNavigationCoefficient, Vector3D missilePosition, Vector3D missileVelocity, Vector3D targetPosition, Vector3D targetVelocity)
|
||
{
|
||
// 预测时间（预测目标前进方向该时间后到达的位置，可以调整）
|
||
double predictionTime = 0.01;
|
||
|
||
// 预测目标位置
|
||
Vector3D predictedTargetPosition = targetPosition + targetVelocity * predictionTime;
|
||
|
||
Vector3D r = predictedTargetPosition - missilePosition;
|
||
Vector3D v = targetVelocity - missileVelocity;
|
||
|
||
Vector3D LOS = r.Normalize();
|
||
Vector3D LOSRate = (v - (LOS * Vector3D.DotProduct(v, LOS))) / r.Magnitude();
|
||
|
||
Vector3D acceleration = Vector3D.CrossProduct(Vector3D.CrossProduct(LOS, LOSRate), missileVelocity.Normalize()) * proportionalNavigationCoefficient * missileVelocity.Magnitude();
|
||
|
||
return acceleration;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 添加高斯噪声
|
||
/// </summary>
|
||
/// <param name="vector">向量</param>
|
||
/// <returns>添加高斯噪声后的向量</returns>
|
||
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
|
||
);
|
||
}
|
||
|
||
|
||
}
|
||
}
|