diff --git a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs b/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs
index 8fba8f3e09..fab51285a3 100644
--- a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs
+++ b/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;
 		}
diff --git a/OpenRA.Mods.Common/Pathfinder/CellInfo.cs b/OpenRA.Mods.Common/Pathfinder/CellInfo.cs
index 57f3e9329e..a8112bc343 100644
--- a/OpenRA.Mods.Common/Pathfinder/CellInfo.cs
+++ b/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);
-		}
-	}
 }
diff --git a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs b/OpenRA.Mods.Common/Pathfinder/PathGraph.cs
index 3204b5282e..268e1331c2 100644
--- a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs
+++ b/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;
 
diff --git a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs b/OpenRA.Mods.Common/Pathfinder/PathSearch.cs
index 254317d030..c4f5355f7d 100644
--- a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs
+++ b/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)
 				{