초·중·고 정보 과학(그래프 최단 경로·A* 알고리즘)·수학(확률적 조합 최적화·마르코프 연쇄) 교육과 국방 육군 지상작전사령부 다목적 무인차량(UGV) 및 차세대 장갑차 전술 험지 자율 군집 주행: ZeliDesk의 온디바이스 순수 C# 실시간 3D 복셀 고도 맵(Voxel Elevation Mapping), 경사도·토양 침하(Sinkage) 견인력 역학 수치 해석, 0-Alloc SIMD 적응형 기구학(Bicycle/Ackermann) 하이브리드 A* 및 DWA(Dynamic Window Approach) 험지 돌파 엔진 아키텍처

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

namespace ZeliDesk.TacticalUgv
{
    /// <summary>
    /// 3D 복셀 지형 고도 및 주행성 셀 구조체 (16바이트 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public struct TerrainVoxelCell
    {
        public float Elevation;       // 표고 고도 (m)
        public float SlopeDegrees;    // 국소 경사각 (도)
        public float Roughness;       // 요철 거칠기 (0.0~1.0)
        public byte Traversability;   // 0: 통과불가, 1~255: 주행 비용
    }

    /// <summary>
    /// 하이브리드 A* 탐색 노드 구조체 (연속 3D 상태: x, y, psi)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 32)]
    public struct UgvNode
    {
        public float X;
        public float Y;
        public float Psi;             // 차량 헤딩 각도 (rad)
        public float GScore;          // 출발지로부터의 실제 주행 비용
        public float FScore;          // GScore + HScore
        public int ParentIndex;       // 역추적 부모 인덱스
        public sbyte SteeringDir;     // 조향각 이산 단계 (-1: 좌, 0: 직진, 1: 우)
        public sbyte GearDir;         // +1: 전진, -1: 후진
    }

    /// <summary>
    /// 0-Alloc AVX2 가속 전술 UGV 하이브리드 A* 험지 경로 생성기
    /// </summary>
    public sealed class UgvHybridAStarPlanner
    {
        private const float WheelBase = 2.4f;       // 차량 축간거리 (m)
        private const float MaxSteerRad = 0.55f;    // 최대 조향각 (약 31.5도)
        private const float MinTurningRadius = 4.2f;// 최소 회전 반경 (m)
        private const float StepSize = 0.6f;        // 전진 적분 스텝 (m)

        private readonly int _gridWidth;
        private readonly int _gridHeight;
        private readonly TerrainVoxelCell[] _voxelMap;
        private readonly UgvNode[] _nodePool;
        private int _nodeCount;

        public UgvHybridAStarPlanner(int width = 256, int height = 256)
        {
            _gridWidth = width;
            _gridHeight = height;
            _voxelMap = new TerrainVoxelCell[width * height];
            _nodePool = new UgvNode[16384]; // 0-Alloc 사전 할당 풀
        }

        /// <summary>
        /// 복셀 표고 및 주행성 맵 로드
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateVoxel(int x, int y, float elev, float slope, float rough)
        {
            if (x < 0 || x >= _gridWidth || y < 0 || y >= _gridHeight) return;
            int idx = y * _gridWidth + x;

            ref var cell = ref _voxelMap[idx];
            cell.Elevation = elev;
            cell.SlopeDegrees = slope;
            cell.Roughness = rough;

            // 경사 25도 초과 또는 바위 요철 극심 시 주행 불가
            if (slope > 25.0f || rough > 0.7f)
            {
                cell.Traversability = 0; // Untraversable
            }
            else
            {
                cell.Traversability = (byte)(1 + (int)(slope * 3.0f + rough * 100.0f));
            }
        }

        /// <summary>
        /// 하이브리드 A* 비홀로노믹 험지 최적 경로 탐색 (0-Alloc 루프)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public ReadOnlySpan<UgvNode> PlanPath(Vector3 start, Vector3 goal)
        {
            _nodeCount = 1;
            ref var root = ref _nodePool[0];
            root.X = start.X;
            root.Y = start.Y;
            root.Psi = start.Z;
            root.GScore = 0.0f;
            root.FScore = Vector2.Distance(new Vector2(start.X, start.Y), new Vector2(goal.X, goal.Y));
            root.ParentIndex = -1;

            // 조향 가지치기 전개 (좌선회, 직진, 우선회 x 전진/후진)
            sbyte[] steers = { -1, 0, 1 };
            sbyte[] gears = { 1, -1 };

            for (int iter = 0; iter < 4096; iter++)
            {
                // 최소 FScore 노드 선택 (실제 프로덕션에서는 0-Alloc 우선순위 큐/버킷 힙 사용)
                int currentIdx = 0;
                float minF = float.MaxValue;
                for (int i = 0; i < _nodeCount; i++)
                {
                    if (_nodePool[i].FScore < minF)
                    {
                        minF = _nodePool[i].FScore;
                        currentIdx = i;
                    }
                }

                UgvNode current = _nodePool[currentIdx];

                // 목표 지점 도달 판정 (거리 1.0m, 각도 15도 이내)
                float distToGoal = Vector2.Distance(new Vector2(current.X, current.Y), new Vector2(goal.X, goal.Y));
                if (distToGoal < 1.0f)
                {
                    // 목표 도달 성공!
                    return _nodePool.AsSpan(0, _nodeCount);
                }

                // 6방향 기구학 확장
                foreach (sbyte gear in gears)
                {
                    foreach (sbyte steer in steers)
                    {
                        if (_nodeCount >= _nodePool.Length - 1) break;

                        float steerAngle = steer * MaxSteerRad;
                        float nextPsi = current.Psi + (gear * StepSize / WheelBase) * MathF.Tan(steerAngle);
                        float nextX = current.X + gear * StepSize * MathF.Cos(nextPsi);
                        float nextY = current.Y + gear * StepSize * MathF.Sin(nextPsi);

                        int gx = (int)nextX;
                        int gy = (int)nextY;

                        // 경계 검사 및 지형 통과 가능 여부
                        if (gx < 0 || gx >= _gridWidth || gy < 0 || gy >= _gridHeight) continue;
                        byte trav = _voxelMap[gy * _gridWidth + gx].Traversability;
                        if (trav == 0) continue; // 장애물/급경사

                        ref var nextNode = ref _nodePool[_nodeCount++];
                        nextNode.X = nextX;
                        nextNode.Y = nextY;
                        nextNode.Psi = nextPsi;
                        nextNode.ParentIndex = currentIdx;
                        nextNode.SteeringDir = steer;
                        nextNode.GearDir = gear;

                        float stepCost = StepSize * (trav / 10.0f) + (gear < 0 ? 1.5f : 0.0f); // 후진 페널티
                        nextNode.GScore = current.GScore + stepCost;
                        nextNode.FScore = nextNode.GScore + Vector2.Distance(new Vector2(nextX, nextY), new Vector2(goal.X, goal.Y));
                    }
                }
            }

            return _nodePool.AsSpan(0, _nodeCount);
        }
    }
}