MUM-T 공중 유무인복합 공중급유(AAR) 붐 방식 스테레오 비전 광학 추적 및 플라잉 붐 능동 안정화 서보 아키텍처 (.NET 9 C# SIMD)

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

namespace ZeliDesk.Defense.Avionics
{
    /// <summary>
    /// 자율 공중급유(AAR) 플라잉 붐 스테레오 비전 추적 및 루디베이터 능동 서보
    /// GC 제로 할당 ref struct 및 AVX-256 SIMD 가속화
    /// </summary>
    public ref struct AerialRefuelingBoomServo
    {
        private readonly ReadOnlySpan<float> _errorY;           // 횡방향 상대 오차 e_y (m)
        private readonly ReadOnlySpan<float> _errorZ;           // 수직방향 상대 오차 e_z (m)
        private readonly ReadOnlySpan<float> _velocityErrorY;   // 횡방향 오차 변화율 de_y/dt (m/s)
        private readonly ReadOnlySpan<float> _velocityErrorZ;   // 수직방향 오차 변화율 de_z/dt (m/s)
        private readonly Span<float> _leftRuddevatorCmd;        // 좌측 루디베이터 각도 지령 (deg)
        private readonly Span<float> _rightRuddevatorCmd;       // 우측 루디베이터 각도 지령 (deg)

        private readonly float _kpY; // 횡방향 P 게인
        private readonly float _kdY; // 횡방향 D 게인
        private readonly float _kpZ; // 수직방향 P 게인
        private readonly float _kdZ; // 수직방향 D 게인
        private readonly float _maxDeflection; // 최대 타각 제한 (deg)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public AerialRefuelingBoomServo(
            ReadOnlySpan<float> errorY,
            ReadOnlySpan<float> errorZ,
            ReadOnlySpan<float> velocityErrorY,
            ReadOnlySpan<float> velocityErrorZ,
            Span<float> leftRuddevatorCmd,
            Span<float> rightRuddevatorCmd,
            float kpY = 18.5f,
            float kdY = 4.2f,
            float kpZ = 22.0f,
            float kdZ = 5.0f,
            float maxDeflection = 25.0f)
        {
            _errorY = errorY;
            _errorZ = errorZ;
            _velocityErrorY = velocityErrorY;
            _velocityErrorZ = velocityErrorZ;
            _leftRuddevatorCmd = leftRuddevatorCmd;
            _rightRuddevatorCmd = rightRuddevatorCmd;
            _kpY = kpY;
            _kdY = kdY;
            _kpZ = kpZ;
            _kdZ = kdZ;
            _maxDeflection = maxDeflection;
        }

        /// <summary>
        /// 8개 마커의 3D 추적 오차를 SIMD 병렬 분석하여 루디베이터 믹싱 연산
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void ProcessBoomFlightControl()
        {
            int length = _errorY.Length;
            int simdLength = length - (length % 8);

            Vector256<float> vKpY = Vector256.Create(_kpY);
            Vector256<float> vKdY = Vector256.Create(_kdY);
            Vector256<float> vKpZ = Vector256.Create(_kpZ);
            Vector256<float> vKdZ = Vector256.Create(_kdZ);
            Vector256<float> vMax = Vector256.Create(_maxDeflection);
            Vector256<float> vMin = Vector256.Create(-_maxDeflection);

            int i = 0;
            if (Avx2.IsSupported)
            {
                for (; i < simdLength; i += 8)
                {
                    Vector256<float> vEy = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_errorY.Slice(i, 8))[0]);
                    Vector256<float> vEz = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_errorZ.Slice(i, 8))[0]);
                    Vector256<float> vDy = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_velocityErrorY.Slice(i, 8))[0]);
                    Vector256<float> vDz = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_velocityErrorZ.Slice(i, 8))[0]);

                    // Rudder cmd: delta_r = KpY * Ey + KdY * Dy
                    Vector256<float> vDeltaR = Avx.Add(Avx.Multiply(vKpY, vEy), Avx.Multiply(vKdY, vDy));
                    // Elevator cmd: delta_e = KpZ * Ez + KdZ * Dz
                    Vector256<float> vDeltaE = Avx.Add(Avx.Multiply(vKpZ, vEz), Avx.Multiply(vKdZ, vDz));

                    // V-Tail mixing: delta_L = delta_e + delta_r, delta_R = delta_e - delta_r
                    Vector256<float> vLeft = Avx.Add(vDeltaE, vDeltaR);
                    Vector256<float> vRight = Avx.Subtract(vDeltaE, vDeltaR);

                    // Clamp to max deflection
                    Vector256<float> vLeftClamped = Avx.Min(vMax, Avx.Max(vMin, vLeft));
                    Vector256<float> vRightClamped = Avx.Min(vMax, Avx.Max(vMin, vRight));

                    vLeftClamped.CopyTo(_leftRuddevatorCmd.Slice(i, 8));
                    vRightClamped.CopyTo(_rightRuddevatorCmd.Slice(i, 8));
                }
            }

            for (; i < length; i++)
            {
                float deltaR = (_kpY * _errorY[i]) + (_kdY * _velocityErrorY[i]);
                float deltaE = (_kpZ * _errorZ[i]) + (_kdZ * _velocityErrorZ[i]);

                float left = Math.Clamp(deltaE + deltaR, -_maxDeflection, _maxDeflection);
                float right = Math.Clamp(deltaE - deltaR, -_maxDeflection, _maxDeflection);

                _leftRuddevatorCmd[i] = left;
                _rightRuddevatorCmd[i] = right;
            }
        }
    }
}