MUM-T 유무인복합 자폭/정찰 드론 군집 인터페로메트리 RF 전파원 방위탐지(DF) 삼각측량 서보 아키텍처 (.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.ElectronicWarfare
{
    /// <summary>
    /// 군집 드론 위상 간섭계 RF 방위탐지(DF) 및 삼각측량 보정 서보
    /// GC 무할당 ref struct 및 AVX-256 SIMD 가속화
    /// </summary>
    public ref struct SwarmRfDirectionFindingServo
    {
        private readonly ReadOnlySpan<float> _phaseDifferences; // 안테나 간 위상차 DeltaPhi (rad)
        private readonly ReadOnlySpan<float> _droneYawAngles;     // 드론 기수각 Yaw (rad)
        private readonly Span<float> _azimuthAngles;            // 계산된 표적 진방위각 Azimuth (rad)
        private readonly Span<float> _bearingVectorsX;          // 도출된 방위 벡터 X 성분
        private readonly Span<float> _bearingVectorsY;          // 도출된 방위 벡터 Y 성분

        private readonly float _invKDistance; // lambda / (2*pi*d)
        private readonly float _maxSinVal;    // 클램핑 한계 (0.999f)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SwarmRfDirectionFindingServo(
            ReadOnlySpan<float> phaseDifferences,
            ReadOnlySpan<float> droneYawAngles,
            Span<float> azimuthAngles,
            Span<float> bearingVectorsX,
            Span<float> bearingVectorsY,
            float waveLength = 0.03f, // 10 GHz RF
            float baselineDistance = 0.05f) // 5 cm 기선
        {
            _phaseDifferences = phaseDifferences;
            _droneYawAngles = droneYawAngles;
            _azimuthAngles = azimuthAngles;
            _bearingVectorsX = bearingVectorsX;
            _bearingVectorsY = bearingVectorsY;
            _invKDistance = waveLength / (2.0f * MathF.PI * baselineDistance);
            _maxSinVal = 0.999f;
        }

        /// <summary>
        /// 8채널의 위상차 및 자세 각도를 SIMD 병렬 연산하여 진방위각 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void ComputeDirectionOfArrival()
        {
            int length = _phaseDifferences.Length;
            int simdLength = length - (length % 8);

            Vector256<float> vInvKd = Vector256.Create(_invKDistance);
            Vector256<float> vMax = Vector256.Create(_maxSinVal);
            Vector256<float> vMin = Vector256.Create(-_maxSinVal);

            int i = 0;
            if (Avx2.IsSupported)
            {
                for (; i < simdLength; i += 8)
                {
                    Vector256<float> vDeltaPhi = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_phaseDifferences.Slice(i, 8))[0]);
                    Vector256<float> vYaw = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_droneYawAngles.Slice(i, 8))[0]);

                    // sin(theta_rel) = DeltaPhi * (lambda / (2*pi*d))
                    Vector256<float> vSin = Avx.Multiply(vDeltaPhi, vInvKd);
                    Vector256<float> vClampedSin = Avx.Min(vMax, Avx.Max(vMin, vSin));

                    // 아크사인 테일러 3차 근사: asin(x) ~ x + (x^3)/6
                    Vector256<float> vSin3 = Avx.Multiply(Avx.Multiply(vClampedSin, vClampedSin), vClampedSin);
                    Vector256<float> vRelAngle = Avx.Add(vClampedSin, Avx.Multiply(vSin3, Vector256.Create(0.166667f)));

                    // 진방위각 Azimuth = Yaw + RelAngle
                    Vector256<float> vAzimuth = Avx.Add(vYaw, vRelAngle);

                    // 벡터 성분 근사: cos(az) ~ 1 - az^2/2, sin(az) ~ az - az^3/6
                    Vector256<float> vAz2 = Avx.Multiply(vAzimuth, vAzimuth);
                    Vector256<float> vCos = Avx.Subtract(Vector256.Create(1.0f), Avx.Multiply(vAz2, Vector256.Create(0.5f)));
                    Vector256<float> vSinAz = Avx.Subtract(vAzimuth, Avx.Multiply(Avx.Multiply(vAz2, vAzimuth), Vector256.Create(0.166667f)));

                    vAzimuth.CopyTo(_azimuthAngles.Slice(i, 8));
                    vCos.CopyTo(_bearingVectorsX.Slice(i, 8));
                    vSinAz.CopyTo(_bearingVectorsY.Slice(i, 8));
                }
            }

            for (; i < length; i++)
            {
                float sinVal = Math.Clamp(_phaseDifferences[i] * _invKDistance, -_maxSinVal, _maxSinVal);
                float relAngle = MathF.Asin(sinVal);
                float azimuth = _droneYawAngles[i] + relAngle;
                _azimuthAngles[i] = azimuth;
                _bearingVectorsX[i] = MathF.Cos(azimuth);
                _bearingVectorsY[i] = MathF.Sin(azimuth);
            }
        }
    }
}