268 lines
10 KiB
C#
268 lines
10 KiB
C#
using ThreatSource.Utils;
|
||
using ThreatSource.Simulation;
|
||
using ThreatSource.Jammer;
|
||
using System.Diagnostics;
|
||
|
||
namespace ThreatSource.Guidance
|
||
{
|
||
/// <summary>
|
||
/// 红外指令导引系统类，实现了基于红外跟踪器的指令制导功能
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 该类提供了红外指令制导系统的核心功能：
|
||
/// - 指令接收和处理
|
||
/// - 转向速率控制
|
||
/// - 提前量计算
|
||
/// - 制导加速度生成
|
||
/// 用于实现红外指令制导的导弹控制
|
||
/// </remarks>
|
||
public class InfraredCommandGuidanceSystem : BaseGuidanceSystem
|
||
{
|
||
/// <summary>
|
||
/// 获取设备支持的阻塞干扰类型
|
||
/// </summary>
|
||
public override IEnumerable<JammingType> SupportedBlockingJammingTypes => [JammingType.Infrared];
|
||
|
||
/// <summary>
|
||
/// 获取设备支持的干扰类型
|
||
/// </summary>
|
||
public override IEnumerable<JammingType> SupportedJammingTypes => [JammingType.Infrared];
|
||
|
||
/// <summary>
|
||
/// 跟踪器到导弹的方向向量 (使用可空类型)
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录上一次接收到的跟踪器指向导弹的方向
|
||
/// 用于计算导弹的相对位置
|
||
/// </remarks>
|
||
private Vector3D? LastTrackerToMissileVector { get; set; }
|
||
|
||
/// <summary>
|
||
/// 跟踪器到目标的方向向量 (使用可空类型)
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录上一次接收到的跟踪器指向目标的方向
|
||
/// 用于计算目标的相对位置
|
||
/// </remarks>
|
||
private Vector3D? LastTrackerToTargetVector { get; set; }
|
||
|
||
/// <summary>
|
||
/// 上一次的期望飞行方向 (使用可空类型)
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录上一次计算的期望飞行方向
|
||
/// 用于计算转向速率
|
||
/// </remarks>
|
||
private Vector3D? LastDesiredDirection { get; set; }
|
||
|
||
/// <summary>
|
||
/// 当前转向速率，单位：弧度/秒
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 记录导弹的实时转向速率
|
||
/// 用于计算提前量
|
||
/// </remarks>
|
||
private double turnRate;
|
||
|
||
/// <summary>
|
||
/// 转向速率平滑因子，取值范围：0.1~0.5
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 用于平滑转向速率的变化
|
||
/// 值越大，平滑效果越强
|
||
/// 影响导弹的机动特性
|
||
/// </remarks>
|
||
private const double TurnRateSmoothingFactor = 0.5;
|
||
|
||
/// <summary>
|
||
/// 提前量因子，单位：秒，一般取0.5秒
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 用于计算转向提前量
|
||
/// 值越大，提前量越大
|
||
/// 影响制导精度和稳定性
|
||
/// </remarks>
|
||
private const double LeadTimeFactor = 0.3;
|
||
|
||
/// <summary>
|
||
/// 初始化红外指令导引系统的新实例
|
||
/// </summary>
|
||
/// <param name="id">系统标识</param>
|
||
/// <param name="missileId">导弹ID</param>
|
||
/// <param name="guidanceConfig">红外指令导引系统配置</param>
|
||
/// <param name="maxAcceleration">最大加速度，单位：米/平方秒</param>
|
||
/// <param name="guidanceCoefficient">制导系数</param>
|
||
/// <param name="simulationManager">模拟管理器</param>
|
||
/// <remarks>
|
||
/// 构造过程：
|
||
/// - 初始化基类参数
|
||
/// - 初始化向量记录
|
||
/// - 清零转向速率
|
||
/// </remarks>
|
||
public InfraredCommandGuidanceSystem(string id, string missileId, InfraredCommandGuidanceConfig guidanceConfig, double maxAcceleration, double guidanceCoefficient, ISimulationManager simulationManager)
|
||
: base(id, missileId, maxAcceleration, guidanceCoefficient, simulationManager)
|
||
{
|
||
turnRate = 0;
|
||
InitializeJamming(guidanceConfig.JammingResistanceThreshold, SupportedJammingTypes, SupportedBlockingJammingTypes);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新制导系统的状态和计算结果
|
||
/// </summary>
|
||
/// <param name="deltaTime">自上次更新以来的时间间隔，单位：秒</param>
|
||
/// <remarks>
|
||
/// 更新过程：
|
||
/// - 更新基类状态
|
||
/// - 计算制导加速度
|
||
/// - 更新导引头朝向
|
||
/// </remarks>
|
||
public override void Update(double deltaTime)
|
||
{
|
||
base.Update(deltaTime);
|
||
if (!IsBlockingJammed)
|
||
{
|
||
if (HasGuidance)
|
||
{
|
||
CalculateGuidanceAcceleration(deltaTime);
|
||
|
||
// 调整导引头朝向目标方向
|
||
UpdateSeekerOrientation();
|
||
}
|
||
else
|
||
{
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
LastTrackerToMissileVector = null;
|
||
LastTrackerToTargetVector = null;
|
||
LastDesiredDirection = null;
|
||
turnRate = 0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
HasGuidance = false;
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
LastTrackerToMissileVector = null;
|
||
LastTrackerToTargetVector = null;
|
||
LastDesiredDirection = null;
|
||
turnRate = 0;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 更新导引头朝向，使其指向目标方向
|
||
/// </summary>
|
||
/// <remarks>
|
||
/// 在制导阶段，导引头需要指向目标方向以保持跟踪
|
||
/// </remarks>
|
||
private void UpdateSeekerOrientation()
|
||
{
|
||
if (LastTrackerToTargetVector.HasValue && LastTrackerToMissileVector.HasValue)
|
||
{
|
||
// 计算从导弹到目标的方向向量
|
||
Vector3D toTarget = (LastTrackerToTargetVector.Value - LastTrackerToMissileVector.Value).Normalize();
|
||
|
||
// 将方向向量转换为朝向
|
||
Orientation targetOrientation = Orientation.FromVector(toTarget);
|
||
|
||
// 更新导引头朝向
|
||
KState.Orientation = targetOrientation;
|
||
|
||
Debug.WriteLine($"[INFRARED_COMMAND] 导引头朝向已调整指向目标: {targetOrientation}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 接收并处理制导指令
|
||
/// </summary>
|
||
/// <param name="trackerToMissileVector">跟踪器到导弹的方向向量</param>
|
||
/// <param name="trackerToTargetVector">跟踪器到目标的方向向量</param>
|
||
/// <remarks>
|
||
/// 处理过程：
|
||
/// - 更新制导状态
|
||
/// - 记录方向向量
|
||
/// - 准备制导计算
|
||
/// </remarks>
|
||
public void ReceiveGuidanceCommand(Vector3D trackerToMissileVector, Vector3D trackerToTargetVector)
|
||
{
|
||
HasGuidance = true;
|
||
LastTrackerToMissileVector = trackerToMissileVector;
|
||
LastTrackerToTargetVector = trackerToTargetVector;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算制导加速度
|
||
/// </summary>
|
||
/// <param name="deltaTime">时间间隔，单位：秒</param>
|
||
/// <remarks>
|
||
/// 计算过程：
|
||
/// - 计算期望方向
|
||
/// - 更新转向速率
|
||
/// - 计算提前量
|
||
/// - 生成制导指令
|
||
/// - 限制最大加速度
|
||
/// </remarks>
|
||
protected void CalculateGuidanceAcceleration(double deltaTime)
|
||
{
|
||
if (LastTrackerToMissileVector == null || LastTrackerToTargetVector == null)
|
||
{
|
||
GuidanceAcceleration = Vector3D.Zero;
|
||
HasGuidance = false;
|
||
return;
|
||
}
|
||
|
||
Vector3D currentDesiredDirection = (LastTrackerToTargetVector.Value - LastTrackerToMissileVector.Value).Normalize();
|
||
|
||
Vector3D currentDirection = KState.Velocity.Normalize();
|
||
|
||
if (LastDesiredDirection != null && deltaTime > 0)
|
||
{
|
||
double instantTurnRate = Vector3D.CrossProduct(LastDesiredDirection.Value, currentDesiredDirection).Magnitude() / deltaTime;
|
||
turnRate = turnRate * (1 - TurnRateSmoothingFactor) + instantTurnRate * TurnRateSmoothingFactor;
|
||
}
|
||
else
|
||
{
|
||
turnRate = 0;
|
||
}
|
||
|
||
Vector3D leadDirection = Vector3D.CrossProduct(currentDesiredDirection, Vector3D.CrossProduct(currentDesiredDirection, currentDirection).Normalize()).Normalize();
|
||
Vector3D desiredDirectionWithLead = (currentDesiredDirection + leadDirection * turnRate * LeadTimeFactor).Normalize();
|
||
Vector3D turnAxis = Vector3D.CrossProduct(currentDirection, desiredDirectionWithLead).Normalize();
|
||
double turnAngle = Vector3D.AngleBetween(currentDirection, desiredDirectionWithLead);
|
||
double accelerationMagnitude = ProportionalNavigationCoefficient * turnAngle * KState.Speed;
|
||
GuidanceAcceleration = Vector3D.CrossProduct(turnAxis, currentDirection) * accelerationMagnitude;
|
||
|
||
if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
|
||
{
|
||
GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
|
||
}
|
||
LastDesiredDirection = currentDesiredDirection;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取红外指令导引系统的详细状态信息
|
||
/// </summary>
|
||
/// <returns>包含导引系统状态的信息对象</returns>
|
||
public override ElementStatusInfo GetStatusInfo()
|
||
{
|
||
// 获取基类状态信息
|
||
var statusInfo = base.GetStatusInfo();
|
||
|
||
// 添加角度信息
|
||
if (LastTrackerToTargetVector != null && LastTrackerToMissileVector != null)
|
||
{
|
||
double angle = Vector3D.AngleBetween(LastTrackerToTargetVector.Value, LastTrackerToMissileVector.Value);
|
||
statusInfo.ExtendedProperties["TurnAngle"] = angle;
|
||
}
|
||
else
|
||
{
|
||
statusInfo.ExtendedProperties["TurnAngle"] = "";
|
||
}
|
||
|
||
// 添加转向速率
|
||
statusInfo.ExtendedProperties["TurnRate"] = turnRate;
|
||
|
||
return statusInfo;
|
||
}
|
||
}
|
||
}
|