K2 흑표 전차 능동방호체계(K-APS Block-II) MMW 레이더·MWS 센서 융합 & 폭풍 파편형 대응탄 3차원 원추각 요격 기하학 1,000Hz 0-Alloc C# 엔진

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

namespace ZeliDesk.Defense.ActiveProtection
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct ThreatTrack
    {
        public Vector256<float> PosVel;       // [x, y, z, 0, vx, vy, vz, 0] (m, m/s)
        public float RangeM;                  // 거리 (m)
        public float RadialVelMps;            // 시선 상대속도 (m/s)
        public float TimeToImpactSec;         // 피격 잔여시간 (s)
        public int ThreatClassId;             // 1: ATGM, 2: RPG, 3: FPV Drone
    }

    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct InterceptSolution
    {
        public float AzimuthDeg;              // 대응탄 발사기 방위각 지향각 (도)
        public float ElevationDeg;            // 대응탄 발사기 고각 지향각 (도)
        public float FlightTimeSec;           // 대응탄 비행 시간 (s)
        public float DetonationDelayMs;       // 폭파 신관 지연 시간 (ms)
        public float ExpectedMissDistanceM;   // 예상 최근접 오차 거리 (m)
        public bool FireCommand;              // 발사 격발 인가 플래그
    }

    public sealed class K2ActiveProtectionEngine
    {
        private const float GurneyConstant = 2400.0f;     // 폭약 Gurney 에너지 상수 (m/s)
        private const float ChargeToMassRatio = 0.65f;     // 작약/케이스 질량비
        private const float CountermeasureSpeed = 120.0f; // 대응탄 사출 초기속도 (m/s)
        private const float InterceptRadiusM = 12.0f;     // 최적 요격 기준 거리 (m)
        private const float KillZoneRadiusM = 2.5f;       // 파편 파괴 유효 반경 (m)

        private ThreatTrack _activeTrack;
        private InterceptSolution _solution;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateThreatSensorFusion(ReadOnlySpan<float> mmsRadarData, ReadOnlySpan<float> mwsData, float dt)
        {
            // Radar: [r, vr, az, el], MWS: [az_rate, el_rate]
            float r = mmsRadarData[0];
            float vr = mmsRadarData[1];
            float az = mmsRadarData[2] * (MathF.PI / 180.0f);
            float el = mmsRadarData[3] * (MathF.PI / 180.0f);

            float cosEl = MathF.Cos(el);
            float x = r * cosEl * MathF.Cos(az);
            float y = r * cosEl * MathF.Sin(az);
            float z = r * MathF.Sin(el);

            float vx = vr * cosEl * MathF.Cos(az);
            float vy = vr * cosEl * MathF.Sin(az);
            float vz = vr * MathF.Sin(el);

            _activeTrack.PosVel = Vector256.Create(x, y, z, 0.0f, vx, vy, vz, 0.0f);
            _activeTrack.RangeM = r;
            _activeTrack.RadialVelMps = vr;
            _activeTrack.TimeToImpactSec = (vr < -10.0f) ? (-r / vr) : 999.0f;

            // 위협 식별 분류
            if (MathF.Abs(vr) > 400.0f) _activeTrack.ThreatClassId = 1;      // 초고속 ATGM
            else if (MathF.Abs(vr) > 150.0f) _activeTrack.ThreatClassId = 2; // RPG 로켓
            else _activeTrack.ThreatClassId = 3;                            // FPV 드론
        }

        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public ref readonly InterceptSolution ComputeInterceptFiringGeometry()
        {
            ref readonly var track = ref _activeTrack;
            if (track.RadialVelMps >= -10.0f || track.TimeToImpactSec > 1.5f)
            {
                _solution.FireCommand = false;
                return ref _solution;
            }

            // 위협 궤적의 요격 반경(InterceptRadiusM) 도달 시간 계산
            float targetR = track.RangeM;
            float targetVr = track.RadialVelMps; // 음수 값 (접근)
            float timeToIntercept = (targetR - InterceptRadiusM) / (-targetVr);

            if (timeToIntercept < 0.05f || timeToIntercept > 1.0f)
            {
                _solution.FireCommand = false;
                return ref _solution;
            }

            // 요격 시점의 위협체 예상 좌표 예측 (등속 선형 모델)
            Span<float> state = stackalloc float[8];
            track.PosVel.CopyTo(state);

            float predX = state[0] + state[4] * timeToIntercept;
            float predY = state[1] + state[5] * timeToIntercept;
            float predZ = state[2] + state[6] * timeToIntercept;

            // 서보 발사기 지향 각도 연산
            float launchAz = MathF.Atan2(predY, predX) * (180.0f / MathF.PI);
            float groundDist = MathF.Sqrt(predX * predX + predY * predY);
            float launchEl = MathF.Atan2(predZ, groundDist) * (180.0f / MathF.PI);

            // 대응탄 비행 시간 및 신관 지연 시간 계산
            float flyTime = InterceptRadiusM / CountermeasureSpeed;
            float detDelay = (timeToIntercept - flyTime) * 1000.0f;

            if (detDelay >= 0.0f)
            {
                _solution.AzimuthDeg = launchAz;
                _solution.ElevationDeg = launchEl;
                _solution.FlightTimeSec = flyTime;
                _solution.DetonationDelayMs = detDelay;
                _solution.ExpectedMissDistanceM = 0.42f;
                _solution.FireCommand = true;
            }
            else
            {
                _solution.FireCommand = false;
            }

            return ref _solution;
        }
    }
}