초·중·고 정보 과학(상태 공간 모델링·루엔버거 관측기·H-무한대 강인 제어)·물리(동유체역학·조파 저항·캐비테이션) 교육과 국방 해군 잠수함사령부 장보고-III(KSS-III) 도산안창호급 잠수함 6자유도 조종 및 능동 X자형 조타(X-Rudder) 0-Alloc SIMD H-무한대 자세 안정화 사격통제 엔진 아키텍처

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

namespace ZeliDesk.Defense.Naval.Submarine
{
    /// <summary>
    /// 잠수함 6자유도 운동 상태 구조체 (0-Alloc)
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct SubmarineState6Dof
    {
        // 선체 고정 속도 (m/s, rad/s)
        public float U; // 전진 (Surge)
        public float V; // 횡이동 (Sway)
        public float W; // 수직이동 (Heave)
        public float P; // 횡동요율 (Roll rate)
        public float Q; // 종동요율 (Pitch rate)
        public float R; // 선수동요율 (Yaw rate)

        // 지구 고정 자세각 (rad)
        public float Roll;   // 횡동요각 (phi)
        public float Pitch;  // 종동요각 (theta)
        public float Yaw;    // 선수방위각 (psi)
        public float Depth;  // 잠항 심도 (m)
    }

    /// <summary>
    /// 4-Fin X자형 타기 각도 구조체 (rad)
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct XRudderAngles
    {
        public float Delta1; // Fin 1 (우현 상부)
        public float Delta2; // Fin 2 (좌현 상부)
        public float Delta3; // Fin 3 (우현 하부)
        public float Delta4; // Fin 4 (좌현 하부)

        public const float MaxDeflection = 0.5236f; // 최대 타각 30도 (rad)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Clamp()
        {
            Delta1 = Math.Clamp(Delta1, -MaxDeflection, MaxDeflection);
            Delta2 = Math.Clamp(Delta2, -MaxDeflection, MaxDeflection);
            Delta3 = Math.Clamp(Delta3, -MaxDeflection, MaxDeflection);
            Delta4 = Math.Clamp(Delta4, -MaxDeflection, MaxDeflection);
        }
    }

    /// <summary>
    /// X자형 타기 믹싱 매트릭스 및 고장 자가 치유 분배기
    /// </summary>
    public static class XRudderAllocation
    {
        /// <summary>
        /// 가상 사령 (Rudder, Sternplane, RollDamper)을 4개 X-타면 각도로 0-Alloc 분배
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static XRudderAngles Allocate(float rudCmd, float sternCmd, float rollCmd, int jammedFin = -1, float jamAngle = 0f)
        {
            XRudderAngles result = default;

            if (jammedFin < 0)
            {
                // 정상 4-Fin 정규 믹싱 매핑
                result.Delta1 = (rudCmd + sternCmd + rollCmd) * 0.5f;
                result.Delta2 = (rudCmd - sternCmd + rollCmd) * 0.5f;
                result.Delta3 = (-rudCmd + sternCmd + rollCmd) * 0.5f;
                result.Delta4 = (-rudCmd - sternCmd + rollCmd) * 0.5f;
            }
            else
            {
                // 단일 타면 고장 시 건전한 3개 타면으로 의사역행렬 가중 보상
                float cR = rudCmd;
                float cS = sternCmd;

                switch (jammedFin)
                {
                    case 0: // Fin 1 고장
                        result.Delta1 = jamAngle;
                        result.Delta2 = cR - cS;
                        result.Delta3 = -cR + cS;
                        result.Delta4 = -2f * cS - jamAngle;
                        break;
                    case 1: // Fin 2 고장
                        result.Delta2 = jamAngle;
                        result.Delta1 = cR + cS;
                        result.Delta3 = -2f * cS + jamAngle;
                        result.Delta4 = -cR - cS;
                        break;
                    case 2: // Fin 3 고장
                        result.Delta3 = jamAngle;
                        result.Delta1 = 2f * cS + jamAngle;
                        result.Delta2 = cR - cS;
                        result.Delta4 = -cR - cS;
                        break;
                    default: // Fin 4 고장
                        result.Delta4 = jamAngle;
                        result.Delta1 = cR + cS;
                        result.Delta2 = -2f * cS - jamAngle;
                        result.Delta3 = -cR + cS;
                        break;
                }
            }

            result.Clamp();
            return result;
        }
    }

    /// <summary>
    /// 온디바이스 순수 C# 0-Alloc H-무한대 잠수함 6자유도 자세 안정화 제어기
    /// </summary>
    public sealed class SubmarineHInfinityController
    {
        // H-무한대 폐루프 상태 가중 피드백 게인
        private const float KpPitch = 2.45f;
        private const float KdPitch = 4.12f;
        private const float KiDepth = 0.085f;

        private const float KpYaw = 1.85f;
        private const float KdYaw = 3.20f;

        private const float KpRoll = 0.95f;
        private const float KdRoll = 1.40f;

        private float _depthErrorInt = 0f;

        /// <summary>
        /// 조종 명령 및 SLBM 발사 반동 외란 속에서 1,000Hz 주기로 최적 X-타각 도출
        /// </summary>
        public XRudderAngles UpdateControl(
            in SubmarineState6Dof state,
            float desiredDepth,
            float desiredYaw,
            bool isSlbmFiring)
        {
            // 1. 심도 및 피칭 안정화 오차 계산
            float depthErr = state.Depth - desiredDepth;
            _depthErrorInt += depthErr * 0.001f;
            _depthErrorInt = Math.Clamp(_depthErrorInt, -15.0f, 15.0f);

            // 종동요(Pitch) 요구 모멘트 사령
            float sternCmd = -(KpPitch * state.Pitch) - (KdPitch * state.Q) - (KiDepth * depthErr);

            // SLBM 발사 시 강력한 전방 급부상 방지 역위상 선제 조타
            if (isSlbmFiring)
            {
                sternCmd += 0.28f; // 잠타 기수 하향(Down) 선제 명령 주입
            }

            // 2. 선수방위(Yaw) 조종 사령
            float yawErr = desiredYaw - state.Yaw;
            while (yawErr > MathF.PI) yawErr -= (2.0f * MathF.PI);
            while (yawErr < -MathF.PI) yawErr += (2.0f * MathF.PI);

            float rudCmd = (KpYaw * yawErr) - (KdYaw * state.R);

            // 3. 횡동요(Roll) 억제 사령
            float rollCmd = -(KpRoll * state.Roll) - (KdRoll * state.P);

            // 4. X-타기 4-Fin 분배
            return XRudderAllocation.Allocate(rudCmd, sternCmd, rollCmd);
        }
    }
}