대한민국 해군 정조대왕급 이지스함 통합소나체계(ISS) 바이스타틱(Bistatic) 음향 핑 동기화 및 1,000Hz 0-Alloc C# 타원 궤적 잠수함 위치 국지화 엔진

using System;
using System.Numerics;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.Navy.BistaticSonar
{
    public readonly record struct BistaticAcousticPing(
        Vector3 TransmitterPositionMeters, // 함수 바우소나 위치
        Vector3 ReceiverPositionMeters,    // 함미 예인소나 MFTA 위치
        float PropagationTimeSeconds,       // 총 측정 전파시간
        float ReceiverBearingDeg,          // MFTA 수신 방위각
        float ReceiverElevationDeg,        // MFTA 수신 고각
        float WaterSoundSpeedMps           // 수중 음속 (~1500 m/s)
    );

    public ref struct SubmarineFixOutput
    {
        public Vector3 EstimatedSubPosition;
        public float TargetRangeFromShipMeters;
        public float LocalizationResidualMeters;
        public bool TrackLocked;
    }

    public static class JeongjoBistaticCore
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void LocalizeSubmarine(
            ref readonly BistaticAcousticPing ping,
            ref SubmarineFixOutput output)
        {
            float totalPathLength = ping.WaterSoundSpeedMps * ping.PropagationTimeSeconds;
            float baseline = Vector3.Distance(ping.TransmitterPositionMeters, ping.ReceiverPositionMeters);

            // 타원 형성 불능 기하 조건 예외 처리
            if (totalPathLength <= baseline + 5.0f)
            {
                output.TrackLocked = false;
                return;
            }

            // 1. 수신 빔 방향 단위벡터 산출
            float azimRad = ping.ReceiverBearingDeg * (MathF.PI / 180.0f);
            float elevRad = ping.ReceiverElevationDeg * (MathF.PI / 180.0f);

            Vector3 beamDir = new Vector3(
                MathF.Cos(elevRad) * MathF.Cos(azimRad),
                MathF.Cos(elevRad) * MathF.Sin(azimRad),
                MathF.Sin(elevRad)
            );

            // 2. 수신기로부터 표적까지의 거리 R_rx 해석적 산출
            // |x_rx + R_rx * beamDir - x_tx| + R_rx = totalPathLength
            Vector3 dPos = ping.ReceiverPositionMeters - ping.TransmitterPositionMeters;
            float cosTheta = Vector3.Dot(Vector3.Normalize(dPos), beamDir);

            // 2차 방정식 근의 공식 분해
            float L = totalPathLength;
            float D = dPos.Length();
            float numerator = L * L - D * D;
            float denominator = 2.0f * (L - Vector3.Dot(dPos, beamDir));

            if (denominator < 1.0f)
            {
                output.TrackLocked = false;
                return;
            }

            float rRx = numerator / denominator;
            if (rRx < 10.0f || rRx > 50000.0f)
            {
                output.TrackLocked = false;
                return;
            }

            // 3. 3차원 잠수함 좌표 확정
            Vector3 subPos = ping.ReceiverPositionMeters + beamDir * rRx;
            output.EstimatedSubPosition = subPos;
            output.TargetRangeFromShipMeters = Vector3.Distance(ping.TransmitterPositionMeters, subPos);
            output.LocalizationResidualMeters = MathF.Abs(Vector3.Distance(ping.TransmitterPositionMeters, subPos) + rRx - totalPathLength);
            output.TrackLocked = (output.LocalizationResidualMeters < 5.0f);
        }
    }
}