Fix pathfinding using PriorityQueue incorrectly.

By providing a comparer that could change over time (as estimated costs on the graph were updated), this meant the priority queue could have its heap property invalidated and thus not maintain a correct ordering. Instead we store elements into the queue with their estimations at the time. This preserves the heap property and thus ensures the queue returns properly ordered results, although it may contain out of date estimations.

This also improves performance. The fixed comparer need not perform expensive lookups into the graph, but can instead use the readily available value. This speeds up adds and removes on the queue significantly.

OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs

@@ -65,7 +65,7 @@ namespace OpenRA.Mods.Common.Pathfinder
 	{
 		public IGraph<CellInfo> Graph { get; set; }
 
-		protected IPriorityQueue<CPos> OpenQueue { get; private set; }
+		protected IPriorityQueue<GraphConnection> OpenQueue { get; private set; }
 
 		public abstract IEnumerable<Pair<CPos, int>> Considered { get; }
 
@@ -80,13 +80,13 @@ namespace OpenRA.Mods.Common.Pathfinder
 		// points considered and their Heuristics to reach
 		// the target. It pretty match identifies, in conjunction of the Actor,
 		// a deterministic set of calculations
-		protected readonly IPriorityQueue<CPos> startPoints;
+		protected readonly IPriorityQueue<GraphConnection> StartPoints;
 
 		protected BasePathSearch(IGraph<CellInfo> graph)
 		{
 			Graph = graph;
-			OpenQueue = new PriorityQueue<CPos>(new PositionComparer(Graph));
-			startPoints = new PriorityQueue<CPos>(new PositionComparer(Graph));
+			OpenQueue = new PriorityQueue<GraphConnection>(GraphConnection.ConnectionCostComparer);
+			StartPoints = new PriorityQueue<GraphConnection>(GraphConnection.ConnectionCostComparer);
 			Debug = false;
 			MaxCost = 0;
 		}

OpenRA.Mods.Common/Pathfinder/CellInfo.cs

@@ -57,22 +57,4 @@ namespace OpenRA.Mods.Common.Pathfinder
 			EstimatedTotal = estimatedTotal;
 		}
 	}
-
-	/// <summary>
-	/// Compares two nodes according to their estimations
-	/// </summary>
-	public class PositionComparer : IComparer<CPos>
-	{
-		readonly IGraph<CellInfo> graph;
-
-		public PositionComparer(IGraph<CellInfo> graph)
-		{
-			this.graph = graph;
-		}
-
-		public int Compare(CPos x, CPos y)
-		{
-			return Math.Sign(graph[x].EstimatedTotal - graph[y].EstimatedTotal);
-		}
-	}
 }

OpenRA.Mods.Common/Pathfinder/PathGraph.cs

@@ -47,6 +47,18 @@ namespace OpenRA.Mods.Common.Pathfinder
 
 	public struct GraphConnection
 	{
+		public static readonly CostComparer ConnectionCostComparer = CostComparer.Instance;
+
+		public sealed class CostComparer : IComparer<GraphConnection>
+		{
+			public static readonly CostComparer Instance = new CostComparer();
+			CostComparer() { }
+			public int Compare(GraphConnection x, GraphConnection y)
+			{
+				return x.Cost.CompareTo(y.Cost);
+			}
+		}
+
 		public readonly CPos Destination;
 		public readonly int Cost;
 

OpenRA.Mods.Common/Pathfinder/PathSearch.cs

@@ -84,9 +84,11 @@ namespace OpenRA.Mods.Common.Pathfinder
 
 		protected override void AddInitialCell(CPos location)
 		{
-			Graph[location] = new CellInfo(0, heuristic(location), location, CellStatus.Open);
-			OpenQueue.Add(location);
-			startPoints.Add(location);
+			var cost = heuristic(location);
+			Graph[location] = new CellInfo(0, cost, location, CellStatus.Open);
+			var connection = new GraphConnection(location, cost);
+			OpenQueue.Add(connection);
+			StartPoints.Add(connection);
 			considered.AddLast(new Pair<CPos, int>(location, 0));
 		}
 
@@ -99,7 +101,7 @@ namespace OpenRA.Mods.Common.Pathfinder
 		/// <returns>The most promising node of the iteration</returns>
 		public override CPos Expand()
 		{
-			var currentMinNode = OpenQueue.Pop();
+			var currentMinNode = OpenQueue.Pop().Destination;
 
 			var currentCell = Graph[currentMinNode];
 			Graph[currentMinNode] = new CellInfo(currentCell.CostSoFar, currentCell.EstimatedTotal, currentCell.PreviousPos, CellStatus.Closed);
@@ -128,10 +130,11 @@ namespace OpenRA.Mods.Common.Pathfinder
 				else
 					hCost = heuristic(neighborCPos);
 
-				Graph[neighborCPos] = new CellInfo(gCost, gCost + hCost, currentMinNode, CellStatus.Open);
+				var estimatedCost = gCost + hCost;
+				Graph[neighborCPos] = new CellInfo(gCost, estimatedCost, currentMinNode, CellStatus.Open);
 
 				if (neighborCell.Status != CellStatus.Open)
-					OpenQueue.Add(neighborCPos);
+					OpenQueue.Add(new GraphConnection(neighborCPos, estimatedCost));
 
 				if (Debug)
 				{