차세대 스마트 항공모함 비행갑판 유무인기 자율 유도 견인차량(Autonomous Tow Tractor) 비주얼 라이다 SLAM 및 파킹 잭 서보 아키텍처 (.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.NavalAviation
{
    /// <summary>
    /// 비행갑판 자율 견인 트랙터 비주얼 라이다 SLAM 및 유압 잭 서보
    /// GC 제로 할당 ref struct 및 AVX-256 SIMD 가속화
    /// </summary>
    public ref struct DeckTowTractorSlamServo
    {
        private readonly ReadOnlySpan<float> _lateralErrors;     // 경로 횡오차 e_y (m)
        private readonly ReadOnlySpan<float> _headingErrors;     // 헤딩 방위 오차 e_psi (rad)
        private readonly ReadOnlySpan<float> _deckTiltRolls;     // 함정 롤 경사각 (rad)
        private readonly Span<float> _steeringAngleCommands;     // 아커만 조향각 지령 (deg)
        private readonly Span<float> _hydraulicJackDisplacements;// 유압 잭 변위 보정 (mm)

        private readonly float _wheelBase;     // 축간 거리 L_t (m)
        private readonly float _steeringGain;  // 횡오차 조향 게인
        private readonly float _headingGain;   // 방위오차 조향 게인
        private readonly float _tiltJackGain;  // 경사 보정 유압 게인 (mm/rad)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DeckTowTractorSlamServo(
            ReadOnlySpan<float> lateralErrors,
            ReadOnlySpan<float> headingErrors,
            ReadOnlySpan<float> deckTiltRolls,
            Span<float> steeringAngleCommands,
            Span<float> hydraulicJackDisplacements,
            float wheelBase = 2.8f,
            float steeringGain = 14.5f,
            float headingGain = 28.0f,
            float tiltJackGain = 120.0f)
        {
            _lateralErrors = lateralErrors;
            _headingErrors = headingErrors;
            _deckTiltRolls = deckTiltRolls;
            _steeringAngleCommands = steeringAngleCommands;
            _hydraulicJackDisplacements = hydraulicJackDisplacements;
            _wheelBase = wheelBase;
            _steeringGain = steeringGain;
            _headingGain = headingGain;
            _tiltJackGain = tiltJackGain;
        }

        /// <summary>
        /// 8채널 센서 데이터를 SIMD 병렬 분석하여 조향각 및 잭 변위 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void ExecuteSteeringJackLoop()
        {
            int length = _lateralErrors.Length;
            int simdLength = length - (length % 8);

            Vector256<float> vKlat = Vector256.Create(_steeringGain);
            Vector256<float> vKpsi = Vector256.Create(_headingGain);
            Vector256<float> vKjack = Vector256.Create(_tiltJackGain);
            Vector256<float> vMaxSteer = Vector256.Create(40.0f);
            Vector256<float> vMinSteer = Vector256.Create(-40.0f);

            int i = 0;
            if (Avx2.IsSupported)
            {
                for (; i < simdLength; i += 8)
                {
                    Vector256<float> vEy = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_lateralErrors.Slice(i, 8))[0]);
                    Vector256<float> vEpsi = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_headingErrors.Slice(i, 8))[0]);
                    Vector256<float> vRoll = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_deckTiltRolls.Slice(i, 8))[0]);

                    // 조향 명령 delta = Klat * Ey + Kpsi * Epsi
                    Vector256<float> vSteer = Avx.Add(Avx.Multiply(vKlat, vEy), Avx.Multiply(vKpsi, vEpsi));
                    Vector256<float> vClampedSteer = Avx.Min(vMaxSteer, Avx.Max(vMinSteer, vSteer));

                    // 유압 잭 변위 = Kjack * Roll
                    Vector256<float> vJack = Avx.Multiply(vKjack, vRoll);

                    vClampedSteer.CopyTo(_steeringAngleCommands.Slice(i, 8));
                    vJack.CopyTo(_hydraulicJackDisplacements.Slice(i, 8));
                }
            }

            for (; i < length; i++)
            {
                float steer = (_steeringGain * _lateralErrors[i]) + (_headingGain * _headingErrors[i]);
                _steeringAngleCommands[i] = Math.Clamp(steer, -40.0f, 40.0f);
                _hydraulicJackDisplacements[i] = _tiltJackGain * _deckTiltRolls[i];
            }
        }
    }
}