ActiveProtect/Models/TerminalSensitiveSubmunition.cs

using System;
using ActiveProtect.SimulationEnvironment;

namespace ActiveProtect.Models
{
    public class TerminalSensitiveSubmunition : MissileBase
    {
        private enum SubmunitionStage
        {
            Separation,
            Deceleration,
            SpiralScan,
            Attack,
            SelfDestruct
        }

        private SubmunitionStage currentStage;
        private double spiralAngle;
        private const double SpiralRotationSpeed = 4 * Math.PI; // 2 rotations per second
        private const double VerticalDescentSpeed = 10; // 10 m/s
        private const double ScanAngle = 20 * Math.PI / 180; // 20 degrees in radians

        // 减速高度 400米
        private const double DecelerationHeight = 400;

        // 扫描高度 200米
        private const double ScanningHeight = 200;

        // 自毁高度 20米
        private const double SelfDestructHeight = 20;

        private const double AttackSpeed = 200;

        private Vector3D lastScanDirection;
        private bool targetDetected;
        private double detectionAngle;
        
        // 添加传感器
        private InfraredDetector infraredDetector;
        private MillimeterWaveRadiometer radiometer;
        private MillimeterWaveAltimeter altimeter;
        private LaserRangefinder rangefinder;

        private MissileConfig missileConfig;

        public TerminalSensitiveSubmunition(string id, MissileConfig missileConfig, ISimulationManager simulationManager)
            : base(id, missileConfig, simulationManager)
        {
            currentStage = SubmunitionStage.Separation;
            spiralAngle = 0;
            lastScanDirection = Vector3D.Zero;
            targetDetected = false;
            detectionAngle = 0;
            this.missileConfig = missileConfig;
            
            // 初始化传感器
            infraredDetector = new InfraredDetector(Position, Orientation, 1000, 30);
            radiometer = new MillimeterWaveRadiometer(Position, Orientation, 94e9, 1e-6);
            altimeter = new MillimeterWaveAltimeter(Position, Orientation, 1000, 0.1);
            rangefinder = new LaserRangefinder(Position, Orientation, 5000, 1000);
        }

        public override void Update(double deltaTime)
        {
            base.Update(deltaTime);

            // 更新传感器
            infraredDetector.Update(deltaTime);
            radiometer.Update(deltaTime);
            altimeter.Update(deltaTime);
            rangefinder.Update(deltaTime);

            switch (currentStage)
            {
                case SubmunitionStage.Separation:
                    UpdateSeparationStage(deltaTime);
                    break;
                case SubmunitionStage.Deceleration:
                    UpdateDecelerationStage(deltaTime);
                    break;
                case SubmunitionStage.SpiralScan:
                    UpdateSpiralScanStage(deltaTime);
                    break;
                case SubmunitionStage.Attack:
                    UpdateAttackStage(deltaTime);
                    break;
                case SubmunitionStage.SelfDestruct:
                    SelfDestruct();
                    break;
            }
        }

        private void UpdateSeparationStage(double deltaTime)
        {
            Console.WriteLine("分离阶段");
            // 分离阶段
            Vector3D deceleration = new Vector3D(0, 9.8, 0); 
            Velocity += deceleration * deltaTime;
            Position += Velocity * deltaTime;

            if (Position.Y <= DecelerationHeight)
            {
                currentStage = SubmunitionStage.Deceleration;
            }
        }

        private void UpdateDecelerationStage(double deltaTime)
        {
            Console.WriteLine("减速阶段");
            // 减速减旋，垂直速度减小
            Vector3D deceleration = new Vector3D(0, 9.8, 0) - Velocity.Normalize() * 50; // 假设减速度为5m/s^2
            Console.WriteLine($"减速速度: {deceleration}");
            Velocity += deceleration * deltaTime;

            if (Velocity.Magnitude() <= VerticalDescentSpeed || Position.Y <= ScanningHeight)
            {
                Velocity = new Vector3D(0, -VerticalDescentSpeed, 0);
                if (Position.Y <= ScanningHeight)
                {
                    currentStage = SubmunitionStage.SpiralScan;
                }
            }
        }

        private void UpdateSpiralScanStage(double deltaTime)
        {
            Console.WriteLine("扫描阶段");
            spiralAngle = spiralAngle % (2 * Math.PI);
            spiralAngle += SpiralRotationSpeed * deltaTime;

            // 计算水平速度分量
            double horizontalSpeed = VerticalDescentSpeed * Math.Tan(ScanAngle);
            Vector3D horizontalVelocity = new Vector3D(
                Math.Cos(spiralAngle) * horizontalSpeed,
                0,
                Math.Sin(spiralAngle) * horizontalSpeed
            );

            // 更新速度
            Velocity = new Vector3D(horizontalVelocity.X, -VerticalDescentSpeed, horizontalVelocity.Z);

            // 计算扫描方向
            Vector3D scanDirection = new Vector3D(
                Math.Cos(spiralAngle) * Math.Sin(ScanAngle),
                -Math.Cos(ScanAngle),
                Math.Sin(spiralAngle) * Math.Sin(ScanAngle)
            );

            if (DetectTarget(scanDirection))
            {
                if (!targetDetected)
                {
                    targetDetected = true;
                    detectionAngle = spiralAngle;
                }
                else if (Math.Abs(spiralAngle - detectionAngle) < 0.1) // 允许一定的误差
                {
                    currentStage = SubmunitionStage.Attack;
                }
            }

            lastScanDirection = scanDirection;

            if (Position.Y <= SelfDestructHeight)
            {
                currentStage = SubmunitionStage.SelfDestruct;
            }
        }

        private void UpdateAttackStage(double deltaTime)
        {
            Console.WriteLine("攻击阶段");
            // 攻击逻辑
            Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
            Vector3D direction = (targetPosition - Position).Normalize();
            Velocity = direction * AttackSpeed;

            if (Vector3D.Distance(Position, targetPosition) < 5)
            {
                Explode();
            }
        }

        private void UpdateSelfDestructStage(double deltaTime)
        {
            SelfDestruct();
        }

        private bool DetectTarget(Vector3D scanDirection)
        {
            Vector3D targetPosition = SimulationManager.GetEntityById(TargetId).Position;
            Vector3D toTarget = (targetPosition - Position).Normalize();
            
            // 检查目标是否在扫描线上
            return Vector3D.DotProduct(scanDirection, toTarget) > Math.Cos(5 * Math.PI / 180); // 允许5度的误差
        }

        public override string GetStatus()
        {
            return $"{base.GetStatus()}\nSubmunition Stage: {currentStage}\nDetection Angle: {detectionAngle * 180 / Math.PI:F2}°\nSpiral Angle: {spiralAngle * 180 / Math.PI:F2}°\nTarget Detected: {targetDetected}";
        }
    }
}