국방 미래 전술 도심항공모빌리티(Tactical eVTOL) 틸트로터(Tilt-Rotor) 천이 비행(Transition Flight) & 분산 전기추진(DEP) 0-Alloc SIMD 비행제어 온디바이스 C# 엔진

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

namespace ZeliDesk.Defense.TacticalEvtol
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct EvtolFlightState
    {
        public Vector256<float> PosVel;       // [x, y, z, 0, u, v, w, 0] (m, m/s)
        public Vector256<float> AnglesRate;   // [phi, theta, psi, 0, p, q, r, 0] (rad, rad/s)
        public Vector256<float> DepRpm;       // 8개 분산 추진 모터 RPM (0~3500)
        public float NacelleAngleDeg;         // 나셀 틸트각 (90도: 수직, 0도: 수평)
        public float Airspeed;                // 대기속도 (m/s)
        public float TransitionProgress;      // 0.0 (호버) ~ 1.0 (순항)
    }

    public sealed class TiltRotorTransitionFlightEngine
    {
        private const float Mass = 2400.0f;           // 공허 + 임무 탑재 중량 (kg)
        private const float Ixx = 4800.0f;            // 롤 관성 (kg*m^2)
        private const float Iyy = 3200.0f;            // 피치 관성 (kg*m^2)
        private const float Izz = 6500.0f;            // 요 관성 (kg*m^2)
        private const float Gravity = 9.80665f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public void StepTransition1000Hz(
            ref EvtolFlightState state,
            in Vector256<float> attitudeCmd, // [phi_cmd, theta_cmd, psi_cmd, alt_cmd]
            float targetNacelleDeg,
            float dt)
        {
            // 1. 대기속도 및 천이 진척도 계산
            float u = state.PosVel.GetElement(4);
            float v = state.PosVel.GetElement(5);
            float w = state.PosVel.GetElement(6);
            float vAir = MathF.Sqrt(u * u + v * v + w * w + 1e-4f);
            state.Airspeed = vAir;

            // 천이 블렌딩 가중치 (15 m/s ~ 50 m/s)
            float blendWeight = Math.Clamp((vAir - 15.0f) / 35.0f, 0.0f, 1.0f);
            state.TransitionProgress = blendWeight;

            // 2. 나셀 틸트 서보 추종 (최대 10 deg/s 속도 제한)
            float nacelleRateLimit = 10.0f * dt;
            float nacelleDelta = Math.Clamp(targetNacelleDeg - state.NacelleAngleDeg, -nacelleRateLimit, nacelleRateLimit);
            state.NacelleAngleDeg += nacelleDelta;

            float nacelleRad = state.NacelleAngleDeg * (MathF.PI / 180.0f);
            float cosNac = MathF.Cos(nacelleRad);
            float sinNac = MathF.Sin(nacelleRad);

            // 3. 자세 오차 및 각속도 댐핑 피드백
            float phi = state.AnglesRate.GetElement(0);
            float theta = state.AnglesRate.GetElement(1);
            float psi = state.AnglesRate.GetElement(2);
            float p = state.AnglesRate.GetElement(4);
            float q = state.AnglesRate.GetElement(5);
            float r = state.AnglesRate.GetElement(6);

            float phiCmd = attitudeCmd.GetElement(0);
            float thetaCmd = attitudeCmd.GetElement(1);
            float altCmd = attitudeCmd.GetElement(3);

            float errRoll = phiCmd - phi;
            float errPitch = thetaCmd - theta;
            float errAlt = altCmd - (-state.PosVel.GetElement(2)); // NED 고도

            // 요구 제어 모멘트 (PD 제어)
            float rollTorqueReq  = 8500.0f * errRoll - 1200.0f * p;
            float pitchTorqueReq = 11000.0f * errPitch - 1500.0f * q;
            float totalThrustReq = Mass * (Gravity + 2.5f * errAlt - 1.8f * w) / MathF.Max(sinNac, 0.25f);

            // 4. 분산 전기추진(DEP) 8모터 제어 배분 (Control Allocation)
            // 좌측 4모터(1,2,3,4), 우측 4모터(5,6,7,8)
            Span<float> motorRpm = stackalloc float[8];
            float baseRpm = Math.Clamp(MathF.Sqrt(MathF.Max(totalThrustReq / 8.0f * 150.0f, 0f)), 800.0f, 3200.0f);

            // 롤 모멘트: 좌우 모터 차동 RPM (호버 및 저속)
            float deltaRpmRoll = (1.0f - blendWeight) * (rollTorqueReq / 12.0f);
            // 피치 모멘트: 전후 모터 차동 RPM
            float deltaRpmPitch = (1.0f - blendWeight) * (pitchTorqueReq / 16.0f);

            for (int i = 0; i < 8; i++)
            {
                bool isLeft = (i < 4);
                bool isFront = (i % 2 == 0);

                float rpm = baseRpm;
                rpm += isLeft ? -deltaRpmRoll : deltaRpmRoll;
                rpm += isFront ? deltaRpmPitch : -deltaRpmPitch;

                motorRpm[i] = Math.Clamp(rpm, 500.0f, 3500.0f);
            }

            // SIMD 벡터로 모터 상태 일괄 갱신 (0-Alloc)
            state.DepRpm = Vector256.Create(
                motorRpm[0], motorRpm[1], motorRpm[2], motorRpm[3],
                motorRpm[4], motorRpm[5], motorRpm[6], motorRpm[7]);

            // 5. 가속도 및 상태 적분 (단순화된 병진 가속도)
            float accelX = (totalThrustReq * cosNac) / Mass;
            float accelZ = (totalThrustReq * sinNac) / Mass - Gravity;

            float newU = u + accelX * dt;
            float newW = w + accelZ * dt;
            float newX = state.PosVel.GetElement(0) + newU * dt;
            float newZ = state.PosVel.GetElement(2) + newW * dt;

            state.PosVel = Vector256.Create(newX, state.PosVel.GetElement(1), newZ, 0f, newU, v, newW, 0f);
        }
    }
}