초공동(Supercavitation) 수중 고속 추진체 환기형 공동 형상 제어 & 1,000Hz 0-Alloc C# 유체동역학 자세 제어 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.Hydrodynamics.Supercavitation
{
    /// 

    /// 초공동 추진체 수중 상태 텔레메트리 (0-Alloc Stack Only)
    /// 
    public readonly ref struct SupercavityTelemetry
    {
        public readonly float ForwardSpeedMps;     // [m/s] 잠항 비행 속도 (예: 120 m/s = 432 km/h)
        public readonly float AmbientDepthMeters;  // [m] 잠항 수심 (예: 20 m)
        public readonly float CavityPressureKpa;   // [kPa] 공동 내부 기포 압력
        public readonly float PitchAngleDeg;       // [deg] 비행체 피치 각도
        public readonly float PitchRateDegPerS;    // [deg/s] 피치 각속도 (IMU 자이로)
        public readonly float CavitatorDiameterMm; // [mm] 캐비테이터 직경 (예: 50 mm)
        public readonly float BodyRadiusMm;        // [mm] 선체 최대 반경 (예: 100 mm)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavityTelemetry(
            float forwardSpeedMps,
            float ambientDepthMeters,
            float cavityPressureKpa,
            float pitchAngleDeg,
            float pitchRateDegPerS,
            float cavitatorDiameterMm,
            float bodyRadiusMm)
        {
            ForwardSpeedMps = forwardSpeedMps;
            AmbientDepthMeters = ambientDepthMeters;
            CavityPressureKpa = cavityPressureKpa;
            PitchAngleDeg = pitchAngleDeg;
            PitchRateDegPerS = pitchRateDegPerS;
            CavitatorDiameterMm = cavitatorDiameterMm;
            BodyRadiusMm = bodyRadiusMm;
        }
    }

    /// 

    /// 초공동 서보 제어 연산 결과 구조체
    /// 
    public readonly ref struct SupercavityControlResult
    {
        public readonly float CommandedCavitatorAngleDeg; // [deg] 캐비테이터 틸트 각도
        public readonly float CommandedGasValvePwm;       // [0.0~1.0] 환기 가스 노즐 PWM 듀티
        public readonly float CavitationNumberSigma;       // [무차원] 유효 캐비테이션 수 sigma
        public readonly float CavityMaxRadiusMm;          // [mm] 공동 최대 반경 R_max
        public readonly float PlaningForceKn;             // [kN] 꼬리부 플래닝 충격력
        public readonly bool IsPlaningEngaged;            // 플래닝 접촉 상태 플래그
        public readonly bool IsCavityStable;              // 초공동 감쌈 안정성 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavityControlResult(
            float commandedCavitatorAngleDeg,
            float commandedGasValvePwm,
            float cavitationNumberSigma,
            float cavityMaxRadiusMm,
            float planingForceKn,
            bool isPlaningEngaged,
            bool isCavityStable)
        {
            CommandedCavitatorAngleDeg = commandedCavitatorAngleDeg;
            CommandedGasValvePwm = commandedGasValvePwm;
            CavitationNumberSigma = cavitationNumberSigma;
            CavityMaxRadiusMm = cavityMaxRadiusMm;
            PlaningForceKn = planingForceKn;
            IsPlaningEngaged = isPlaningEngaged;
            IsCavityStable = isCavityStable;
        }
    }

    /// 

    /// 1,000Hz 실시간 초공동 환기 및 플래닝 제어 엔진
    /// 
    public ref struct SupercavitationPlaningEngine
    {
        private const float WaterDensity = 1025.0f; // [kg/m^3] 해수 밀도
        private const float Gravity = 9.80665f;     // [m/s^2]
        private const float AtmPressureKpa = 101.325f;

        private float integratedPitchError;

        /// 

        /// 1ms 주기 공동 치수 연산, 플래닝 감지 및 서보 출력 루프
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavityControlResult Update(in SupercavityTelemetry telemetry)
        {
            // 1. 수심에 따른 주변 정수압 계산 (P_inf = P_atm + rho * g * h)
            float ambientPressureKpa = AtmPressureKpa + (WaterDensity * Gravity * telemetry.AmbientDepthMeters * 1e-3f);

            // 2. 캐비테이션 수 sigma 산출: sigma = (P_inf - P_c) / (0.5 * rho * U^2)
            float dynPressureKpa = 0.5f * WaterDensity * telemetry.ForwardSpeedMps * telemetry.ForwardSpeedMps * 1e-3f;
            dynPressureKpa = MathF.Max(1.0f, dynPressureKpa);

            float sigma = (ambientPressureKpa - telemetry.CavityPressureKpa) / dynPressureKpa;
            sigma = Math.Clamp(sigma, 0.015f, 0.15f);

            // 3. 로그비노비치 모델 공동 최대 반경 계산: R_max = (d_c / 2) * sqrt(Cx0 / sigma)
            float cx0 = 0.82f * (1.0f + sigma);
            float dcMm = telemetry.CavitatorDiameterMm;
            float rMaxMm = (dcMm * 0.5f) * MathF.Sqrt(cx0 / sigma);

            // 4. 후방 선체 여유 간극 및 플래닝 충격력 추정
            float tailCavityRadiusMm = rMaxMm * 0.88f; // 꼬리부 축소비 반영
            float bodyRadiusMm = telemetry.BodyRadiusMm;
            float tailImmersionMm = bodyRadiusMm - tailCavityRadiusMm;

            bool isPlaning = tailImmersionMm > 0.0f;
            float planingForceKn = 0.0f;
            if (isPlaning)
            {
                // 플래닝 복원력 계산: F = 0.5 * rho * U^2 * S_wet
                float wetAreaM2 = (tailImmersionMm * 1e-3f) * (dcMm * 1e-3f) * 4.0f;
                planingForceKn = (dynPressureKpa * wetAreaM2);
            }

            // 5. 피치 제어: 캐비테이터 틸트 각도 서보 (PD 제어 + 플래닝 댐핑)
            float targetPitchDeg = 0.0f; // 수평 유지
            float pitchError = targetPitchDeg - telemetry.PitchAngleDeg;
            integratedPitchError += pitchError * 0.001f;
            integratedPitchError = Math.Clamp(integratedPitchError, -2.0f, 2.0f);

            // 캐비테이터 각도 명령 (플래닝 발생 시 즉시 기수 내림/올림 감쇠)
            float deltaCavitatorDeg = (pitchError * 1.2f) - (telemetry.PitchRateDegPerS * 0.08f);
            if (isPlaning)
            {
                // 후방 플래닝 모멘트를 상쇄하기 위한 추가 바이어스 틸트
                deltaCavitatorDeg -= (planingForceKn * 0.05f);
            }
            deltaCavitatorDeg = Math.Clamp(deltaCavitatorDeg, -12.0f, 12.0f);

            // 6. 환기 가스 밸브 PWM 듀티 산출 (목표 sigma = 0.035 유지)
            float targetSigma = 0.035f;
            float sigmaError = sigma - targetSigma; // sigma가 크면 가스 추가 분출
            float valvePwm = Math.Clamp(0.3f + (sigmaError * 8.0f), 0.0f, 1.0f);

            bool isStable = (rMaxMm > bodyRadiusMm * 1.05f) && (MathF.Abs(telemetry.PitchAngleDeg) < 8.0f);

            return new SupercavityControlResult(
                commandedCavitatorAngleDeg: deltaCavitatorDeg,
                commandedGasValvePwm: valvePwm,
                cavitationNumberSigma: sigma,
                cavityMaxRadiusMm: rMaxMm,
                planingForceKn: planingForceKn,
                isPlaningEngaged: isPlaning,
                isCavityStable: isStable
            );
        }
    }
}