AIDT 온디바이스 국방 MUM-T 수상함 초공동 초고속 유도 폭뢰(Guided Depth Charge) 적 어뢰 하드킬 요격 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.GuidedDepthCharge
{
    /// <summary>
    /// 초공동 유도 폭뢰 동역학 및 근접 센서 계측 상태
    /// </summary>
    public readonly ref struct DepthChargeTelemetryState
    {
        public readonly double ForwardSpeedMps;        // 수중 전진 속도 [m/s]
        public readonly double CurrentDepthMeters;     // 현재 잠항 심도 [m]
        public readonly double RelativeRangeToTorpedoM;// 표적 어뢰 상대 거리 [m]
        public readonly double LosRatePitchRadPerSec;  // 시선각속도 피치 성분 [rad/s]
        public readonly double LosRateYawRadPerSec;    // 시선각속도 요 성분 [rad/s]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DepthChargeTelemetryState(double speed, double depth, double range, double losQ, double losR)
        {
            ForwardSpeedMps = speed;
            CurrentDepthMeters = depth;
            RelativeRangeToTorpedoM = range;
            LosRatePitchRadPerSec = losQ;
            LosRateYawRadPerSec = losR;
        }
    }

    /// <summary>
    /// 캐비테이터 타각 및 신관 작동 지령
    /// </summary>
    public readonly ref struct DepthChargeGuidanceOutput
    {
        public readonly double CavitatorPitchRad;    // 캐비테이터 피치 타각 [rad]
        public readonly double CavitatorYawRad;      // 캐비테이터 요 타각 [rad]
        public readonly bool TriggerProximityFuze;   // 근접 신관 활성화 플래그
        public readonly bool IsWithinLethalRadius;   // 치명 반경(<3.0m) 진입 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DepthChargeGuidanceOutput(double pitch, double yaw, bool trigger, bool lethal)
        {
            CavitatorPitchRad = pitch;
            CavitatorYawRad = yaw;
            TriggerProximityFuze = trigger;
            IsWithinLethalRadius = lethal;
        }
    }

    /// <summary>
    /// 10,000Hz 초공동 초고속 유도 폭뢰 요격 서보 엔진
    /// </summary>
    public ref struct SupercavitatingDepthChargeServoEngine
    {
        private const double WaterDensity = 1025.0; // 해수 밀도 [kg/m^3]
        private const double LethalDistanceM = 3.0; // 3m 치명 요격 반경
        private readonly double _cavitatorRadiusM;
        private readonly double _navConstantNPrime;
        private readonly double _maxCavitatorAngleRad;

        private double _prevRangeM;

        public SupercavitatingDepthChargeServoEngine(
            double cavitatorRadiusM,
            double navConstantNPrime,
            double maxCavitatorAngleDeg)
        {
            _cavitatorRadiusM = cavitatorRadiusM;
            _navConstantNPrime = navConstantNPrime;
            _maxCavitatorAngleRad = maxCavitatorAngleDeg * Math.PI / 180.0;
            _prevRangeM = 999.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DepthChargeGuidanceOutput ProcessCycle(
            in DepthChargeTelemetryState state,
            double dtSeconds)
        {
            // 1. 비례항법(PNG) 요구 가속도 계산 [m/s^2]
            double cmdAccelPitch = _navConstantNPrime * state.ForwardSpeedMps * state.LosRatePitchRadPerSec;
            double cmdAccelYaw = _navConstantNPrime * state.ForwardSpeedMps * state.LosRateYawRadPerSec;

            // 2. 초공동 캐비테이터 양력 계수 기반 요구 타각 환산
            double dynPress = 0.5 * WaterDensity * state.ForwardSpeedMps * state.ForwardSpeedMps;
            double cavArea = Math.PI * _cavitatorRadiusM * _cavitatorRadiusM;
            double liftSlope = 0.85; // 양력 기울기 계수
            double liftPerRadian = dynPress * cavArea * liftSlope;

            // 질량 약 80kg 가정
            const double depthChargeMassKg = 80.0;
            double reqPitchRad = (cmdAccelPitch * depthChargeMassKg) / Math.Max(1e3, liftPerRadian);
            double reqYawRad = (cmdAccelYaw * depthChargeMassKg) / Math.Max(1e3, liftPerRadian);

            double pitchCmd = Math.Clamp(reqPitchRad, -_maxCavitatorAngleRad, _maxCavitatorAngleRad);
            double yawCmd = Math.Clamp(reqYawRad, -_maxCavitatorAngleRad, _maxCavitatorAngleRad);

            // 3. 근접 신관 격발 판정 (치명 반경 3m 이내 & 최근접 거리 통과)
            double rangeRate = (state.RelativeRangeToTorpedoM - _prevRangeM) / dtSeconds;
            _prevRangeM = state.RelativeRangeToTorpedoM;

            bool isLethal = state.RelativeRangeToTorpedoM <= LethalDistanceM;
            bool triggerFuze = isLethal && (rangeRate >= 0.0); // 거리 증가 전환 순간 격발

            return new DepthChargeGuidanceOutput(
                pitchCmd,
                yawCmd,
                triggerFuze,
                isLethal);
        }
    }
}