ActiveProtect/Models/MissileBase.cs

using System;
using System.Collections.Generic;
using ActiveProtect.SimulationEnvironment;
using Model;

namespace ActiveProtect.Models
{
    /// <summary>
    /// 表示仿真中的导弹
    /// </summary>
    public class MissileBase : SimulationElement
    {
        /// <summary>
        /// 导弹类型
        /// </summary>
        public MissileType Type { get; protected set; }        
        
        /// <summary>
        /// 当前速度(米/秒)
        /// </summary>
        public double Speed { get; protected set; }
        
        /// <summary>
        /// 最大速度(米/秒)
        /// </summary>
        public double MaxSpeed { get; protected set; }
        
        /// <summary>
        /// 目标ID
        /// </summary>
        public string TargetId { get; protected set; }
        
        /// <summary>
        /// 最大飞行时间(秒)
        /// </summary>
        public double MaxFlightTime { get; protected set; }
        
        /// <summary>
        /// 最大飞行距离(米)
        /// </summary>
        public double MaxFlightDistance { get; protected set; }
        
        /// <summary>
        /// 当前飞行时间(秒)
        /// </summary>
        public double FlightTime { get; protected set; }
        
        /// <summary>
        /// 当前飞行距离(米)
        /// </summary>
        public double FlightDistance { get; protected set; }
        
        /// <summary>
        /// 与目标的距离(米)
        /// </summary>
        public double DistanceToTarget { get; protected set; }
        
        /// <summary>
        /// 导弹距离参数
        /// </summary>
        public MissileDistanceParams DistanceParams { get; protected set; }
        
        /// <summary>
        /// 飞行阶段配置
        /// </summary>
        public FlightStageConfig StageConfig { get; protected set; }
        
        /// <summary>
        /// 当前飞行阶段
        /// </summary>
        public FlightStage CurrentStage { get; protected set; } 
        
        /// <summary>
        /// 上一帧目标位置
        /// </summary>
        private Vector3D LastTargetPosition;
        
        /// <summary>
        /// 比例导引系数
        /// </summary>
        private const double N = 3;

        /// <summary>
        /// 推力加速度(米/秒²)
        /// </summary>
        public double ThrustAcceleration { get; protected set; }
        
        /// <summary>
        /// 当前发动机燃烧时间(秒)
        /// </summary>
        public double EngineBurnTime { get; protected set; }
        
        /// <summary>
        /// 最大发动机燃烧时间(秒)
        /// </summary>
        public double MaxEngineBurnTime { get; protected set; }

        /// <summary>
        /// 最大加速度(米/秒²)
        /// </summary>
        public double MaxAcceleration { get; protected set; } = 100;

        /// <summary>
        /// 比例导引系数
        /// </summary>
        public double ProportionalNavigationCoefficient { get; set; }

        /// <summary>
        /// 是否有制导
        /// </summary>
        public bool HasGuidance { get; protected set; } = false;
        
        /// <summary>
        /// 失去制导的时间(秒)
        /// </summary>
        protected double LostGuidanceTime { get; set; } = 0;
        
        /// <summary>
        /// 最后已知的速度向量
        /// </summary>
        protected Vector3D LastKnownVelocity = Vector3D.Zero;
        
        /// <summary>
        /// 发射速度(米/秒)
        /// </summary>
        public const double LAUNCH_SPEED = 10;
        
        /// <summary>
        /// 发射阶段持续时间(秒)
        /// </summary>
        public const double LAUNCH_DURATION = 0.5;

        protected IMissileStageStrategy currentStage;
        private Dictionary<FlightStage, IMissileStageStrategy> stageStrategies;

        /// <summary>
        /// 导弹质量(千克)
        /// </summary>
        public double Mass { get; protected set; } = 100;

