대한민국 해병대 차기 상륙돌격장갑차(KAAV-II) 해상 수상 활주(Hydroplaning) 차체 플랩 전개 & 2축 워터제트(Waterjet) 추력 벡터 편향 1,000Hz 0-Alloc C# 해상 기동 안정화 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.AmphibiousMobility
{
    /// <summary>
    /// 상륙장갑차 6자유도 해상 동요 센서 상태 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public readonly struct MarineMotionSensors
    {
        public readonly float SpeedKts;       // 수상 속도 (노트)
        public readonly float TrimAngleRad;   // 종경사각 (Pitch, 선수 들림 양수)
        public readonly float HeelAngleRad;   // 횡경사각 (Roll, 우현 기울기 양수)
        public readonly float YawRateRps;     // 요 각속도 (rad/s)
        public readonly float WaveHeightM;    // 실시간 조우 파고 (m)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public MarineMotionSensors(float spd, float trim, float heel, float yr, float wave)
        {
            SpeedKts = spd; TrimAngleRad = trim; HeelAngleRad = heel;
            YawRateRps = yr; WaveHeightM = wave;
        }
    }

    /// <summary>
    /// 수상 활주 플랩 및 워터제트 액추에이터 제어 출력 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct KaavActuatorCommand
    {
        public float BowFlapAngleDeg;        // 전방 뱃머리 플랩 전개각 (0~35도)
        public float SideFlapAngleDeg;       // 측면 하이드로 플랩 각도 (0~25도)
        public float LeftJetNozzleDeg;       // 좌현 워터제트 조향각 (-30~+30도)
        public float RightJetNozzleDeg;      // 우현 워터제트 조향각 (-30~+30도)
        public float LeftEngineRpm;          // 좌현 펌프 엔진 RPM
        public float RightEngineRpm;         // 우현 펌프 엔진 RPM
        public bool IsHydroplaningActive;    // 수상 활주 모드 돌입 여부
    }

    /// <summary>
    /// KAAV-II 수상 활주 및 2축 워터제트 추력 벡터 1,000Hz 0-Alloc C# 안정화 엔진
    /// </summary>
    public sealed class Kaav2HydroplaningEngine
    {
        private const float DegToRad = MathF.PI / 180.0f;
        private const float RadToDeg = 180.0f / MathF.PI;
        private const float TargetPlaningTrimRad = 4.0f * DegToRad; // 최적 활주 트림각 (4.0도)
        private const float PlaningThresholdSpeedKts = 15.0f;       // 활주 천이 임계 속도 (15노트)

        // 제어 PID 게인
        private readonly float _kpTrim = 85.0f;
        private readonly float _kdTrim = 12.5f;
        private readonly float _kpHdr = 60.0f;
        private readonly float _kdHdr = 8.0f;

        private float _prevTrimError;
        private float _prevHeelError;

        public Kaav2HydroplaningEngine()
        {
            _prevTrimError = 0f;
            _prevHeelError = 0f;
        }

        /// <summary>
        /// 1,000Hz 실시간 틱: 해상 자세 및 속도로부터 플랩 및 워터제트 지령 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateTick(
            in MarineMotionSensors sensors,
            float pilotThrottlePercent,      // 조종수 가속 페달 (0~100%)
            float pilotSteeringPercent,      // 조종수 조향 핸들 (-100% 좌 ~ +100% 우)
            float dt,
            ref KaavActuatorCommand command)
        {
            bool isPlaning = sensors.SpeedKts >= PlaningThresholdSpeedKts;

            // 1. 최적 트림(Pitch) 각도 오차 산출
            float trimError = sensors.TrimAngleRad - TargetPlaningTrimRad;
            float trimRate = (trimError - _prevTrimError) / dt;
            _prevTrimError = trimError;

            // 2. 횡경사(Roll) 각도 오차 산출
            float heelError = sensors.HeelAngleRad;
            float heelRate = (heelError - _prevHeelError) / dt;
            _prevHeelError = heelError;

            // 3. 전방 뱃머리 활주 플랩(Bow Flap) 제어 (선수 들림 억제 및 동적 양력 생성)
            // 저속에서는 저항 감소를 위해 수납, 12노트 이상부터 서서히 전개
            float targetBowFlap = 0f;
            if (sensors.SpeedKts > 10.0f)
            {
                // 속도가 빠를수록 양력을 조절하기 위해 피치 오차 반영
                targetBowFlap = 18.0f - (_kpTrim * trimError * RadToDeg) - (_kdTrim * trimRate * RadToDeg);
            }
            command.BowFlapAngleDeg = MathF.Clamp(targetBowFlap, 0.0f, 35.0f);

            // 4. 측면 하이드로 플랩 제어 (횡경사 복원 모멘트)
            float targetSideFlap = 12.0f + (_kpHdr * MathF.Abs(heelError) * RadToDeg);
            command.SideFlapAngleDeg = MathF.Clamp(targetSideFlap, 0.0f, 25.0f);

            // 5. 좌우 2축 워터제트 추진 및 조향 믹싱
            float baseRpm = 1000.0f + (pilotThrottlePercent * 16.0f); // 최대 2,600 RPM
            float steeringDiffRpm = pilotSteeringPercent * 4.5f;

            // 조향 핸들 입력을 노즐 편향각으로 매핑 (-30도 ~ +30도)
            float targetNozzleAngle = (pilotSteeringPercent / 100.0f) * 28.0f;

            // 횡경사 발생 시 반대 방향 차등 추력 배분 (Differential Thrust)
            float rollCompRpm = heelError * 250.0f;

            command.LeftEngineRpm = MathF.Clamp(baseRpm - steeringDiffRpm - rollCompRpm, 800.0f, 2600.0f);
            command.RightEngineRpm = MathF.Clamp(baseRpm + steeringDiffRpm + rollCompRpm, 800.0f, 2600.0f);

            command.LeftJetNozzleDeg = MathF.Clamp(targetNozzleAngle, -30.0f, 30.0f);
            command.RightJetNozzleDeg = MathF.Clamp(targetNozzleAngle, -30.0f, 30.0f);

            command.IsHydroplaningActive = isPlaning;
        }
    }
}