ActiveProtect/Models/InfraredCommandGuidanceSystem.cs

using System;
using ActiveProtect.SimulationEnvironment;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 红外指令导引系统类
    /// </summary>
    public class InfraredCommandGuidanceSystem : BasicGuidanceSystem
    {
        private Vector3D lastTrackerToMissileVector;
        private Vector3D lastTrackerToTargetVector;
        private Vector3D lastDesiredDirection;
        private double turnRate;
        private const double TurnRateSmoothingFactor = 0.5; // 转向速率平滑因子，越大越平滑，0.1~0.5
        private const double LeadTimeFactor = 0.3; // 提前量因子，单位：秒，越大提前量越大，一般取0.5秒

        /// <summary>
        /// 制导系数
        /// </summary>
        public double GuidanceCoefficient { get; set; }

        /// <summary>
        /// 最大加速度
        /// </summary>
        public double MaxAcceleration { get; set; }

        /// <summary>
        /// 构造函数
        /// </summary>
        public InfraredCommandGuidanceSystem(double maxAcceleration, double guidanceCoefficient)
            : base()
        {
            lastTrackerToMissileVector = Vector3D.Zero;
            lastTrackerToTargetVector = Vector3D.Zero;
            lastDesiredDirection = Vector3D.Zero;
            MaxAcceleration = maxAcceleration;
            GuidanceCoefficient = guidanceCoefficient;
            turnRate = 0;
        }

        /// <summary>
        /// 更新制导系统
        /// </summary>
        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
            CalculateGuidanceAcceleration(deltaTime);
        }

        /// <summary>
        /// 接收制导指令
        /// </summary>
        public void ReceiveGuidanceCommand(Vector3D trackerToMissileVector, Vector3D trackerToTargetVector)
        {
            HasGuidance = true;
            lastTrackerToMissileVector = trackerToMissileVector;
            lastTrackerToTargetVector = trackerToTargetVector;
        }

        /// <summary>
        /// 计算制导加速度
        /// </summary>
        protected override void CalculateGuidanceAcceleration(double deltaTime)
        {
            if (HasGuidance)
            {
                // 计算期望飞行方向（从导弹指向目标）
                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 = Math.Acos(Vector3D.DotProduct(currentDirection, desiredDirectionWithLead));

                // 计算制导加速度
                double accelerationMagnitude = GuidanceCoefficient * turnAngle * Velocity.Magnitude();
                GuidanceAcceleration = Vector3D.CrossProduct(turnAxis, currentDirection) * accelerationMagnitude;

                // 限制最大加速度
                if (GuidanceAcceleration.Magnitude() > MaxAcceleration)
                {
                    GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration;
                }

                lastDesiredDirection = currentDesiredDirection;

                // Console.WriteLine($"Missile Guidance: Current Position: {Position}, Velocity: {Velocity}, " +
                //                   $"Acceleration: {GuidanceAcceleration}, Current Direction: {currentDirection}, " +
                //                   $"Desired Direction: {desiredDirectionWithLead}, Turn Angle: {turnAngle}, Turn Rate: {turnRate}\n");
            }
            else
            {
                GuidanceAcceleration = Vector3D.Zero;
            }
        }

        /// <summary>
        /// 获取制导系统状态
        /// </summary>
        public override string GetStatus()
        {
            return base.GetStatus() + $", GuidanceAcceleration: {GuidanceAcceleration}, " +
                   $"Tracker to Target Vector: {lastTrackerToTargetVector}, Tracker to Missile Vector: {lastTrackerToMissileVector}, " +
                   $"Turn Rate: {turnRate}";
        }
    }
}