초공동(Supercavitation) 수중 로켓 추진 & 1,000Hz 0-Alloc C# 공동 포락선·테일 슬래밍(Tail-Slamming) 방지 서보 제어기

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Defense.Hydrodynamics
{
    /// <summary>
    /// 초공동 비행체 수중 항법 및 유체역학 계측 센서 프레임
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct SupercavitySensorFrame
    {
        public readonly double TimestampSeconds;
        public readonly double Velocity_m_s;         // 비행 속도 U_inf (m/s)
        public readonly double AmbientPressurePa;    // 주변 정수압 p_inf (Pa)
        public readonly double CavityPressurePa;     // 공동 내부 기압 p_c (Pa)
        public readonly double PitchAngleRad;        // 피치 각도 (rad)
        public readonly double PitchRateRadPerSec;   // 피치 각속도 (rad/s)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavitySensorFrame(double timestamp, double vel, double pInf, double pCav, double pitch, double pitchRate)
        {
            TimestampSeconds = timestamp;
            Velocity_m_s = vel;
            AmbientPressurePa = pInf;
            CavityPressurePa = pCav;
            PitchAngleRad = pitch;
            PitchRateRadPerSec = pitchRate;
        }
    }

    /// <summary>
    /// 초공동 비행체 제어 지령 출력 프레임
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct SupercavityControlOutput
    {
        public readonly double CavitationNumberSigma;     // 무차원 공동수 sigma
        public readonly double TailClearanceMeters;       // 탄체 후미-기포 간극 (m)
        public readonly double CavitatorAngleRad;         // 캐비테이터 틸트 각도 지령 (rad)
        public readonly double TvcDeflectionRad;          // TVC 로켓 노즐 편향각 (rad)
        public readonly bool IsTailSlammingImminent;      // 테일 슬래밍 임박 경보

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavityControlOutput(double sigma, double clearance, double cavAngle, double tvcAngle, bool isSlamming)
        {
            CavitationNumberSigma = sigma;
            TailClearanceMeters = clearance;
            CavitatorAngleRad = cavAngle;
            TvcDeflectionRad = tvcAngle;
            IsTailSlammingImminent = isSlamming;
        }
    }

    /// <summary>
    /// 1,000Hz 0-Alloc C# 초공동 포락선 추정 및 테일 슬래밍 방지 서보 엔진
    /// </summary>
    public ref struct SupercavitationServoEngine
    {
        private const double WaterDensity = 1025.0;      // 해수 밀도 (kg/m^3)
        private const double CavitatorDiameter = 0.05;   // 캐비테이터 직경 d_n = 50mm
        private const double VehicleLength = 2.8;        // 탄체 전장 L = 2.8m
        private const double VehicleRadius = 0.10;       // 후미 탄체 반경 R_b = 100mm

        // 자세 제어 게인
        private const double Kp_Pitch = 1.8;
        private const double Kd_Pitch = 0.35;

        private double _lastTimestamp;
        private bool _isInitialized;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            _lastTimestamp = 0.0;
            _isInitialized = true;
        }

        /// <summary>
        /// 1,000Hz(1ms 주기) 공동수 계산 및 테일 슬래밍 억제 서보 제어 틱
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavityControlOutput ProcessTick(in SupercavitySensorFrame frame)
        {
            if (!_isInitialized)
            {
                Initialize();
            }

            double dt = frame.TimestampSeconds - _lastTimestamp;
            if (dt <= 0.0 || dt > 0.01) dt = 0.001; // 1ms 표준 샘플링
            _lastTimestamp = frame.TimestampSeconds;

            // 1. 무차원 공동수(sigma) 산출: sigma = (p_inf - p_c) / (0.5 * rho * U^2)
            double dynamicPressure = 0.5 * WaterDensity * frame.Velocity_m_s * frame.Velocity_m_s;
            double deltaP = Math.Max(frame.AmbientPressurePa - frame.CavityPressurePa, 100.0);
            double sigma = deltaP / Math.Max(dynamicPressure, 1000.0);
            sigma = Math.Clamp(sigma, 0.015, 0.20);

            // 2. 로기노비치 원칙 기반 탄체 후미(x = L) 위치의 공동 포락선 반경 산출
            double cxn = 0.82 * (1.0 + sigma);
            double rMax = (CavitatorDiameter / 2.0) * Math.Sqrt(cxn / (sigma * 0.85));
            double lCavity = CavitatorDiameter * (Math.Sqrt(cxn) / sigma) * Math.Sqrt(Math.Log(1.0 / sigma));

            // 타원체 근사: R(x) = R_max * sqrt(1 - ((x - L_c/2) / (L_c/2))^2)
            double normalizedX = (VehicleLength - (lCavity * 0.5)) / (lCavity * 0.5);
            normalizedX = Math.Clamp(normalizedX, -0.99, 0.99);
            double cavityRadiusAtTail = rMax * Math.Sqrt(1.0 - (normalizedX * normalizedX));

            // 3. 탄체 후미와 기포 벽면 사이의 간극(Clearance) 산출
            // 피치각에 의한 후미 편차 dZ = L * sin(pitch)
            double tailDeflection = VehicleLength * Math.Sin(frame.PitchAngleRad);
            double tailClearance = (cavityRadiusAtTail - VehicleRadius) - Math.Abs(tailDeflection);

            // 4. 테일 슬래밍 임박 경보 및 능동 복원 제어량 산출
            bool isSlamming = tailClearance < 0.015; // 간극 15mm 미만 시 긴급 개입

            // PD 자세 제어
            double pitchError = -frame.PitchAngleRad;
            double commandRate = Kp_Pitch * pitchError - Kd_Pitch * frame.PitchRateRadPerSec;

            // 긴급 테일 슬래밍 방지 오프셋
            if (isSlamming)
            {
                commandRate += Math.Sign(frame.PitchAngleRad) * 0.25; // 강제 중앙 복귀 킥
            }

            double cavitatorAngle = Math.Clamp(commandRate * 0.6, -0.15, 0.15); // 캐비테이터 틸트 (rad)
            double tvcAngle = Math.Clamp(commandRate * 0.8, -0.20, 0.20);       // TVC 로켓 노즐 편향 (rad)

            return new SupercavityControlOutput(
                sigma: sigma,
                clearance: tailClearance,
                cavAngle: cavitatorAngle,
                tvcAngle: tvcAngle,
                isSlamming: isSlamming
            );
        }
    }
}