AIDT 온디바이스 국방 MUM-T 잠수함 발사 잠대공 미사일(IDAS) 수면 돌파 천이 유체역동 충격 상쇄 및 추력편향(TVC) 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.SubLaunchedAntiAir
{
    /// <summary>
    /// IDAS 수면 돌파 비행 텔레메트리 상태
    /// </summary>
    public readonly ref struct WaterExitFlightTelemetryState
    {
        public readonly double WaterDepthMeters;         // 현재 수심 (+: 수중, -: 공중) [m]
        public readonly double VerticalVelocityMps;      // 수직 상승 속도 [m/s]
        public readonly double PitchAngleDegrees;        // 피치 각도 [deg]
        public readonly double PitchRateDegPerSec;       // 피치 각속도 [deg/s]
        public readonly double TransverseAccelerationG;  // 횡방향 슬래밍 가속도 [G]
        public readonly double RocketThrustKiloNewtons;  // 로켓 모터 추력 [kN]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public WaterExitFlightTelemetryState(
            double depth, double vZ, double pitch, double q, double accG, double thrust)
        {
            WaterDepthMeters = depth;
            VerticalVelocityMps = vZ;
            PitchAngleDegrees = pitch;
            PitchRateDegPerSec = q;
            TransverseAccelerationG = accG;
            RocketThrustKiloNewtons = thrust;
        }
    }

    /// <summary>
    /// TVC 및 공력 제어핀 구동 명령
    /// </summary>
    public readonly ref struct IdasTvcServoCommand
    {
        public readonly double TvcPitchDeflectionDeg;    // TVC 제트 베인 피치 편향각 [deg]
        public readonly double AerodynamicFinDeflectionDeg; // 공력 조종핀 편향각 [deg]
        public readonly double EstimatedSlammingMomentNm;// 추정 슬래밍 모멘트 [Nm]
        public readonly bool WaterExitPhaseCompleted;    // 수면 돌파 완료 및 공중 천이 성공 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public IdasTvcServoCommand(double tvc, double fin, double moment, bool completed)
        {
            TvcPitchDeflectionDeg = tvc;
            AerodynamicFinDeflectionDeg = fin;
            EstimatedSlammingMomentNm = moment;
            WaterExitPhaseCompleted = completed;
        }
    }

    /// <summary>
    /// 10,000Hz 주기 IDAS 잠대공 미사일 수면 돌파 TVC 서보 엔진
    /// </summary>
    public ref struct IdasWaterExitTvcServoEngine
    {
        private const double MissileMassKg = 120.0;
        private const double TvcMomentArmMeters = 1.25;
        private const double MaxTvcDeflectionDeg = 15.0;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public IdasTvcServoCommand CalculateServoControl(
            in WaterExitFlightTelemetryState state, double dtSeconds)
        {
            // 1. 수면 근접(-0.5m ~ +1.5m) 슬래밍 모멘트 추정
            bool inTransitionZone = (state.WaterDepthMeters > -0.5) && (state.WaterDepthMeters < 1.5);
            double estimatedSlammingMomentNm = 0.0;

            if (inTransitionZone)
            {
                // 횡가속도로부터 유체 비대칭 모멘트 역산
                estimatedSlammingMomentNm = state.TransverseAccelerationG * 9.80665 * MissileMassKg * 0.85;
            }

            // 2. TVC 제트 베인 편향각 산출 (슬래밍 상쇄 + 자세 복원)
            double thrustNewtons = Math.Max(1000.0, state.RocketThrustKiloNewtons * 1000.0);
            double requiredTvcForceN = estimatedSlammingMomentNm / TvcMomentArmMeters;

            // 자세 복원 PD 제어
            double attitudeCorrectionDeg = -1.8 * state.PitchAngleDegrees - 0.22 * state.PitchRateDegPerSec;

            double tvcAngleDeg = (requiredTvcForceN / thrustNewtons) * (180.0 / Math.PI) + attitudeCorrectionDeg;
            tvcAngleDeg = Math.Clamp(tvcAngleDeg, -MaxTvcDeflectionDeg, MaxTvcDeflectionDeg);

            // 3. 공기 중 진입 시 공력 핀 활성화
            double finAngleDeg = 0.0;
            if (state.WaterDepthMeters < 0.0)
            {
                finAngleDeg = Math.Clamp(attitudeCorrectionDeg * 1.5, -20.0, 20.0);
            }

            bool exitCompleted = state.WaterDepthMeters < -2.0; // 수면 2m 이상 공중 진입 완료

            return new IdasTvcServoCommand(
                tvcAngleDeg,
                finAngleDeg,
                estimatedSlammingMomentNm,
                exitCompleted
            );
        }
    }
}