        /// <summary>
        /// 构造函数
        /// </summary>
        public MissileBase(string id, MissileConfig missileConfig, ISimulationManager simulationManager)
            : base(id, missileConfig.InitialPosition, missileConfig.InitialOrientation, simulationManager)
        {
            // 初始化导弹属性
            Speed = missileConfig.InitialSpeed;
            MaxSpeed = missileConfig.MaxSpeed;
            MaxFlightTime = missileConfig.MaxFlightTime;
            MaxFlightDistance = missileConfig.MaxFlightDistance;
            DistanceParams = missileConfig.DistanceParams;
            StageConfig = missileConfig.StageConfig;
            IsActive = true;
            FlightTime = 0;
            FlightDistance = 0;
            SimulationManager = simulationManager;
            ThrustAcceleration = missileConfig.ThrustAcceleration;
            EngineBurnTime = 0;
            MaxEngineBurnTime = missileConfig.MaxEngineBurnTime;
            MaxAcceleration = missileConfig.MaxAcceleration;
            ProportionalNavigationCoefficient = missileConfig.ProportionalNavigationCoefficient;
            TargetId = $"Tank_{missileConfig.TargetIndex + 1}";
            Vector3D horizontalDirection = new Vector3D(Orientation.ToVector().X, 0, Orientation.ToVector().Z).Normalize();
            Velocity = horizontalDirection * missileConfig.InitialSpeed;

            // 初始化策略字典
            stageStrategies = new Dictionary<FlightStage, IMissileStageStrategy>
            {
                { FlightStage.Launch, new LaunchStageStrategy(this) },
                { FlightStage.Acceleration, new AccelerationStageStrategy(this) },
                { FlightStage.Cruise, new CruiseStageStrategy(this) },
                { FlightStage.TerminalGuidance, new TerminalGuidanceStageStrategy(this) },
                { FlightStage.Attack, new AttackStageStrategy(this) }
            };

            // 设置初始阶段
            SetInitialStage();

            currentStage = stageStrategies[CurrentStage];

            LastTargetPosition = Position;

            DistanceToTarget = Vector3D.Distance(Position, SimulationManager.GetEntityById(TargetId).Position); 
        }

        /// <summary>
        /// 设置导弹的初始飞行阶段
        /// </summary>
        private void SetInitialStage()
        {
            if (StageConfig.EnableLaunch) CurrentStage = FlightStage.Launch;
            else if (StageConfig.EnableAcceleration) CurrentStage = FlightStage.Acceleration;
            else if (StageConfig.EnableCruise) CurrentStage = FlightStage.Cruise;
            else if (StageConfig.EnableTerminalGuidance) CurrentStage = FlightStage.TerminalGuidance;
            else if (StageConfig.EnableAttack) CurrentStage = FlightStage.Attack;

            Console.WriteLine($"导弹 {Id} 的初始阶段: {CurrentStage}");
        }

        /// <summary>
        /// 更新导弹状态
        /// </summary>
        public override void Update(double deltaTime)
        {
            if (!IsActive) return;

            if (ShouldSelfDestruct())
            {
                SelfDestruct();
                return;
            }

            UpdateEngineBurnTime(deltaTime);

            Vector3D guidanceCommand = GetGuidanceCommand();

            UpdateMotionState(deltaTime, guidanceCommand);

            if (CheckHit())
            {
                Explode();
                return;
            }
        
            currentStage.Update(deltaTime);
        }

        protected virtual Vector3D GetGuidanceCommand()
        {
            throw new NotImplementedException();
        }

        private void UpdateMotionState(double deltaTime, Vector3D guidanceCommand)
        {
            Vector3D acceleration = CalculateAcceleration(Velocity, guidanceCommand);

            // 使用四阶龙格-库塔方法更新导弹的位置和速度
            (Position, Velocity) = BasicGuidanceSystem.RungeKutta4(deltaTime, Position, Velocity, acceleration);

            // 限制速度不超过最大速度
            if (Velocity.Magnitude() > MaxSpeed)
            {
                Velocity = Velocity.Normalize() * MaxSpeed;
            }

            UpdateMotionStatus(deltaTime);
        }

        protected virtual void UpdateMotionStatus(double deltaTime)
        {
            Speed = Velocity.Magnitude();
            Orientation = Orientation.FromVector(Velocity);
            FlightTime += deltaTime;
            FlightDistance += Speed * deltaTime;
            DistanceToTarget = Vector3D.Distance(Position, SimulationManager.GetEntityById(TargetId).Position);
        }
        /// <summary>
        /// 计算导弹的加速度
        /// </summary>
        private Vector3D CalculateAcceleration(Vector3D velocity, Vector3D guidanceCommand)
        {
            Vector3D thrustAcceleration = Vector3D.Zero;
            Vector3D guidanceAcceleration = Vector3D.Zero;

            switch (CurrentStage)
            {
                case FlightStage.Launch:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                    break;
                case FlightStage.Acceleration:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                    guidanceAcceleration = guidanceCommand;
                    break;
                case FlightStage.Cruise:
                    thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.05);
                    guidanceAcceleration = guidanceCommand * 1;
                    break;
                case FlightStage.TerminalGuidance:
                    thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.05);
                    guidanceAcceleration = guidanceCommand * 1;
                    break;
                case FlightStage.Attack:
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration*0.05;
                    guidanceAcceleration = guidanceCommand * 1;
                    break;
            }

