해상 무인수상정(USV) 국제해상충돌예방규칙(COLREGs) 준수 동적 장애물 회피 비선형 모델예측제어(NMPC) 서보 아키텍처 (.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.Maritime
{
    /// <summary>
    /// 무인수상정(USV) COLREGs 준수 동적 장애물 회피 NMPC 서보
    /// GC 무할당 ref struct 및 AVX-256 SIMD 가속화
    /// </summary>
    public ref struct UsvColregsMpcServo
    {
        private readonly ReadOnlySpan<float> _predPosX;       // 예측 경로 X 좌표 (m)
        private readonly ReadOnlySpan<float> _predPosY;       // 예측 경로 Y 좌표 (m)
        private readonly ReadOnlySpan<float> _targetDistances;// 타겟 장애물 상대 거리 (m)
        private readonly Span<float> _colregsPenalties;       // 산출된 COLREGs 위험도 비용
        private readonly Span<float> _rudderCommands;         // 최적 방향타 조타각 (deg)

        private readonly float _safeRadius;     // 안전 반경 (m)
        private readonly float _colregsWeight;  // 규칙 준수 페널티 가중치
        private readonly float _maxRudder;      // 최대 조타각 제한 (deg)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UsvColregsMpcServo(
            ReadOnlySpan<float> predPosX,
            ReadOnlySpan<float> predPosY,
            ReadOnlySpan<float> targetDistances,
            Span<float> colregsPenalties,
            Span<float> rudderCommands,
            float safeRadius = 50.0f,
            float colregsWeight = 250.0f,
            float maxRudder = 35.0f)
        {
            _predPosX = predPosX;
            _predPosY = predPosY;
            _targetDistances = targetDistances;
            _colregsPenalties = colregsPenalties;
            _rudderCommands = rudderCommands;
            _safeRadius = safeRadius;
            _colregsWeight = colregsWeight;
            _maxRudder = maxRudder;
        }

        /// <summary>
        /// 예측 궤적 상의 COLREGs 페널티 및 방향타 명령을 SIMD 8개 병렬 스텝으로 연산
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void SolveMpcStep()
        {
            int length = _targetDistances.Length;
            int simdLength = length - (length % 8);

            Vector256<float> vSafeRad = Vector256.Create(_safeRadius);
            Vector256<float> vWeight = Vector256.Create(_colregsWeight);
            Vector256<float> vZero = Vector256<float>.Zero;
            Vector256<float> vMaxRud = Vector256.Create(_maxRudder);
            Vector256<float> vMinRud = Vector256.Create(-_maxRudder);

            int i = 0;
            if (Avx2.IsSupported)
            {
                for (; i < simdLength; i += 8)
                {
                    Vector256<float> vDist = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_targetDistances.Slice(i, 8))[0]);
                    Vector256<float> vX = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_predPosX.Slice(i, 8))[0]);
                    Vector256<float> vY = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_predPosY.Slice(i, 8))[0]);

                    // 거리 침범량: Breach = max(0, safeRad - dist)
                    Vector256<float> vBreach = Avx.Max(vZero, Avx.Subtract(vSafeRad, vDist));

                    // 페널티 = weight * (Breach / safeRad)^2
                    Vector256<float> vNormBreach = Avx.Divide(vBreach, vSafeRad);
                    Vector256<float> vPenalty = Avx.Multiply(vWeight, Avx.Multiply(vNormBreach, vNormBreach));

                    // 우현 조타 유도 (COLREGs Rule 14, 15: starboard turn > 0)
                    // Rudder = clamp(penalty * 0.25f, -maxRud, maxRud)
                    Vector256<float> vRawRudder = Avx.Multiply(vPenalty, Vector256.Create(0.25f));
                    Vector256<float> vClampedRudder = Avx.Min(vMaxRud, Avx.Max(vMinRud, vRawRudder));

                    vPenalty.CopyTo(_colregsPenalties.Slice(i, 8));
                    vClampedRudder.CopyTo(_rudderCommands.Slice(i, 8));
                }
            }

            for (; i < length; i++)
            {
                float breach = MathF.Max(0.0f, _safeRadius - _targetDistances[i]);
                float normBreach = breach / _safeRadius;
                float penalty = _colregsWeight * normBreach * normBreach;
                float rawRudder = penalty * 0.25f;
                _colregsPenalties[i] = penalty;
                _rudderCommands[i] = MathF.Min(_maxRudder, MathF.Max(-_maxRudder, rawRudder));
            }
        }
    }
}