diff --git a/src/Pathfinding/BinaryMinHeap{T}.cs b/src/Pathfinding/BinaryMinHeap{T}.cs index d4fa07b2de..3340327c44 100644 --- a/src/Pathfinding/BinaryMinHeap{T}.cs +++ b/src/Pathfinding/BinaryMinHeap{T}.cs @@ -21,18 +21,6 @@ public class BinaryMinHeap : IPriorityQueue private readonly List _innerList = new(); private readonly IComparer _elementComparer; - /// Reused variable to reduce stack allocations. - private int _i; - - /// Reused variable to reduce stack allocations. - private int _parentIndex; - - /// Reused variable to reduce stack allocations. - private int _left; - - /// Reused variable to reduce stack allocations. - private int _right; - /// /// Initializes a new instance of the class. /// @@ -67,27 +55,19 @@ public BinaryMinHeap(IComparer comparer, int capacity) /// public void Push(T item) { - this._i = this._innerList.Count; + var index = this._innerList.Count; this._innerList.Add(item); - do + while (index > 0) { - if (this._i == 0) + var parentIndex = unchecked(index - 1) >> 1; + if (this.Compare(index, parentIndex) >= 0) { break; } - this._parentIndex = unchecked(this._i - 1) >> 1; - if (this.OnCompareWithElementOfI(this._parentIndex) < 0) - { - this.SwitchElementsParentWithI(); - this._i = this._parentIndex; - } - else - { - break; - } + this.Swap(index, parentIndex); + index = parentIndex; } - while (true); } /// @@ -99,32 +79,31 @@ public T Pop() } var result = this._innerList[0]; - this._i = 0; + var index = 0; this._innerList[0] = this._innerList[^1]; this._innerList.RemoveAt(this._innerList.Count - 1); - do + while (true) { - this._parentIndex = this._i; - this._left = unchecked((this._i << 1) + 1); - this._right = unchecked((this._i << 1) + 2); - if (this._innerList.Count > this._left && this.OnCompareWithElementOfI(this._left) > 0) + var smallest = index; + var left = unchecked((index << 1) + 1); + var right = unchecked((index << 1) + 2); + if (this._innerList.Count > left && this.Compare(index, left) > 0) { - this._i = this._left; + index = left; } - if (this._innerList.Count > this._right && this.OnCompareWithElementOfI(this._right) > 0) + if (this._innerList.Count > right && this.Compare(index, right) > 0) { - this._i = this._right; + index = right; } - if (this._i == this._parentIndex) + if (index == smallest) { break; } - this.SwitchElementsParentWithI(); + this.Swap(smallest, index); } - while (true); return result; } @@ -150,16 +129,14 @@ public void Clear() } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private void SwitchElementsParentWithI() + private void Swap(int i, int j) { - T h = this._innerList[this._i]; - this._innerList[this._i] = this._innerList[this._parentIndex]; - this._innerList[this._parentIndex] = h; + (this._innerList[i], this._innerList[j]) = (this._innerList[j], this._innerList[i]); } [MethodImpl(MethodImplOptions.AggressiveInlining)] - private int OnCompareWithElementOfI(int j) + private int Compare(int i, int j) { - return this._elementComparer.Compare(this._innerList[this._i], this._innerList[j]); + return this._elementComparer.Compare(this._innerList[i], this._innerList[j]); } } \ No newline at end of file diff --git a/src/Pathfinding/FullGridNetwork.cs b/src/Pathfinding/FullGridNetwork.cs index 9d8265ff3a..579387729e 100644 --- a/src/Pathfinding/FullGridNetwork.cs +++ b/src/Pathfinding/FullGridNetwork.cs @@ -40,9 +40,10 @@ public override Node GetNodeAt(Point position) /// public override bool Prepare(Point start, Point end, byte[,] grid, bool includeSafezone) { - foreach (var node in this._nodes.Where(n => n != null)) + var nodes = this._nodes; + for (int i = 0; i < nodes.Length; i++) { - node.Status = NodeStatus.Undefined; + nodes[i]?.Status = NodeStatus.Undefined; } return base.Prepare(start, end, grid, includeSafezone); diff --git a/src/Pathfinding/PathFinder.cs b/src/Pathfinding/PathFinder.cs index 7aa46e4e85..d07df775bc 100644 --- a/src/Pathfinding/PathFinder.cs +++ b/src/Pathfinding/PathFinder.cs @@ -24,6 +24,8 @@ public class PathFinder : IPathFinder private readonly INetwork _network; private readonly IPriorityQueue _openList; + private int _maximumDistance; + private int _maximumDistanceSquared; /// /// Initializes a new instance of the class. @@ -52,8 +54,17 @@ public PathFinder(INetwork network, IPriorityQueue openList) /// /// Gets or sets the maximum distance until which the path should be resolved. + /// A value of 0 disables the check. /// - public int MaximumDistance { get; set; } + public int MaximumDistance + { + get => this._maximumDistance; + set + { + this._maximumDistance = value; + this._maximumDistanceSquared = value * value; + } + } /// /// Gets or sets the search limit. @@ -78,7 +89,7 @@ public PathFinder(INetwork network, IPriorityQueue openList) { var stopwatch = Stopwatch.StartNew(); var result = this.FindPathInner(start, end, terrain, includeSafezone, cancellationToken); - var elapsedMs = (double)stopwatch.ElapsedTicks / TimeSpan.TicksPerMillisecond; + var elapsedMs = stopwatch.Elapsed.TotalMilliseconds; if (result is null) { FailedSearches.Add(1); @@ -120,10 +131,7 @@ public void ResetPathFinder() return null; } - if (this.Heuristic != null) - { - this.Heuristic.HeuristicEstimateMultiplier = this.HeuristicEstimate; - } + this.Heuristic.HeuristicEstimateMultiplier = this.HeuristicEstimate; var closeNodeCounter = 0; var startNode = this._network.GetNodeAt(start); @@ -163,7 +171,7 @@ public void ResetPathFinder() if (pathFound) { - return this.GetCalculatedPath(end).Reverse().ToList(); + return this.GetCalculatedPath(end); } return null; @@ -178,7 +186,7 @@ private void ExpandNodes(Node node, Point start, Point end) continue; } - var heuristicEstimate = this.Heuristic?.CalculateHeuristicDistance(newNode.Position, end) ?? 0; + var heuristicEstimate = this.Heuristic.CalculateHeuristicDistance(newNode.Position, end); newNode.PredictedTotalCost = newNode.CostUntilNow + heuristicEstimate; newNode.Status = NodeStatus.Open; newNode.PreviousNode = node; @@ -186,35 +194,53 @@ private void ExpandNodes(Node node, Point start, Point end) } } - private IEnumerable GetCalculatedPath(Point end) + private List GetCalculatedPath(Point end) { + var path = new List(); var node = this._network.GetNodeAt(end); while (node!.PreviousNode != node) { - yield return new PathResultNode(node.Position, node.PreviousNode!.Position); + path.Add(new PathResultNode(node.Position, node.PreviousNode!.Position)); node = node.PreviousNode; } + + path.Reverse(); + return path; } private bool MaximumDistanceExceeded(Point start, Point end) { - if (this.MaximumDistance != 0) + if (this._maximumDistance == 0) { - return start.EuclideanDistanceTo(end) > this.MaximumDistance; + return false; } - return false; + return start.EuclideanDistanceSquaredTo(end) > this._maximumDistanceSquared; } private bool MaximumDistanceExceeded(Point start, Point end, Node node) { - if (this.MaximumDistance != 0) + if (this._maximumDistance == 0) + { + return false; + } + + // Compare squared distances to avoid the expensive square root. + // Triangle inequality on squared values is not exact, so we compare + // the sum of roots indirectly: (a + b)^2 > max^2. + var startToNodeSquared = start.EuclideanDistanceSquaredTo(node.Position); + if (startToNodeSquared > this._maximumDistanceSquared) + { + return true; + } + + var nodeToEndSquared = node.Position.EuclideanDistanceSquaredTo(end); + if (nodeToEndSquared > this._maximumDistanceSquared) { - var distance = start.EuclideanDistanceTo(node.Position); - distance += node.Position.EuclideanDistanceTo(end); - return distance > this.MaximumDistance; + return true; } - return false; + var detour = Math.Sqrt(startToNodeSquared) + Math.Sqrt(nodeToEndSquared); + return detour > this._maximumDistance; } } \ No newline at end of file diff --git a/src/Pathfinding/Point.cs b/src/Pathfinding/Point.cs index b73212922c..e77f6ba6a2 100644 --- a/src/Pathfinding/Point.cs +++ b/src/Pathfinding/Point.cs @@ -51,7 +51,20 @@ public record struct Point(byte X, byte Y) /// The distance between this point and another point. public double EuclideanDistanceTo(Point otherPoint) { - return Math.Sqrt(Math.Pow(Math.Abs(this.X - otherPoint.X), 2) + Math.Pow(Math.Abs(this.Y - otherPoint.Y), 2)); + return Math.Sqrt(this.EuclideanDistanceSquaredTo(otherPoint)); + } + + /// + /// Gets the squared euclidean distance between this point and another point. + /// Use this for distance comparisons to avoid the expensive square root. + /// + /// The other point. + /// The squared distance between this point and another point. + public int EuclideanDistanceSquaredTo(Point otherPoint) + { + var dx = this.X - otherPoint.X; + var dy = this.Y - otherPoint.Y; + return (dx * dx) + (dy * dy); } /// diff --git a/src/Pathfinding/ScopedGridNetwork.cs b/src/Pathfinding/ScopedGridNetwork.cs index 25f80d5862..35b1205b2a 100644 --- a/src/Pathfinding/ScopedGridNetwork.cs +++ b/src/Pathfinding/ScopedGridNetwork.cs @@ -4,6 +4,8 @@ namespace MUnique.OpenMU.Pathfinding; +using System.Numerics; + /// /// Network which is built of a two-dimensional grid of nodes where /// each coordinate has a fixed cost to reach it from any direction. @@ -72,7 +74,7 @@ public override bool Prepare(Point start, Point end, byte[,] grid, bool includeS this._actualSegmentSideLength *= 2; } - this._bitsPerCoordinate = (int)Math.Log(this._actualSegmentSideLength, 2); + this._bitsPerCoordinate = BitOperations.Log2(this._actualSegmentSideLength); var avg = (start / 2) + (end / 2); var offsetX = GetOffset(avg.X, grid.GetUpperBound(0) + 1); diff --git a/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.csproj b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.csproj new file mode 100644 index 0000000000..9b5d94ab78 --- /dev/null +++ b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.csproj @@ -0,0 +1,44 @@ + + + + net10.0 + enable + nullable;CS4014;VSTHRD110;VSTHRD100 + false + false + + Exe + + MUnique.OpenMU.Pathfinding.Benchmarks + AnyCPU + + + + bin\Debug\ + bin\Debug\MUnique.OpenMU.Pathfinding.Benchmarks.xml + + + bin\Release\ + bin\Release\MUnique.OpenMU.Pathfinding.Benchmarks.xml + + + + + + + + + + + + + + + + + + + + + + diff --git a/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.sln b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.sln new file mode 100644 index 0000000000..bbdf5b7afd --- /dev/null +++ b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.sln @@ -0,0 +1,24 @@ +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio 15 +VisualStudioVersion = 15.0.27703.2047 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "MUnique.OpenMU.Pathfinding.Benchmarks", "MUnique.OpenMU.Pathfinding.Benchmarks.csproj", "{C3D4E5F6-7890-ABCD-EF12-345678901234}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Any CPU = Debug|Any CPU + Release|Any CPU = Release|Any CPU + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {C3D4E5F6-7890-ABCD-EF12-345678901234}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {C3D4E5F6-7890-ABCD-EF12-345678901234}.Debug|Any CPU.Build.0 = Debug|Any CPU + {C3D4E5F6-7890-ABCD-EF12-345678901234}.Release|Any CPU.ActiveCfg = Release|Any CPU + {C3D4E5F6-7890-ABCD-EF12-345678901234}.Release|Any CPU.Build.0 = Release|Any CPU + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {D4E5F678-90AB-CDEF-1234-567890123456} + EndGlobalSection +End diff --git a/tests/MUnique.OpenMU.Pathfinding.Benchmarks/PathFinderBenchmarks.cs b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/PathFinderBenchmarks.cs new file mode 100644 index 0000000000..d6433a33cd --- /dev/null +++ b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/PathFinderBenchmarks.cs @@ -0,0 +1,123 @@ +// +// Licensed under the MIT License. See LICENSE file in the project root for full license information. +// + +namespace MUnique.OpenMU.Pathfinding.Benchmarks; + +/// +/// Benchmarks for the A* implementation. +/// Covers the production scenarios: short-range +/// searches (pooled in GameLogic) and long-range +/// searches (bot travel), for both successful and failed searches. +/// +[MemoryDiagnoser] +[ThreadingDiagnoser] +public class PathFinderBenchmarks +{ + private PathFinder _scopedFinder = null!; + private PathFinder _fullFinder = null!; + private byte[,] _grid = null!; + + /// + /// Global setup: builds a 256x256 terrain resembling the unit tests, + /// plus a wall with a gap to force non-trivial searches. + /// + [GlobalSetup] + public void Setup() + { + this._grid = new byte[0x100, 0x100]; + for (int x = 100; x < 200; x++) + { + for (int y = 100; y < 200; y++) + { + this._grid[x, y] = 10; + } + } + + // Safezone block: + for (int x = 50; x < 100; x++) + { + for (int y = 50; y < 100; y++) + { + this._grid[x, y] = 0b1000_0001; + } + } + + // Vertical wall at x=120 (y 100..130) with a gap at y=115, + // to force the longer benchmarks around an obstacle. + for (int y = 100; y <= 130; y++) + { + if (y != 115) + { + this._grid[120, y] = 0; + } + } + + this._scopedFinder = new PathFinder(new ScopedGridNetwork()); + this._fullFinder = new PathFinder(new FullGridNetwork(true)); + } + + /// + /// Short straight path on the scoped network (5 steps). + /// + [Benchmark(Baseline = true)] + public IList? Scoped_ShortStraightPath() + { + return this._scopedFinder.FindPath(new Point(110, 100), new Point(115, 100), this._grid, false); + } + + /// + /// Diagonal path on the scoped network (10 steps). + /// + [Benchmark] + public IList? Scoped_DiagonalPath() + { + return this._scopedFinder.FindPath(new Point(100, 100), new Point(110, 110), this._grid, false); + } + + /// + /// Longer path around a wall on the scoped network (max 16x16 segment). + /// Start and end are on opposite sides of a wall with a single gap, + /// forcing the search to detour. + /// + [Benchmark] + public IList? Scoped_LongerPathAroundWall() + { + return this._scopedFinder.FindPath(new Point(118, 110), new Point(122, 120), this._grid, false); + } + + /// + /// Unreachable target on the scoped network (failure path). + /// + [Benchmark] + public IList? Scoped_UnreachableTarget() + { + return this._scopedFinder.FindPath(new Point(110, 100), new Point(115, 99), this._grid, false); + } + + /// + /// Longer path on the full-grid network (bot travel scenario). + /// + [Benchmark] + public IList? FullGrid_LongerPath() + { + return this._fullFinder.FindPath(new Point(100, 100), new Point(150, 150), this._grid, false); + } + + /// + /// Longer path with a maximum distance constraint (exercises distance checks). + /// + [Benchmark] + public IList? Scoped_WithMaximumDistance() + { + this._scopedFinder.MaximumDistance = 100; + try + { + return this._scopedFinder.FindPath(new Point(118, 110), new Point(122, 120), this._grid, false); + } + finally + { + this._scopedFinder.MaximumDistance = 0; + } + } +} diff --git a/tests/MUnique.OpenMU.Pathfinding.Benchmarks/Program.cs b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/Program.cs new file mode 100644 index 0000000000..e2cb727baa --- /dev/null +++ b/tests/MUnique.OpenMU.Pathfinding.Benchmarks/Program.cs @@ -0,0 +1,28 @@ +// +// Licensed under the MIT License. See LICENSE file in the project root for full license information. +// + +#pragma warning disable SA1200 + +global using BenchmarkDotNet.Attributes; +global using BenchmarkDotNet.Jobs; +global using BenchmarkDotNet.Running; + +#pragma warning restore SA1200 + +namespace MUnique.OpenMU.Pathfinding.Benchmarks; + +/// +/// The class of the entry point of the benchmark. +/// +public static class Program +{ + /// + /// The entry point of the benchmark. + /// + /// The arguments. + public static void Main(string[] args) + { + BenchmarkSwitcher.FromAssembly(typeof(Program).Assembly).Run(args); + } +}