            if (!HasGuidance)
            {
                if (velocity.Magnitude() > 0)
                {
                    thrustAcceleration = velocity.Normalize() * ThrustAcceleration;
                }
                else
                {
                    thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
                }
            }

            //计算重力加速度(反坦克导弹一般升力很小, 主要依靠初始发射动能和持续的推力来维持飞行)
            //Vector3D gravityCompensation = new(0, 9.81, 0);
            Vector3D gravityAcceleration = new(0, -9.81, 0);

            // 计算空气阻力的影响
            Vector3D dragAcceleration = velocity.Normalize() * -1 * CalculateDrag(velocity.Magnitude()) / Mass;
            Vector3D totalAcceleration = guidanceAcceleration + thrustAcceleration + gravityAcceleration +dragAcceleration;
            //Vector3D totalAcceleration = guidanceAcceleration + thrustAcceleration +dragAcceleration;

            Console.WriteLine($"导弹 {Id} 的加速度: {totalAcceleration}, 制导加速度: {guidanceAcceleration}, 推力加速度: {thrustAcceleration}, 空气阻力加速度: {dragAcceleration}");
            if (totalAcceleration.Magnitude() > MaxAcceleration)
            {
                totalAcceleration = totalAcceleration.Normalize() * MaxAcceleration;
            }

