대한민국 공군 KF-21 보라매 조종사 헬멧장착시현기(HMD) 광학·자기 하이브리드 머리 추적(Head Tracker) 시선각 지연 보상 & 고오프보어사이트(HOBS) 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.AvionicsHmd
{
    /// <summary>
    /// 헬멧 광학 마커 및 자기장 센서 계측 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public readonly struct HmdRawSensorData
    {
        public readonly float MagX;          // 자기 센서 계측치
        public readonly float MagY;
        public readonly float MagZ;
        public readonly float OptMarkerX;    // 광학 카메라 2D/3D 마커 좌표
        public readonly float OptMarkerY;
        public readonly float OptMarkerZ;
        public readonly bool IsOpticalTracked; // 광학 마커 유효성

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public HmdRawSensorData(float mx, float my, float mz, float ox, float oy, float oz, bool optValid)
        {
            MagX = mx; MagY = my; MagZ = mz;
            OptMarkerX = ox; OptMarkerY = oy; OptMarkerZ = oz;
            IsOpticalTracked = optValid;
        }
    }

    /// <summary>
    /// HMD 조준선 및 HOBS 미사일 슬레이브 출력 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct HmdTargetingOutput
    {
        public float SightAzimuthDeg;       // 조종사 시선 방위각 (deg, 기수 기준)
        public float SightElevationDeg;     // 조종사 시선 고각 (deg, 기수 기준)
        public float OffBoresightAngleDeg;  // 오프-보어사이트 이탈각 (0~85도)
        public float PredictedLosX;         // 지연 보상된 3차원 시선 벡터 X
        public float PredictedLosY;         // Y
        public float PredictedLosZ;         // Z
        public bool IsHobsLockAvailable;    // HOBS 유도탄 발사 제원 충족 여부
    }

    /// <summary>
    /// KF-21 HMD 광학·자기 하이브리드 머리 추적 1,000Hz 0-Alloc C# 엔진
    /// </summary>
    public sealed class Kf21HmdTargetingEngine
    {
        private const float DegToRad = MathF.PI / 180.0f;
        private const float RadToDeg = 180.0f / MathF.PI;
        private const float LatencyCompensationSec = 0.0035f; // 3.5ms 지연 외삽

        // 자세 상태 (쿼터니언 w, x, y, z)
        private float _qw = 1.0f, _qx = 0f, _qy = 0f, _qz = 0f;
        private float _rateX = 0f, _rateY = 0f, _rateZ = 0f; // 머리 회전 각속도

        public Kf21HmdTargetingEngine() { }

        /// <summary>
        /// 1,000Hz 실시간 틱: 복합 센서 데이터로부터 지연 보상된 시선각 및 HOBS 지령 산출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateTick(
            in HmdRawSensorData sensor,
            float gyroRateP, float gyroRateQ, float gyroRateR, // 자이로 계측치
            float dt,
            ref HmdTargetingOutput output)
        {
            // 1. 센서 융합: 광학 마커 유효 시 광학 기준 갱신, 가림 시 자기/자이로 추측 항법
            float targetYaw = sensor.MagZ;
            float targetPitch = sensor.MagY;
            float targetRoll = sensor.MagX;

            if (sensor.IsOpticalTracked)
            {
                // 광학 마커 좌표로부터 정밀 각도 역산 및 자기장 왜곡 보정
                targetYaw = MathF.Atan2(sensor.OptMarkerY, sensor.OptMarkerX);
                targetPitch = MathF.Atan2(-sensor.OptMarkerZ, MathF.Sqrt(sensor.OptMarkerX * sensor.OptMarkerX + sensor.OptMarkerY * sensor.OptMarkerY));
            }

            // 2. 머리 회전 각속도 필터링
            _rateX += (gyroRateP - _rateX) * 0.35f;
            _rateY += (gyroRateQ - _rateY) * 0.35f;
            _rateZ += (gyroRateR - _rateZ) * 0.35f;

            // 3. 쿼터니언 적분 (dt = 0.001s)
            float halfDt = 0.5f * dt;
            float dqW = -halfDt * (_qx * _rateX + _qy * _rateY + _qz * _rateZ);
            float dqX =  halfDt * (_qw * _rateX + _qy * _rateZ - _qz * _rateY);
            float dqY =  halfDt * (_qw * _rateY - _qx * _rateZ + _qz * _rateX);
            float dqZ =  halfDt * (_qw * _rateZ + _qx * _rateY - _qy * _rateX);

            _qw += dqW; _qx += dqX; _qy += dqY; _qz += dqZ;

            // 정규화
            float normInv = 1.0f / MathF.Sqrt(_qw * _qw + _qx * _qx + _qy * _qy + _qz * _qz);
            _qw *= normInv; _qx *= normInv; _qy *= normInv; _qz *= normInv;

            // 4. 지연 시간(3.5ms) 외삽 선행 예측 (Lead Prediction)
            float leadFactor = 0.5f * LatencyCompensationSec;
            float pW = _qw - leadFactor * (_qx * _rateX + _qy * _rateY + _qz * _rateZ);
            float pX = _qx + leadFactor * (_qw * _rateX + _qy * _rateZ - _qz * _rateY);
            float pY = _qy + leadFactor * (_qw * _rateY - _qx * _rateZ + _qz * _rateX);
            float pZ = _qz + leadFactor * (_qw * _rateZ + _qx * _rateY - _qy * _rateX);

            float pNorm = 1.0f / MathF.Sqrt(pW * pW + pX * pX + pY * pY + pZ * pZ);
            pW *= pNorm; pX *= pNorm; pY *= pNorm; pZ *= pNorm;

            // 5. 예측된 쿼터니언으로 시선 벡터 [1, 0, 0] 동체 회전
            // v' = q * [1, 0, 0] * q*
            float losX = 1.0f - 2.0f * (pY * pY + pZ * pZ);
            float losY = 2.0f * (pX * pY + pW * pZ);
            float losZ = 2.0f * (pX * pZ - pW * pY);

            // 6. 시선 방위각, 고각 및 오프-보어사이트 각도 계산
            float sightAzDeg = MathF.Atan2(losY, losX) * RadToDeg;
            float sightElDeg = MathF.Atan2(-losZ, MathF.Sqrt(losX * losX + losY * losY)) * RadToDeg;
            float offBoresightAngle = MathF.Acos(MathF.Clamp(losX, -1.0f, 1.0f)) * RadToDeg;

            // 7. HOBS 유도탄 락온 가용성 판단 (최대 80도 오프보어사이트 지원)
            bool lockAvailable = offBoresightAngle <= 80.0f;

            // 8. 출력 기록
            output.SightAzimuthDeg = sightAzDeg;
            output.SightElevationDeg = sightElDeg;
            output.OffBoresightAngleDeg = offBoresightAngle;
            output.PredictedLosX = losX;
            output.PredictedLosY = losY;
            output.PredictedLosZ = losZ;
            output.IsHobsLockAvailable = lockAvailable;
        }
    }
}