대한민국 육군 차기 대포병 탐지 레이더(TPQ-74K, 천경-II) C-밴드 능동위상배열(AESA) 동시 다표적 포탄 추적 & 외탄도학 역추적(Back-Tracking) 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.CounterBatteryRadar
{
    /// <summary>
    /// 레이더 포탄 관측 데이터 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public readonly struct ShellTrackObservation
    {
        public readonly int TrackId;         // 표적 번호
        public readonly float PosX;          // 관측 위치 (m, NED)
        public readonly float PosY;
        public readonly float PosZ;
        public readonly float VelX;          // 관측 속도 (m/s, NED)
        public readonly float VelY;
        public readonly float VelZ;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ShellTrackObservation(int id, float px, float py, float pz, float vx, float vy, float vz)
        {
            TrackId = id; PosX = px; PosY = py; PosZ = pz;
            VelX = vx; VelY = vy; VelZ = vz;
        }
    }

    /// <summary>
    /// 적 발사원점 역추적 결과 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct LaunchOriginResult
    {
        public int TrackId;                  // 표적 ID
        public float LaunchOriginX;          // 적 발사원점 X 좌표 (m)
        public float LaunchOriginY;          // Y 좌표 (m)
        public float LaunchOriginZ;          // 고도 Z (m)
        public float EstimatedMuzzleSpeed;   // 추정 포구초속 (m/s)
        public float EstimatedFiringAngleDeg;// 추정 발사앙각 (deg)
        public float FlightTimeToOriginSec;  // 발사 시점까지의 역적분 시간 (초)
        public bool IsOriginConverged;       // 수치 수렴 성공 여부
    }

    /// <summary>
    /// TPQ-74K 천경-II 외탄도학 역추적 1,000Hz 0-Alloc C# 엔진
    /// </summary>
    public sealed class Cheongyeong2RadarEngine
    {
        private const float Gravity = 9.80665f;
        private const float AirDensitySeaLevel = 1.225f;
        private const float ScaleHeight = 8500.0f; // 기압고도 계수
        private const float ShellMassKg = 45.0f;   // 155mm 표준 탄두 질량
        private const float ShellArea = 0.0188f;   // 기준 단면적 (m^2)
        private const float DragCoeff = 0.28f;     // 표준 마하 2 항력 계수

        public Cheongyeong2RadarEngine() { }

        /// <summary>
        /// 1,000Hz 틱: 관측된 탄도 궤적으로부터 시간 역행 4차 룬게-쿠타 적분으로 발사원점 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void BacktrackLaunchOrigin(
            in ShellTrackObservation obs,
            float terrainHeightM,            // 해당 지역 DEM 평균 지표고도 (NED Z는 음수 상향)
            ref LaunchOriginResult result)
        {
            result.TrackId = obs.TrackId;

            float px = obs.PosX;
            float py = obs.PosY;
            float pz = obs.PosZ; // NED 좌표계에서 지면 고도 0, 상공은 음수

            float vx = obs.VelX;
            float vy = obs.VelY;
            float vz = obs.VelZ;

            float dt = -0.025f; // 25ms 간격 시간 역행 적분
            float totalTime = 0f;
            bool converged = false;

            // 최대 2,000 스텝 (최대 50초 역추적)
            for (int step = 0; step < 2000; step++)
            {
                // 지표면 도달 검사 (NED 좌표계에서 pz >= -terrainHeightM)
                if (pz >= -terrainHeightM)
                {
                    converged = true;
                    break;
                }

                // RK4 적분 가속도 함수
                CalculateAccel(pz, vx, vy, vz, out float ax1, out float ay1, out float az1);

                float pz_mid = pz + 0.5f * dt * vz;
                float vx_mid = vx + 0.5f * dt * ax1;
                float vy_mid = vy + 0.5f * dt * ay1;
                float vz_mid = vz + 0.5f * dt * az1;
                CalculateAccel(pz_mid, vx_mid, vy_mid, vz_mid, out float ax2, out float ay2, out float az2);

                float vx_mid2 = vx + 0.5f * dt * ax2;
                float vy_mid2 = vy + 0.5f * dt * ay2;
                float vz_mid2 = vz + 0.5f * dt * az2;
                CalculateAccel(pz_mid, vx_mid2, vy_mid2, vz_mid2, out float ax3, out float ay3, out float az3);

                float pz_end = pz + dt * vz;
                float vx_end = vx + dt * ax3;
                float vy_end = vy + dt * ay3;
                float vz_end = vz + dt * az3;
                CalculateAccel(pz_end, vx_end, vy_end, vz_end, out float ax4, out float ay4, out float az4);

                // 상태 갱신
                px += (dt / 6.0f) * (vx + 2f * vx_mid + 2f * vx_mid2 + vx_end);
                py += (dt / 6.0f) * (vy + 2f * vy_mid + 2f * vy_mid2 + vy_end);
                pz += (dt / 6.0f) * (vz + 2f * vz_mid + 2f * vz_mid2 + vz_end);

                vx += (dt / 6.0f) * (ax1 + 2f * ax2 + 2f * ax3 + ax4);
                vy += (dt / 6.0f) * (ay1 + 2f * ay2 + 2f * ay3 + ay4);
                vz += (dt / 6.0f) * (az1 + 2f * az2 + 2f * az3 + az4);

                totalTime += -dt;
            }

            // 결과 기록
            float muzzleSpeed = MathF.Sqrt(vx * vx + vy * vy + vz * vz);
            float firingAngleDeg = MathF.Atan2(-vz, MathF.Sqrt(vx * vx + vy * vy)) * (180.0f / MathF.PI);

            result.LaunchOriginX = px;
            result.LaunchOriginY = py;
            result.LaunchOriginZ = pz;
            result.EstimatedMuzzleSpeed = muzzleSpeed;
            result.EstimatedFiringAngleDeg = firingAngleDeg;
            result.FlightTimeToOriginSec = totalTime;
            result.IsOriginConverged = converged;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void CalculateAccel(
            float pz, float vx, float vy, float vz,
            out float ax, out float ay, out float az)
        {
            float altitude = MathF.Max(0f, -pz);
            float rho = AirDensitySeaLevel * MathF.Exp(-altitude / ScaleHeight);
            float vSq = vx * vx + vy * vy + vz * vz;
            float v = MathF.Sqrt(vSq);

            // 공기저항 항력 가속도: a_drag = 0.5 * rho * S * Cd * v / m
            float dragFactor = (0.5f * rho * ShellArea * DragCoeff) / ShellMassKg;

            ax = -dragFactor * v * vx;
            ay = -dragFactor * v * vy;
            az = -dragFactor * v * vz + Gravity; // NED에서 아래쪽이 +Gravity
        }
    }
}