feat: Refactors A* Movement to use PriorityQueue (#2244)

### Summary

* Replaces the node chain with a PriorityQueue.
* Performance difference is up to 3x faster.

### Benchmarks
```cs
| Method              | Mean     | Error     | StdDev    | Gen0   | Allocated |
|-------------------- |---------:|----------:|----------:|-------:|----------:|
| FastAStar_PQueue    | 2.112 us | 0.0186 us | 0.0165 us | 0.0038 |      64 B |
| FastAStar_NodeChain | 6.430 us | 0.0800 us | 0.1807 us |      - |      64 B |
```

### Video

https://github.com/user-attachments/assets/3edd2524-5146-4775-be45-3516f0b01c27

Projects/UOContent/Engines/Pathing/FastAStarAlgorithm.cs

@@ -1,19 +1,37 @@
-using System.Collections;
+/*************************************************************************
+ * ModernUO                                                              *
+ * Copyright 2019-2025 - ModernUO Development Team                       *
+ * Email: hi@modernuo.com                                                *
+ * File: FastAStarAlgorithm.cs                                           *
+ *                                                                       *
+ * This program is free software: you can redistribute it and/or modify  *
+ * it under the terms of the GNU General Public License as published by  *
+ * the Free Software Foundation, either version 3 of the License, or     *
+ * (at your option) any later version.                                   *
+ *                                                                       *
+ * You should have received a copy of the GNU General Public License     *
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.  *
+ ************************************************************************/
+
+using System;
 using Server.Mobiles;
 using CalcMoves = Server.Movement.Movement;
 using MoveImpl = Server.Movement.MovementImpl;
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
 
-namespace Server.PathAlgorithms.FastAStar
+namespace Server.PathAlgorithms.FastAStar;
+
+public class FastAStarAlgorithm : PathAlgorithm
 {
-    public struct PathNode
+    private struct PathNode
     {
-        public int cost, total;
+        public int cost;
-        public int parent, next, prev;
+        public int total;
+        public int parent;
         public int z;
     }
 
-    public class FastAStarAlgorithm : PathAlgorithm
-    {
     private const int MaxDepth = 300;
     private const int AreaSize = 38;
 
@@ -22,20 +40,20 @@ namespace Server.PathAlgorithms.FastAStar
     private const int PlaneOffset = 128;
     private const int PlaneCount = 13;
     private const int PlaneHeight = 20;
-        public static PathAlgorithm Instance = new FastAStarAlgorithm();
+    public static readonly PathAlgorithm Instance = new FastAStarAlgorithm();
 
     private static readonly Direction[] _path = new Direction[AreaSize * AreaSize];
     private static readonly PathNode[] _nodes = new PathNode[NodeCount];
-        private static readonly BitArray _touched = new(NodeCount);
+    private static readonly byte[] _nodeStates = new byte[NodeCount];
-        private static readonly BitArray _onOpen = new(NodeCount);
     private static readonly int[] _successors = new int[8];
+    private static readonly PriorityQueue<int, int> _openQueue = new();
 
     private static int _xOffset;
     private static int _yOffset;
-        private static int _openList;
 
     private Point3D _goal;
 
+    [MethodImpl(MethodImplOptions.AggressiveInlining)]
     public int Heuristic(int x, int y, int z)
     {
         x -= _goal.X - _xOffset;
@@ -51,63 +69,6 @@ namespace Server.PathAlgorithms.FastAStar
     public override bool CheckCondition(Mobile m, Map map, Point3D start, Point3D goal) =>
         Utility.InRange(start, goal, AreaSize);
 
-        private void RemoveFromChain(int node)
-        {
-            if (node is < 0 or >= NodeCount)
-            {
-                return;
-            }
-
-            if (!_touched[node] || !_onOpen[node])
-            {
-                return;
-            }
-
-            var prev = _nodes[node].prev;
-            var next = _nodes[node].next;
-
-            if (_openList == node)
-            {
-                _openList = next;
-            }
-
-            if (prev != -1)
-            {
-                _nodes[prev].next = next;
-            }
-
-            if (next != -1)
-            {
-                _nodes[next].prev = prev;
-            }
-
-            _nodes[node].prev = -1;
-            _nodes[node].next = -1;
-        }
-
-        private void AddToChain(int node)
-        {
-            if (node is < 0 or >= NodeCount)
-            {
-                return;
-            }
-
-            RemoveFromChain(node);
-
-            if (_openList != -1)
-            {
-                _nodes[_openList].prev = node;
-            }
-
-            _nodes[node].next = _openList;
-            _nodes[node].prev = -1;
-
-            _openList = node;
-
-            _touched[node] = true;
-            _onOpen[node] = true;
-        }
-
     public override Direction[] Find(Mobile m, Map map, Point3D start, Point3D goal)
     {
         if (!Utility.InRange(start, goal, AreaSize))
@@ -115,8 +76,7 @@ namespace Server.PathAlgorithms.FastAStar
             return null;
         }
 
-            _touched.SetAll(false);
+        Array.Clear(_nodeStates);
-            _onOpen.SetAll(false);
 
         _goal = goal;
 
@@ -126,17 +86,13 @@ namespace Server.PathAlgorithms.FastAStar
         var fromNode = GetIndex(start.X, start.Y, start.Z);
         var destNode = GetIndex(goal.X, goal.Y, goal.Z);
 
-            _openList = fromNode;
+        _nodes[fromNode].cost = 0;
+        _nodes[fromNode].total = Heuristic(start.X - _xOffset, start.Y - _yOffset, start.Z);
+        _nodes[fromNode].parent = -1;
+        _nodes[fromNode].z = start.Z;
 
-            _nodes[_openList].cost = 0;
+        _openQueue.Enqueue(fromNode, _nodes[fromNode].total);
-            _nodes[_openList].total = Heuristic(start.X - _xOffset, start.Y - _yOffset, start.Z);
+        _nodeStates[fromNode] = 1;
-            _nodes[_openList].parent = -1;
-            _nodes[_openList].next = -1;
-            _nodes[_openList].prev = -1;
-            _nodes[_openList].z = start.Z;
-
-            _onOpen[_openList] = true;
-            _touched[_openList] = true;
 
         var bc = m as BaseCreature;
 
@@ -144,15 +100,26 @@ namespace Server.PathAlgorithms.FastAStar
 
         var path = _path;
 
-            while (_openList != -1)
+        while (_openQueue.Count > 0)
         {
-                var bestNode = FindBest(_openList);
-
             if (++depth > MaxDepth)
             {
                 break;
             }
 
+            if (!_openQueue.TryDequeue(out var bestNode, out var bestTotal))
+            {
+                break;
+            }
+
+            // Duplicate, lower priority
+            if (_nodeStates[bestNode] == 2 || _nodes[bestNode].total != bestTotal)
+            {
+                continue;
+            }
+
+            _nodeStates[bestNode] = 2;
+
             if (bc != null)
             {
                 MoveImpl.AlwaysIgnoreDoors = bc.CanOpenDoors;
@@ -170,38 +137,39 @@ namespace Server.PathAlgorithms.FastAStar
 
             if (count == 0)
             {
-                    break;
+                continue;
             }
 
             for (var i = 0; i < count; ++i)
             {
                 var newNode = vals[i];
 
-                    var wasTouched = _touched[newNode];
+                // Skip if the node is already closed
-
+                if (_nodeStates[newNode] == 2)
-                    if (wasTouched)
                 {
                     continue;
                 }
 
-                    var newCost = _nodes[bestNode].cost + 1;
+                var isDiagonal = i % 2 == 1;
+                var moveCost = isDiagonal ? 14 : 10;
+                var newCost = _nodes[bestNode].cost + moveCost;
                 var newTotal = newCost + Heuristic(
                     newNode % AreaSize,
                     newNode / AreaSize % AreaSize,
                     _nodes[newNode].z
                 );
 
+                if (_nodeStates[newNode] == 0 || newTotal < _nodes[newNode].total)
+                {
                     _nodes[newNode].parent = bestNode;
                     _nodes[newNode].cost = newCost;
                     _nodes[newNode].total = newTotal;
 
-                    if (_onOpen[newNode])
+                    // Requeue (duplicates allowed), and mark as open
-                    {
+                    _openQueue.Enqueue(newNode, newTotal);
-                        continue;
+                    _nodeStates[newNode] = 1;
                 }
 
-                    AddToChain(newNode);
-
                 if (newNode != destNode)
                 {
                     continue;
@@ -234,14 +202,16 @@ namespace Server.PathAlgorithms.FastAStar
                     dirs[backtrack++] = path[--pathCount];
                 }
 
+                _openQueue.Clear();
                 return dirs;
             }
         }
 
+        _openQueue.Clear();
         return null;
     }
 
-        private int GetIndex(int x, int y, int z)
+    private static int GetIndex(int x, int y, int z)
     {
         x -= _xOffset;
         y -= _yOffset;
@@ -251,31 +221,7 @@ namespace Server.PathAlgorithms.FastAStar
         return x + y * AreaSize + z * AreaSize * AreaSize;
     }
 
-        private int FindBest(int node)
+    private static int GetSuccessors(int p, Mobile m, Map map)
-        {
-            var least = _nodes[node].total;
-            var leastNode = node;
-
-            while (node != -1)
-            {
-                if (_nodes[node].total < least)
-                {
-                    least = _nodes[node].total;
-                    leastNode = node;
-                }
-
-                node = _nodes[node].next;
-            }
-
-            RemoveFromChain(leastNode);
-
-            _touched[leastNode] = true;
-            _onOpen[leastNode] = false;
-
-            return leastNode;
-        }
-
-        public int GetSuccessors(int p, Mobile m, Map map)
     {
         var px = p % AreaSize;
         var py = p / AreaSize % AreaSize;
@@ -348,5 +294,4 @@ namespace Server.PathAlgorithms.FastAStar
 
         return count;
     }
-    }
 }

version.json

@@ -1,4 +1,4 @@
 {
   "$schema": "https://raw.githubusercontent.com/dotnet/Nerdbank.GitVersioning/master/src/NerdBank.GitVersioning/version.schema.json",
-  "version": "0.15.0"
+  "version": "0.15.1"
 }