해상초계 무인기(MPA-UAV) 자기 이상 탐지(MAD) 잠수함 유기 자기장 왜곡 추적 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 자기 이상 추적 서보 엔진

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

namespace ZeliDesk.Defense.AntiSubmarine.MAD
{
    public readonly ref struct MadState
    {
        public readonly float TargetDipoleStrength;
        public readonly float CleanedAnomalyNt;
        public readonly float EstimatedDepthMeters;
        public readonly bool AnomalyDetected;

        public MadState(float strength, float anomaly, float depth, bool detected)
        {
            TargetDipoleStrength = strength;
            CleanedAnomalyNt = anomaly;
            EstimatedDepthMeters = depth;
            AnomalyDetected = detected;
        }
    }

    public sealed class SubSurfaceMadServoEngine
    {
        // 톨스-로슨 18계수 보정 가중치
        private readonly float[] _tlCoefficients = new float[18];
        private readonly float[] _uBasis = new float[18];

        private float _prevUx, _prevUy, _prevUz;
        private const float SampleRateHz = 20000.0f;
        private const float Dt = 1.0f / SampleRateHz;
        private const float DetectionThresholdNt = 0.12f;

        public SubSurfaceMadServoEngine()
        {
            // 기본 기체 영구/유도 자기 계수 초기화 (테스트 캘리브레이션 값)
            _tlCoefficients[0] = 12.5f;  // Perm X
            _tlCoefficients[1] = -8.3f;  // Perm Y
            _tlCoefficients[2] = 24.1f;  // Perm Z
            for (int i = 3; i < 18; i++) _tlCoefficients[i] = 0.05f * (i - 10);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public unsafe MadState ProcessMagnetometerSample(
            float rawTotalFieldNt,
            float earthFieldNt,
            float cosAlpha, float cosBeta, float cosGamma,
            float flightAltitudeMeters)
        {
            // 1. 방향 코사인 및 각속도 미분 계산
            float dUx = (cosAlpha - _prevUx) * SampleRateHz;
            float dUy = (cosBeta - _prevUy) * SampleRateHz;
            float dUz = (cosGamma - _prevUz) * SampleRateHz;

            _prevUx = cosAlpha;
            _prevUy = cosBeta;
            _prevUz = cosGamma;

            // 2. 18개 톨스-로슨 기저 벡터 구성
            _uBasis[0] = cosAlpha;
            _uBasis[1] = cosBeta;
            _uBasis[2] = cosGamma;

            _uBasis[3] = cosAlpha * cosAlpha * earthFieldNt;
            _uBasis[4] = cosAlpha * cosBeta * earthFieldNt;
            _uBasis[5] = cosAlpha * cosGamma * earthFieldNt;
            _uBasis[6] = cosBeta * cosBeta * earthFieldNt;
            _uBasis[7] = cosBeta * cosGamma * earthFieldNt;
            _uBasis[8] = cosGamma * cosGamma * earthFieldNt;

            _uBasis[9] = cosAlpha * dUx * earthFieldNt;
            _uBasis[10] = cosAlpha * dUy * earthFieldNt;
            _uBasis[11] = cosAlpha * dUz * earthFieldNt;
            _uBasis[12] = cosBeta * dUx * earthFieldNt;
            _uBasis[13] = cosBeta * dUy * earthFieldNt;
            _uBasis[14] = cosBeta * dUz * earthFieldNt;
            _uBasis[15] = cosGamma * dUx * earthFieldNt;
            _uBasis[16] = cosGamma * dUy * earthFieldNt;
            _uBasis[17] = cosGamma * dUz * earthFieldNt;

            // 3. AVX2 SIMD 기반 18차원 내적 연산으로 기체 간섭 자기장 계산
            float aircraftInterferenceNt = 0.0f;

            if (Avx2.IsSupported)
            {
                fixed (float* pBasis = _uBasis)
                fixed (float* pCoeff = _tlCoefficients)
                {
                    Vector256 sumVec = Vector256.Zero;

                    // 8 floats * 2 = 16 floats
                    for (int i = 0; i < 16; i += 8)
                    {
                        var vB = Avx.LoadVector256(pBasis + i);
                        var vC = Avx.LoadVector256(pCoeff + i);
                        sumVec = Avx.Add(sumVec, Avx.Multiply(vB, vC));
                    }

                    // 수평 합산
                    var vLow = sumVec.GetLower();
                    var vHigh = sumVec.GetUpper();
                    var vSum128 = Sse.Add(vLow, vHigh);
                    vSum128 = Sse3.HorizontalAdd(vSum128, vSum128);
                    vSum128 = Sse3.HorizontalAdd(vSum128, vSum128);
                    aircraftInterferenceNt = vSum128.GetElement(0);

                    // 나머지 2개 요소 스칼라 가산
                    aircraftInterferenceNt += pBasis[16] * pCoeff[16] + pBasis[17] * pCoeff[17];
                }
            }
            else
            {
                for (int i = 0; i < 18; i++)
                {
                    aircraftInterferenceNt += _uBasis[i] * _tlCoefficients[i];
                }
            }

            // 4. 순수 잠수함 신호 추출: DeltaB = Raw - (Earth + Aircraft)
            float cleanedAnomalyNt = rawTotalFieldNt - (earthFieldNt + aircraftInterferenceNt);
            float absAnomaly = MathF.Abs(cleanedAnomalyNt);

            bool isDetected = absAnomaly >= DetectionThresholdNt;

            // 5. 쌍극자 역산 추정: 거리 r ~ (M / DeltaB)^(1/3)
            // 잠수함 공칭 자기모멘트 M = 1.0e8 A*m^2 가정 시 거리 추정
            float estimatedRange = MathF.Pow(1.0e8f * 1.0e-7f / MathF.Max(absAnomaly * 1.0e-9f, 1.0e-12f), 1.0f / 3.0f);
            float estimatedDepth = MathF.Max(0.0f, estimatedRange - flightAltitudeMeters);

            return new MadState(
                strength: absAnomaly,
                anomaly: cleanedAnomalyNt,
                depth: estimatedDepth,
                detected: isDetected
            );
        }
    }
}