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
This commit is contained in:
Bohica
GitHub
parent
6938735113
commit
d6bb8316d0
2 changed files with 253 additions and 308 deletions
|
|
@ -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
|
||||
{
|
||||
public struct PathNode
|
||||
{
|
||||
public int cost, total;
|
||||
public int parent, next, prev;
|
||||
public int z;
|
||||
}
|
||||
namespace Server.PathAlgorithms.FastAStar;
|
||||
|
||||
public class FastAStarAlgorithm : PathAlgorithm
|
||||
{
|
||||
private struct PathNode
|
||||
{
|
||||
public int cost;
|
||||
public int total;
|
||||
public int parent;
|
||||
public int z;
|
||||
}
|
||||
|
||||
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 BitArray _onOpen = new(NodeCount);
|
||||
private static readonly byte[] _nodeStates = new byte[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);
|
||||
_onOpen.SetAll(false);
|
||||
Array.Clear(_nodeStates);
|
||||
|
||||
_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;
|
||||
_nodes[_openList].total = Heuristic(start.X - _xOffset, start.Y - _yOffset, start.Z);
|
||||
_nodes[_openList].parent = -1;
|
||||
_nodes[_openList].next = -1;
|
||||
_nodes[_openList].prev = -1;
|
||||
_nodes[_openList].z = start.Z;
|
||||
|
||||
_onOpen[_openList] = true;
|
||||
_touched[_openList] = true;
|
||||
_openQueue.Enqueue(fromNode, _nodes[fromNode].total);
|
||||
_nodeStates[fromNode] = 1;
|
||||
|
||||
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];
|
||||
|
||||
if (wasTouched)
|
||||
// Skip if the node is already closed
|
||||
if (_nodeStates[newNode] == 2)
|
||||
{
|
||||
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])
|
||||
{
|
||||
continue;
|
||||
// Requeue (duplicates allowed), and mark as open
|
||||
_openQueue.Enqueue(newNode, newTotal);
|
||||
_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)
|
||||
{
|
||||
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)
|
||||
private static int GetSuccessors(int p, Mobile m, Map map)
|
||||
{
|
||||
var px = p % AreaSize;
|
||||
var py = p / AreaSize % AreaSize;
|
||||
|
|
@ -349,4 +295,3 @@ namespace Server.PathAlgorithms.FastAStar
|
|||
return count;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue