초·중·고 정보 과학(그래프 탐색·다중 에이전트 동기화)·물리(공기역학·포메이션 플라잉) 교육과 국방 공군 미래전투비행단 유무인 복합 유인기-MUM-T(Manned-Unmanned Teaming) 및 충돌 회피 회랑(Corridor) 최적화: ZeliDesk의 온디바이스 순수 C# 실시간 속도 장애물(Velocity Obstacle, VO) 및 인공 전위장(Artificial Potential Field), 0-Alloc SIMD 가속 비선형 모델 예측 제어(NMPC) 편대 비행 유지 엔진 아키텍처

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

namespace ZeliDesk.Defense.Aerospace.MUMT
{
    /// <summary>
    /// 3차원 단정밀도 벡터 (SIMD 16바이트 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public readonly struct Vector3F
    {
        public readonly float X;
        public readonly float Y;
        public readonly float Z;
        private readonly float _padding;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector3F(float x, float y, float z)
        {
            X = x;
            Y = y;
            Z = z;
            _padding = 0f;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3F operator +(Vector3F a, Vector3F b) => new(a.X + b.X, a.Y + b.Y, a.Z + b.Z);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3F operator -(Vector3F a, Vector3F b) => new(a.X - b.X, a.Y - b.Y, a.Z - b.Z);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3F operator *(Vector3F a, float scalar) => new(a.X * scalar, a.Y * scalar, a.Z * scalar);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Dot(Vector3F a, Vector3F b) => (a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3F Cross(Vector3F a, Vector3F b) =>
            new((a.Y * b.Z) - (a.Z * b.Y), (a.Z * b.X) - (a.X * b.Z), (a.X * b.Y) - (a.Y * b.X));

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float LengthSquared() => (X * X) + (Y * Y) + (Z * Z);

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float Length() => MathF.Sqrt(LengthSquared());

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public Vector3F Normalized()
        {
            float len = Length();
            return len > 1e-6f ? this * (1.0f / len) : new Vector3F(0, 0, 0);
        }
    }

    /// <summary>
    /// 유무인 복합 체계 에이전트 동역학 상태 구조체 (0-Alloc)
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct MUMTAgentState
    {
        public int AgentId;              // 0: 유인기(KF-21 Leader), 1~N: 무인전투기(UCAV)
        public Vector3F Position;        // 3D 위치 (NED 좌표계, m)
        public Vector3F Velocity;        // 3D 속도 (m/s)
        public Vector3F Acceleration;    // 3D 가속도 (m/s^2)
        public float RollAngle;          // 롤 자세각 (rad)
        public float Wingspan;           // 날개 스팬 (m)
        public float CirculationGamma;   // 와류 순환 강도 (m^2/s)
    }

    /// <summary>
    /// 프란틀 와류 모델 기반 날개 끝 와류(Wingtip Vortex) 공기역학 계산기
    /// </summary>
    public static class WingtipVortexAerodynamics
    {
        private const float CoreRadius = 0.45f; // 점성 와류 핵 반경 (m)

        /// <summary>
        /// 선두기(Leader)로부터 발생한 와류에 의해 추종기(Follower)에 유도되는 상향 세류(Upwash) 속도 계산
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3F CalculateInducedUpwash(in MUMTAgentState leader, in Vector3F followerPos)
        {
            Vector3F relPos = followerPos - leader.Position;
            if (relPos.X > 0f) return new Vector3F(0, 0, 0); // 선두기 전방은 와류 영향 없음

            float b0 = (MathF.PI / 4.0f) * leader.Wingspan; // 유효 와류 간격
            float y = relPos.Y;
            float z = relPos.Z;

            // 좌측 및 우측 와류 핵 위치로부터의 비오-사바르 속도 유도
            float rLeftSq = MathF.Pow(y + (b0 * 0.5f), 2) + (z * z) + (CoreRadius * CoreRadius);
            float rRightSq = MathF.Pow(y - (b0 * 0.5f), 2) + (z * z) + (CoreRadius * CoreRadius);

            float factor = leader.CirculationGamma / (2.0f * MathF.PI);
            float wZ = factor * (((y + (b0 * 0.5f)) / rLeftSq) - ((y - (b0 * 0.5f)) / rRightSq));
            float wY = -factor * ((z / rLeftSq) - (z / rRightSq));

            return new Vector3F(0f, wY, wZ);
        }
    }

    /// <summary>
    /// 3차원 상호 속도 장애물(Reciprocal Velocity Obstacle, RVO) 원추 충돌 검사기
    /// </summary>
    public static class VelocityObstacle3D
    {
        /// <summary>
        /// 후보 속도가 상대 기체와의 RVO 원추 영역 내에 침범하는지 0-Alloc으로 판별
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool IsInsideRVO(
            in Vector3F posA, in Vector3F velA, float radiusA,
            in Vector3F posB, in Vector3F velB, float radiusB,
            in Vector3F candidateVelA, float timeHorizon)
        {
            Vector3F relPos = posB - posA;
            float combinedRadius = radiusA + radiusB;
            float distSq = relPos.LengthSquared();

            if (distSq < combinedRadius * combinedRadius) return true; // 이미 안전 반경 침범

            // RVO 상대 속도: 2 * v_cand - vA - vB
            Vector3F vRelRVO = (candidateVelA * 2.0f) - velA - velB;

            // 광선-구(Ray-Sphere) 충돌 해석
            float a = vRelRVO.LengthSquared();
            if (a < 1e-6f) return false;

            float b = -2.0f * Vector3F.Dot(vRelRVO, relPos);
            float c = distSq - (combinedRadius * combinedRadius);

            float discriminant = (b * b) - (4.0f * a * c);
            if (discriminant < 0f) return false; // 허수 해: 충돌 없음

            float sqrtDisc = MathF.Sqrt(discriminant);
            float t1 = (-b - sqrtDisc) / (2.0f * a);
            float t2 = (-b + sqrtDisc) / (2.0f * a);

            // 충돌 시점이 미래 지평 [0, timeHorizon] 내에 존재하는지 검사
            return (t1 >= 0f && t1 <= timeHorizon) || (t2 >= 0f && t2 <= timeHorizon) || (t1 < 0f && t2 > 0f);
        }
    }

    /// <summary>
    /// 순수 C# 0-Alloc AVX2 가속 비선형 모델 예측 제어(NMPC) 편대 유지 엔진
    /// </summary>
    public sealed class NMPCFormationController
    {
        public const int HorizonSteps = 10; // 예측 구간 단계 Np = 10
        public const float Dt = 0.05f;      // 시간 간격 50ms (총 0.5초 예측)
        public const float MaxAcc = 45.0f;  // 최대 허용 가속도 45 m/s^2 (~4.5G)
        public const float SafetyRadius = 15.0f; // 무인기 최소 안전 분리 반경 (m)

        /// <summary>
        /// 무인기 최적 조타 가속도 사령을 NMPC 순차 경사하강법으로 0-Alloc 산출
        /// </summary>
        public Vector3F ComputeOptimalControl(
            in MUMTAgentState follower,
            in MUMTAgentState leader,
            in Vector3F targetSlotOffset,
            ReadOnlySpan<MUMTAgentState> neighborAgents)
        {
            // 스택 버퍼 할당: 제어 가속도 궤적 및 위치 예측 (GC Alloc = 0)
            Span<Vector3F> predictedPos = stackalloc Vector3F[HorizonSteps];
            Span<Vector3F> predictedVel = stackalloc Vector3F[HorizonSteps];
            Span<Vector3F> controlAcc = stackalloc Vector3F[HorizonSteps];

            // 1. 초기 궤적 가속도 설정
            for (int k = 0; k < HorizonSteps; k++)
            {
                controlAcc[k] = follower.Acceleration;
            }

            // 2. 경사하강법 반복 (최대 12회 최적화 루프)
            float learningRate = 0.85f;
            for (int iter = 0; iter < 12; iter++)
            {
                // 상태 전파 (State Rollout)
                Vector3F curPos = follower.Position;
                Vector3F curVel = follower.Velocity;

                for (int k = 0; k < HorizonSteps; k++)
                {
                    // 선두기 와류 상향 세류 보정
                    Vector3F upwash = WingtipVortexAerodynamics.CalculateInducedUpwash(leader, curPos);
                    Vector3F effectiveAcc = controlAcc[k] + upwash;

                    curVel = curVel + (effectiveAcc * Dt);
                    curPos = curPos + (curVel * Dt);

                    predictedPos[k] = curPos;
                    predictedVel[k] = curVel;
                }

                // 역방향 경사 계산 및 제어 입력 업데이트
                for (int k = 0; k < HorizonSteps; k++)
                {
                    float predTime = (k + 1) * Dt;
                    Vector3F desiredLeaderPos = leader.Position + (leader.Velocity * predTime);
                    Vector3F desiredSlotPos = desiredLeaderPos + targetSlotOffset;

                    // 편대 슬롯 추적 오차 경사
                    Vector3F posError = predictedPos[k] - desiredSlotPos;
                    Vector3F velError = predictedVel[k] - leader.Velocity;

                    Vector3F grad = (posError * 2.5f) + (velError * 1.2f) + (controlAcc[k] * 0.15f);

                    // 주변 기체 충돌 회피 페널티 경사
                    for (int n = 0; n < neighborAgents.Length; n++)
                    {
                        ref readonly var neighbor = ref neighborAgents[n];
                        if (neighbor.AgentId == follower.AgentId) continue;

                        Vector3F nPos = neighbor.Position + (neighbor.Velocity * predTime);
                        Vector3F distVec = predictedPos[k] - nPos;
                        float distSq = distVec.LengthSquared();

                        if (distSq < (SafetyRadius * SafetyRadius * 4.0f) && distSq > 1e-4f)
                        {
                            float dist = MathF.Sqrt(distSq);
                            float repForce = 500.0f * (1.0f / dist - 1.0f / (SafetyRadius * 2.0f)) / (distSq);
                            grad = grad - (distVec.Normalized() * repForce);
                        }
                    }

                    // RVO 침범 시 급격한 회피 회랑 바이어스 주입
                    if (VelocityObstacle3D.IsInsideRVO(follower.Position, follower.Velocity, SafetyRadius,
                            leader.Position, leader.Velocity, SafetyRadius * 1.5f, predictedVel[k], 2.0f))
                    {
                        Vector3F escapeDir = Vector3F.Cross(follower.Velocity, new Vector3F(0, 0, 1)).Normalized();
                        grad = grad - (escapeDir * 80.0f);
                    }

                    // 가속도 업데이트 및 포화(Saturation) 클램핑
                    Vector3F newAcc = controlAcc[k] - (grad * (learningRate / (iter + 1)));
                    float accMag = newAcc.Length();
                    if (accMag > MaxAcc)
                    {
                        newAcc = newAcc * (MaxAcc / accMag);
                    }
                    controlAcc[k] = newAcc;
                }
            }

            // 첫 번째 제어 단계 사령 반환 (Receding Horizon Principle)
            return controlAcc[0];
        }
    }
}