ThreatSourceLibaray/ThreatSource/src/Guidance/InfraredCommandGuidanceSystem.cs

using ThreatSource.Utils;
using ThreatSource.Simulation;
using ThreatSource.Jammer;

namespace ThreatSource.Guidance
{
    /// <summary>
    /// 红外指令导引系统类，实现了基于红外跟踪器的指令制导功能
    /// </summary>
    /// <remarks>
    /// 该类提供了红外指令制导系统的核心功能：
    /// - 指令接收和处理
    /// - 转向速率控制
    /// - 提前量计算
    /// - 制导加速度生成
    /// 用于实现红外指令制导的导弹控制
    /// </remarks>
    public class InfraredCommandGuidanceSystem : BasicGuidanceSystem
    {
        /// <summary>
        /// 跟踪器到导弹的方向向量
        /// </summary>
        /// <remarks>
        /// 记录上一次接收到的跟踪器指向导弹的方向
        /// 用于计算导弹的相对位置
        /// </remarks>
        private Vector3D lastTrackerToMissileVector;

        /// <summary>
        /// 跟踪器到目标的方向向量
        /// </summary>
        /// <remarks>
        /// 记录上一次接收到的跟踪器指向目标的方向
        /// 用于计算目标的相对位置
        /// </remarks>
        private Vector3D lastTrackerToTargetVector;

        /// <summary>
        /// 上一次的期望飞行方向
        /// </summary>
        /// <remarks>
        /// 记录上一次计算的期望飞行方向
        /// 用于计算转向速率
        /// </remarks>
        private Vector3D lastDesiredDirection;

        /// <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="guidanceConfig">红外指令导引系统配置</param>
        /// <param name="maxAcceleration">最大加速度，单位：米/平方秒</param>
        /// <param name="guidanceCoefficient">制导系数</param>
        /// <param name="simulationManager">模拟管理器</param>
        /// <remarks>
        /// 构造过程：
        /// - 初始化基类参数
        /// - 初始化向量记录
        /// - 清零转向速率
        /// </remarks>
        public InfraredCommandGuidanceSystem(string id, InfraredCommandGuidanceConfig guidanceConfig, double maxAcceleration, double guidanceCoefficient, ISimulationManager simulationManager)
            : base(id, maxAcceleration, guidanceCoefficient, simulationManager)
        {
            lastTrackerToMissileVector = Vector3D.Zero;
            lastTrackerToTargetVector = Vector3D.Zero;
            lastDesiredDirection = Vector3D.Zero;
            turnRate = 0;
            InitializeJamming(guidanceConfig.JammingResistanceThreshold, [JammingType.Infrared]);
        }

        /// <summary>
        /// 更新制导系统的状态和计算结果
        /// </summary>
        /// <param name="deltaTime">自上次更新以来的时间间隔，单位：秒</param>
        /// <param name="missilePosition">导弹当前位置，单位：米</param>
        /// <param name="missileVelocity">导弹当前速度，单位：米/秒</param>
        /// <remarks>
        /// 更新过程：
        /// - 更新基类状态
        /// - 计算制导加速度
        /// </remarks>
        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
            if (!IsJammed)
            {
                if (HasGuidance)
                {
                    CalculateGuidanceAcceleration(deltaTime);
                }
                else
                {
                    GuidanceAcceleration = Vector3D.Zero;
                }
            }
            else
            {
                HasGuidance = false;
                GuidanceAcceleration = Vector3D.Zero;
            }
        }

        /// <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)
        {
            // 计算期望飞行方向（从导弹指向目标）
            Vector3D currentDesiredDirection = (lastTrackerToTargetVector - lastTrackerToMissileVector).Normalize();

            // 计算当前飞行方向
            Vector3D currentDirection = Velocity.Normalize();
            // 更新转向速率
            if (lastDesiredDirection != Vector3D.Zero)
            {
                double instantTurnRate = Vector3D.CrossProduct(lastDesiredDirection, currentDesiredDirection).Magnitude() / deltaTime;
                turnRate = turnRate * (1 - TurnRateSmoothingFactor) + instantTurnRate * TurnRateSmoothingFactor;
            }

            // 计算带有提前量的期望方向
            Vector3D leadDirection = Vector3D.CrossProduct(currentDesiredDirection, Vector3D.CrossProduct(currentDesiredDirection, currentDirection).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 * Velocity.Magnitude();
            GuidanceAcceleration = Vector3D.CrossProduct(turnAxis, currentDirection) * accelerationMagnitude;
            // 限制最大加速度
            if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
            {
                GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
            }
            lastDesiredDirection = currentDesiredDirection;
        }

        /// <summary>
        /// 获取制导系统的详细状态信息
        /// </summary>
        /// <returns>包含完整状态参数的字符串</returns>
        /// <remarks>
        /// 返回信息：
        /// - 基本状态信息
        /// - 制导加速度
        /// - 方向向量
        /// - 转向速率
        /// 用于系统监控和调试
        /// </remarks>
        public override string GetStatus()
        {
            return base.GetStatus() + 
                   $" 转向角度: {Vector3D.AngleBetween(lastTrackerToTargetVector, lastTrackerToMissileVector)} 弧度," +
                   $" 转向速率: {turnRate} 弧度/秒";
        }
    }
}