대한민국 해군 윤영하급 유도탄고속함(PKG) 40노트 고속 워터제트 디플렉터 유압 서보 및 1,000Hz 0-Alloc C# 급변침 동역학 제어 엔진

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Navy.HighSpeedVessel
{
    public readonly record struct ShipMotionState(
        float ForwardSpeedKnots, // u
        float LateralSpeedKnots, // v
        float YawRateDegPerSec,  // r
        float RollAngleDeg       // phi
    );

    public readonly record struct HelmCommand(
        float DesiredHeadingDeg,
        float DesiredSpeedKnots,
        bool EmergencyCrashStop
    );

    public ref struct WaterjetActuatorOutput
    {
        public float CenterNozzleRpm;
        public float PortDeflectorDeg;     // 좌현 노즐 각도 (-30 ~ +30)
        public float StarboardDeflectorDeg;// 우현 노즐 각도
        public float ReverserBucketDeploy; // 0.0 (전진) ~ 1.0 (최대 역추력)
        public float ResultingThrustKn;
        public float ResultingYawMomentKnm;
    }

    public static class YoonYoungHaWaterjetDynamicsEngine
    {
        private const float KnotsToMps = 0.514444f;
        private const float SeawaterDensity = 1025.0f; // kg/m^3
        private const float MaxDeflectorAngleDeg = 30.0f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdateSteering(
            ref readonly ShipMotionState state,
            ref readonly HelmCommand helm,
            out WaterjetActuatorOutput output)
        {
            output = default;
            float uMps = state.ForwardSpeedKnots * KnotsToMps;

            // 1. 긴급 정지(Crash Stop) 로직
            if (helm.EmergencyCrashStop)
            {
                output.CenterNozzleRpm = 1800.0f; // 전출력 분사 유지
                output.PortDeflectorDeg = 0.0f;
                output.StarboardDeflectorDeg = 0.0f;
                output.ReverserBucketDeploy = 1.0f; // 버킷 100% 전개 -> 급감속 역분사
                output.ResultingThrustKn = -320.0f; // 강력한 역추력
                output.ResultingYawMomentKnm = 0.0f;
                return;
            }

            // 2. 침로 오차 산출 및 PD 비례-미분 제어
            float headingError = helm.DesiredHeadingDeg; // 상대 오차 각도
            while (headingError > 180.0f) headingError -= 360.0f;
            while (headingError < -180.0f) headingError += 360.0f;

            float kp = 1.2f;
            float kd = 0.45f;
            float desiredDeflection = (kp * headingError) - (kd * state.YawRateDegPerSec);

            // 롤(Roll) 연성 안정화 감쇠 (고속 급선회 시 내측 기울림 모멘트 보정)
            float rollDamping = 0.15f * state.RollAngleDeg;
            desiredDeflection -= rollDamping;

            // 서보 물리적 한계 클램핑
            desiredDeflection = Math.Clamp(desiredDeflection, -MaxDeflectorAngleDeg, MaxDeflectorAngleDeg);

            // 속도 제어 및 유량 추정
            float targetRpm = Math.Clamp(helm.DesiredSpeedKnots * 45.0f, 600.0f, 2100.0f);
            float jetVelocity = targetRpm * 0.025f; // 근사 분사 속도 (m/s)
            float nozzleArea = 0.28f; // m^2
            float massFlow = SeawaterDensity * nozzleArea * jetVelocity;

            // 추력 및 요 모멘트 계산
            float rad = desiredDeflection * (MathF.PI / 180.0f);
            float forwardThrustN = 3.0f * massFlow * (jetVelocity * MathF.Cos(rad) - uMps);
            float lateralThrustN = 2.0f * massFlow * jetVelocity * MathF.Sin(rad);

            float rudderMomentArmMeters = 24.5f; // 무게중심에서 트랜섬까지 거리
            float yawMomentNm = lateralThrustN * rudderMomentArmMeters;

            output.CenterNozzleRpm = targetRpm;
            output.PortDeflectorDeg = desiredDeflection;
            output.StarboardDeflectorDeg = desiredDeflection;
            output.ReverserBucketDeploy = 0.0f;
            output.ResultingThrustKn = forwardThrustN / 1000.0f;
            output.ResultingYawMomentKnm = yawMomentNm / 1000.0f;
        }
    }
}