            return totalAcceleration;
        }
        /// <summary>
        /// 切换导弹飞行阶段
        /// </summary>
        public void ChangeStage(FlightStage newStage)
        {
            if (stageStrategies.TryGetValue(newStage, out var strategy))
            {
                if (IsStageEnabled(newStage))
                {
                    CurrentStage = newStage;
                    currentStage = strategy;
                    Console.WriteLine($"导弹 {Id} 切换到 {newStage} 阶段");
                }
                else
                {
                    // 如果目标阶段不可用，尝试切换到下一个可用阶段
                    TryChangeToNextAvailableStage(newStage);
                }
            }
            else
            {
                Console.WriteLine($"导弹 {Id} 无法切换到未知阶段 {newStage}");
            }
        }

        /// <summary>
        /// 检查指定飞行阶段是否启用
        /// </summary>
        private bool IsStageEnabled(FlightStage stage)
        {
            return stage switch
            {
                FlightStage.Launch => StageConfig.EnableLaunch,
                FlightStage.Acceleration => StageConfig.EnableAcceleration,
                FlightStage.Cruise => StageConfig.EnableCruise,
                FlightStage.TerminalGuidance => StageConfig.EnableTerminalGuidance,
                FlightStage.Attack => StageConfig.EnableAttack,
                _ => false
            };
        }

        /// <summary>
        /// 尝试切换到下一个可用的飞行阶段
        /// </summary>
        private void TryChangeToNextAvailableStage(FlightStage startStage)
        {
            FlightStage[] stageOrder = new FlightStage[] {FlightStage.Launch, FlightStage.Acceleration, FlightStage.Cruise, FlightStage.TerminalGuidance, FlightStage.Attack};
            int startIndex = Array.IndexOf(stageOrder, startStage);

            for (int i = startIndex + 1; i < stageOrder.Length; i++)
            {
                if (IsStageEnabled(stageOrder[i]))
                {
                    ChangeStage(stageOrder[i]);
                    return;
                }
            }

            // 如果没有可用的下一个阶段，导弹自毁
            Console.WriteLine($"导弹 {Id} 没有可用的下一个阶段，准备自毁");
            SelfDestruct();
        }

        /// <summary>
        /// 计算导弹的加速度
        /// </summary>
        // private (Vector3D, Vector3D) CalculateDerivatives_RK4(Vector3D position, Vector3D velocity, double deltaTime)
        // {
        //     Vector3D guidanceAcceleration = Vector3D.Zero;
        //     Vector3D thrustAcceleration = Vector3D.Zero;

        //     switch (CurrentStage)
        //     {
        //         case FlightStage.Launch:
        //             thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
        //             break;
        //         case FlightStage.Acceleration:
        //             thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
        //             guidanceAcceleration = CalculateProportionalNavigation(position);
        //             break;
        //         case FlightStage.Cruise:
        //             thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.1);
        //             guidanceAcceleration = CalculateProportionalNavigation(position) * 0.5;
        //             break;
        //         case FlightStage.TerminalGuidance:
        //             thrustAcceleration = Orientation.ToVector() * (ThrustAcceleration * 0.1);
        //             guidanceAcceleration = CalculateProportionalNavigation(position) * 1.5;
        //             break;
        //         case FlightStage.Attack:
        //             thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
        //             guidanceAcceleration = CalculateProportionalNavigation(position) * 2;
        //             break;
        //     }

        //     if (!HasGuidance)
        //     {
        //         guidanceAcceleration = Vector3D.Zero;
        //         if (velocity.Magnitude() > 0)
        //         {
        //             thrustAcceleration = velocity.Normalize() * ThrustAcceleration;
        //         }
        //         else
        //         {
        //             thrustAcceleration = Orientation.ToVector() * ThrustAcceleration;
        //         }
        //     }

        //     // 计算空气阻力的影响
        //     Vector3D dragAcceleration = velocity.Normalize() * -1 * CalculateDrag(velocity.Magnitude()) / Mass;

        //     // 计算重力加速度(反坦克导弹一般升力很小, 主要依靠初始发射动能和持续的推力来维持飞行)
        //     //Vector3D gravityCompensation = new(0, 9.81, 0);
        //     Vector3D gravityAcceleration = new(0, -9.81, 0);

        //     Vector3D totalAcceleration = guidanceAcceleration + thrustAcceleration + gravityAcceleration + dragAcceleration;

        //     if (totalAcceleration.Magnitude() > MaxAcceleration)
        //     {
        //         totalAcceleration = totalAcceleration.Normalize() * MaxAcceleration;
        //     }

        //     return (velocity, totalAcceleration);
        // }

        /// <summary>
        /// 计算空气阻力
        /// </summary>
        private static double CalculateDrag(double speed)
        {
            const double dragCoefficient = 0.1; // 减小阻力系数
            const double airDensity = 1.225; // 海平面空气密度，kg/m^3
            const double referenceArea = 0.01; // 减小导弹的参考面积，m^2

            return 0.5 * dragCoefficient * airDensity * referenceArea * speed * speed;
        }

        /// <summary>
        /// 检查是否命中目标
        /// </summary>
        protected bool CheckHit()
        {
            return DistanceToTarget <= DistanceParams.ExplosionDistance;
        }

        /// <summary>
        /// 检查是否应该自毁
        /// </summary>
        protected bool ShouldSelfDestruct()
        {
            if (FlightTime >= MaxFlightTime)
            {
                Console.WriteLine($"导弹 {Id} 超出最大飞行时间 ({FlightTime:F2}/{MaxFlightTime:F2})，准备自毁");
                return true;
            }
            if (FlightDistance >= MaxFlightDistance)
            {
                Console.WriteLine($"导弹 {Id} 超出最大飞行距离 ({FlightDistance:F2}/{MaxFlightDistance:F2})，准备自毁");
                return true;
            }
            if (Position.Y <= -1.0) // 数字略小于0
            {
                Console.WriteLine($"导弹 {Id} 高度小于等于0 ({Position.Y:F2})，准备自毁");
                return true;
            }
            return false;
        }

        /// <summary>
        /// 更新发动机燃烧时间
        /// </summary>
        private void UpdateEngineBurnTime(double deltaTime)
        {
            if (CurrentStage == FlightStage.Acceleration && EngineBurnTime < MaxEngineBurnTime)
            {
                EngineBurnTime += deltaTime;
            }
        }

        /// <summary>
        /// 计算比例导引加速度
        /// </summary>
        private Vector3D CalculateProportionalNavigation(Vector3D position)
        {
            Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
            Vector3D LOS = targetPosition - position;
            Vector3D LOSRate = (targetPosition - LastTargetPosition) / FlightTime - Velocity;
            LastTargetPosition = targetPosition;

            return Vector3D.CrossProduct(Vector3D.CrossProduct(LOS, LOSRate), LOS).Normalize() 
                   * ProportionalNavigationCoefficient * Velocity.Magnitude();
        }

        /// <summary>
        /// 导弹爆炸
        /// </summary>
        public void Explode()
        {
            Deactivate();
            SimulationManager.HandleTargetHit(TargetId, Id);
            Console.WriteLine($"导弹 {Id} 在 {Position} 爆炸，命中目标！");
        }

        /// <summary>
        /// 导弹自毁
        /// </summary>
        public void SelfDestruct()
        {
            if (IsActive)
            {
                string reason = FlightTime >= MaxFlightTime ? "超出最大飞行时间" :
                                FlightDistance >= MaxFlightDistance ? "超出最大飞行距离" :
                                Position.Y <= 0 ? "高度小于等于0" :
                                !HasGuidance ? "失去引导" : "未知原因";
                
                Console.WriteLine($"导弹 {Id} 自毁。原因: {reason}");
                Deactivate();
            }
        }

        /// <summary>
        /// 设置比例导引系数
        /// </summary>
        public void SetProportionalNavigationCoefficient(double newCoefficient)
        {
            ProportionalNavigationCoefficient = newCoefficient;
            Console.WriteLine($"导弹 {Id} 的比例导引系数已更新为 {newCoefficient}");
        }

        /// <summary>
        /// 获取导弹状态
        /// </summary>
        public override string GetStatus()
        {
            MissileRunningState missileRunningState = GetState();
            return $"导弹 {missileRunningState.Id}:\n" +
                   $"  位置: {missileRunningState.Position}\n" +
                   $"  速度: {missileRunningState.Speed:F2} m/s\n" +
                   $"  当前状态: {missileRunningState.CurrentStage}\n" +
                   $"  飞行时间: {missileRunningState.FlightTime:F2}/{MaxFlightTime:F2}\n" +
                   $"  飞行距离: {missileRunningState.FlightDistance:F2}/{MaxFlightDistance:F2}\n" +
                   $"  距离目标: {missileRunningState.DistanceToTarget:F2}\n" +
                   $"  发动机工作时间: {missileRunningState.EngineBurnTime:F2}/{MaxEngineBurnTime:F2}\n" +
                   $"  有引导: {(missileRunningState.HasGuidance ? "是" : "否")}\n" +
                   $"  失去引导时间: {missileRunningState.LostGuidanceTime:F2}";
        }

        /// <summary>
        /// 更新导弹的制导状态
        /// </summary>
        protected virtual void UpdateGuidanceStatus()
        {
            // 基类中的默认实现
        }

        public MissileRunningState GetState()
        {
            return new MissileRunningState
            {
                Id = Id,
                Type = Type,
                Position = Position,
                Velocity = Velocity,
                Orientation = Orientation,
                Speed = Speed,
                TargetId = TargetId,
                FlightTime = FlightTime,
                FlightDistance = FlightDistance,
                DistanceToTarget = DistanceToTarget,
                CurrentStage = CurrentStage,
                HasGuidance = HasGuidance,
                LostGuidanceTime = LostGuidanceTime,
                EngineBurnTime = EngineBurnTime,
                IsActive = IsActive
            };
        }
    }

    /// <summary>
    /// 表示导弹的运行状态信息
    /// </summary>
    public struct MissileRunningState
    {
        /// <summary>
        /// 导弹ID
        /// </summary>
        public string Id { get; set; }

        /// <summary>
        /// 导弹类型
        /// </summary>
        public MissileType Type { get; set; }

        /// <summary>
        /// 当前位置
        /// </summary>
        public Vector3D Position { get; set; }

        /// <summary>
        /// 当前速度向量
        /// </summary>
        public Vector3D Velocity { get; set; }

        /// <summary>
        /// 当前朝向
        /// </summary>
        public Orientation Orientation { get; set; }

        /// <summary>
        /// 当前速度(米/秒)
        /// </summary>
        public double Speed { get; set; }

        /// <summary>
        /// 目标ID
        /// </summary>
        public string TargetId { get; set; }

        /// <summary>
        /// 当前飞行时间(秒)
        /// </summary>
        public double FlightTime { get; set; }

        /// <summary>
        /// 当前飞行距离(米)
        /// </summary>
        public double FlightDistance { get; set; }

        /// <summary>
        /// 与目标的距离(米)
        /// </summary>
        public double DistanceToTarget { get; set; }

        /// <summary>
        /// 当前飞行阶段
        /// </summary>
        public FlightStage CurrentStage { get; set; }

        /// <summary>
        /// 是否有制导
        /// </summary>
        public bool HasGuidance { get; set; }

        /// <summary>
        /// 失去制导时间(秒)
        /// </summary>
        public double LostGuidanceTime { get; set; }

        /// <summary>
        /// 当前发动机燃烧时间(秒)
        /// </summary>
        public double EngineBurnTime { get; set; }

        /// <summary>
        /// 是否处于活动状态
        /// </summary>
        public bool IsActive { get; set; }
    }
}