Add benchmark to PathFinder and fix search hot spots

src/Pathfinding/BinaryMinHeap{T}.cs

@@ -21,18 +21,6 @@ public class BinaryMinHeap<T> : IPriorityQueue<T>
     private readonly List<T> _innerList = new();
     private readonly IComparer<T> _elementComparer;
 
-    /// <summary>Reused variable to reduce stack allocations.</summary>
-    private int _i;
-
-    /// <summary>Reused variable to reduce stack allocations.</summary>
-    private int _parentIndex;
-
-    /// <summary>Reused variable to reduce stack allocations.</summary>
-    private int _left;
-
-    /// <summary>Reused variable to reduce stack allocations.</summary>
-    private int _right;
-
     /// <summary>
     /// Initializes a new instance of the <see cref="BinaryMinHeap{T}"/> class.
     /// </summary>
@@ -67,27 +55,19 @@ public BinaryMinHeap(IComparer<T> comparer, int capacity)
     /// <inheritdoc/>
     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);
     }
 
     /// <inheritdoc/>
@@ -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]);
     }
 }

src/Pathfinding/FullGridNetwork.cs

@@ -40,9 +40,10 @@ public override Node GetNodeAt(Point position)
     /// <inheritdoc/>
     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);

src/Pathfinding/PathFinder.cs

@@ -24,6 +24,8 @@ public class PathFinder : IPathFinder
 
     private readonly INetwork _network;
     private readonly IPriorityQueue<Node> _openList;
+    private int _maximumDistance;
+    private int _maximumDistanceSquared;
 
     /// <summary>
     /// Initializes a new instance of the <see cref="PathFinder"/> class.
@@ -52,8 +54,17 @@ public PathFinder(INetwork network, IPriorityQueue<Node> openList)
 
     /// <summary>
     /// Gets or sets the maximum distance until which the path should be resolved.
+    /// A value of 0 disables the check.
     /// </summary>
-    public int MaximumDistance { get; set; }
+    public int MaximumDistance
+    {
+        get => this._maximumDistance;
+        set
+        {
+            this._maximumDistance = value;
+            this._maximumDistanceSquared = value * value;
+        }
+    }
 
     /// <summary>
     /// Gets or sets the search limit.
@@ -78,7 +89,7 @@ public PathFinder(INetwork network, IPriorityQueue<Node> 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,43 +186,61 @@ 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;
             this._openList.Push(newNode);
         }
     }
 
-    private IEnumerable<PathResultNode> GetCalculatedPath(Point end)
+    private List<PathResultNode> GetCalculatedPath(Point end)
     {
+        var path = new List<PathResultNode>();
         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;
     }
 }

src/Pathfinding/Point.cs

@@ -51,7 +51,20 @@ public record struct Point(byte X, byte Y)
     /// <returns>The distance between this point and another point.</returns>
     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));
+    }
+
+    /// <summary>
+    /// Gets the squared euclidean distance between this point and another point.
+    /// Use this for distance comparisons to avoid the expensive square root.
+    /// </summary>
+    /// <param name="otherPoint">The other point.</param>
+    /// <returns>The squared distance between this point and another point.</returns>
+    public int EuclideanDistanceSquaredTo(Point otherPoint)
+    {
+        var dx = this.X - otherPoint.X;
+        var dy = this.Y - otherPoint.Y;
+        return (dx * dx) + (dy * dy);
     }
 
     /// <inheritdoc/>

src/Pathfinding/ScopedGridNetwork.cs

@@ -4,6 +4,8 @@
 
 namespace MUnique.OpenMU.Pathfinding;
 
+using System.Numerics;
+
 /// <summary>
 /// 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);

tests/MUnique.OpenMU.Pathfinding.Benchmarks/MUnique.OpenMU.Pathfinding.Benchmarks.csproj

@@ -0,0 +1,44 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net10.0</TargetFramework>
+    <Nullable>enable</Nullable>
+    <WarningsAsErrors>nullable;CS4014;VSTHRD110;VSTHRD100</WarningsAsErrors>
+    <AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
+    <GenerateAssemblyInfo>false</GenerateAssemblyInfo>
+    <ApplicationIcon />
+    <OutputType>Exe</OutputType>
+    <StartupObject />
+    <AssemblyName>MUnique.OpenMU.Pathfinding.Benchmarks</AssemblyName>
+    <PlatformTarget>AnyCPU</PlatformTarget>
+  </PropertyGroup>
+
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|AnyCPU'">
+    <OutputPath>bin\Debug\</OutputPath>
+    <DocumentationFile>bin\Debug\MUnique.OpenMU.Pathfinding.Benchmarks.xml</DocumentationFile>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|AnyCPU'">
+    <OutputPath>bin\Release\</OutputPath>
+	<DocumentationFile>bin\Release\MUnique.OpenMU.Pathfinding.Benchmarks.xml</DocumentationFile>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <Compile Include="..\..\src\SharedAssemblyInfo.cs" Link="Properties\SharedAssemblyInfo.cs" />
+    <Compile Include="..\..\src\SharedGlobalUsings.cs" Link="SharedGlobalUsings.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="..\..\src\.editorconfig" Link=".editorconfig" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <AdditionalFiles Include="..\..\src\stylecop.json" Link="stylecop.json" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <PackageReference Include="BenchmarkDotNet" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\..\src\Pathfinding\MUnique.OpenMU.Pathfinding.csproj" />
+  </ItemGroup>
+</Project>

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