PathFinder minor optimizations.

OpenRA.Game/Primitives/PriorityQueue.cs

@@ -20,6 +20,8 @@ public interface IPriorityQueue<T>
 		bool Empty { get; }
 		T Peek();
 		T Pop();
+		bool TryPeek(out T elem);
+		bool TryPop(out T elem);
 	}
 
 	public class PriorityQueue<T> : IPriorityQueue<T>
@@ -42,11 +44,17 @@ public void Add(T item)
 			var addLevel = level;
 			var addIndex = index;
 
-			while (addLevel >= 1 && comparer.Compare(Above(addLevel, addIndex), item) > 0)
+			while (addLevel >= 1)
 			{
-				items[addLevel][addIndex] = Above(addLevel, addIndex);
-				--addLevel;
-				addIndex >>= 1;
+				var above = Above(addLevel, addIndex);
+				if (comparer.Compare(above, item) > 0)
+				{
+					items[addLevel][addIndex] = above;
+					--addLevel;
+					addIndex >>= 1;
+				}
+				else
+					break;
 			}
 
 			items[addLevel][addIndex] = item;
@@ -91,6 +99,33 @@ public T Pop()
 			return ret;
 		}
 
+		public bool TryPeek(out T item)
+		{
+			if (level == 0)
+			{
+				item = default;
+				return false;
+			}
+
+			item = At(0, 0);
+			return true;
+		}
+
+		public bool TryPop(out T item)
+		{
+			if (level == 0)
+			{
+				item = default;
+				return false;
+			}
+
+			item = At(0, 0);
+			BubbleInto(0, 0, Last());
+			if (--index < 0)
+				index = (1 << --level) - 1;
+			return true;
+		}
+
 		void BubbleInto(int intoLevel, int intoIndex, T val)
 		{
 			var downLevel = intoLevel + 1;

OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs

@@ -59,7 +59,7 @@ public interface IPathSearch : IDisposable
 		bool IsTarget(CPos location);
 
 		bool CanExpand { get; }
-		CPos Expand();
+		bool TryExpand(out CPos cell);
 	}
 
 	public abstract class BasePathSearch : IPathSearch
@@ -177,7 +177,7 @@ public bool IsTarget(CPos location)
 		}
 
 		public bool CanExpand => !OpenQueue.Empty;
-		public abstract CPos Expand();
+		public abstract bool TryExpand(out CPos cell);
 
 		protected virtual void Dispose(bool disposing)
 		{

OpenRA.Mods.Common/Pathfinder/PathSearch.cs

@@ -98,18 +98,31 @@ protected override void AddInitialCell(CPos location)
 
 		/// <summary>
 		/// This function analyzes the neighbors of the most promising node in the Pathfinding graph
-		/// using the A* algorithm (A-star) and returns that node
+		/// using the A* algorithm (A-star).
 		/// </summary>
-		/// <returns>The most promising node of the iteration</returns>
-		public override CPos Expand()
+		/// <param name="mostPromisingNode">If the graph can be expanded, contains the next lowest cost cell.</param>
+		/// <returns>True if the graph was expanded and mostPromisingNode contains a valid cell, otherwise false.</returns>
+
+		public override bool TryExpand(out CPos mostPromisingNode)
 		{
-			var currentMinNode = OpenQueue.Pop().Destination;
+			if (!OpenQueue.TryPop(out var currentMinConnection))
+			{
+				mostPromisingNode = default(CPos);
+				return false;
+			}
+
+			var currentMinNode = currentMinConnection.Destination;
 
 			var currentCell = Graph[currentMinNode];
 			Graph[currentMinNode] = new CellInfo(currentCell.CostSoFar, currentCell.EstimatedTotal, currentCell.PreviousPos, CellStatus.Closed);
 
-			if (Graph.CustomCost != null && Graph.CustomCost(currentMinNode) == PathGraph.CostForInvalidCell)
-				return currentMinNode;
+			var customCost = Graph.CustomCost;
+
+			if (customCost != null && customCost(currentMinNode) == PathGraph.CostForInvalidCell)
+			{
+				mostPromisingNode = currentMinNode;
+				return true;
+			}
 
 			foreach (var connection in Graph.GetConnections(currentMinNode))
 			{
@@ -147,7 +160,8 @@ public override CPos Expand()
 				}
 			}
 
-			return currentMinNode;
+			mostPromisingNode = currentMinNode;
+			return true;
 		}
 	}
 }

OpenRA.Mods.Common/Traits/World/PathFinder.cs

@@ -132,11 +132,10 @@ public List<CPos> FindUnitPathToRange(CPos source, SubCell srcSub, WPos target,
 
 		public List<CPos> FindPath(IPathSearch search)
 		{
-			List<CPos> path = null;
+			List<CPos> path = EmptyPath;
 
-			while (search.CanExpand)
+			while (search.TryExpand(out var p))
 			{
-				var p = search.Expand();
 				if (search.IsTarget(p))
 				{
 					path = MakePath(search.Graph, p);
@@ -146,34 +145,29 @@ public List<CPos> FindPath(IPathSearch search)
 
 			search.Graph.Dispose();
 
-			if (path != null)
-				return path;
-
-			// no path exists
-			return EmptyPath;
+			return path;
 		}
 
 		// Searches from both ends toward each other. This is used to prevent blockings in case we find
 		// units in the middle of the path that prevent us to continue.
 		public List<CPos> FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest)
 		{
-			List<CPos> path = null;
+			List<CPos> path = EmptyPath;
 
-			while (fromSrc.CanExpand && fromDest.CanExpand)
+			while (fromSrc.TryExpand(out var p) && fromDest.TryExpand(out var q))
 			{
 				// make some progress on the first search
-				var p = fromSrc.Expand();
-				if (fromDest.Graph[p].Status == CellStatus.Closed &&
-					fromDest.Graph[p].CostSoFar < int.MaxValue)
+				var pci = fromDest.Graph[p];
+				if (pci.Status == CellStatus.Closed && pci.CostSoFar < int.MaxValue)
 				{
 					path = MakeBidiPath(fromSrc, fromDest, p);
 					break;
 				}
 
 				// make some progress on the second search
-				var q = fromDest.Expand();
-				if (fromSrc.Graph[q].Status == CellStatus.Closed &&
-					fromSrc.Graph[q].CostSoFar < int.MaxValue)
+				var qci = fromSrc.Graph[q];
+				if (qci.Status == CellStatus.Closed &&
+					qci.CostSoFar < int.MaxValue)
 				{
 					path = MakeBidiPath(fromSrc, fromDest, q);
 					break;
@@ -183,10 +177,7 @@ public List<CPos> FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest)
 			fromSrc.Graph.Dispose();
 			fromDest.Graph.Dispose();
 
-			if (path != null)
-				return path;
-
-			return EmptyPath;
+			return path;
 		}
 
 		// Build the path from the destination. When we find a node that has the same previous
@@ -195,11 +186,12 @@ static List<CPos> MakePath(IGraph<CellInfo> cellInfo, CPos destination)
 		{
 			var ret = new List<CPos>();
 			var currentNode = destination;
-
-			while (cellInfo[currentNode].PreviousPos != currentNode)
+			var prevNode = cellInfo[currentNode].PreviousPos;
+			while (prevNode != currentNode)
 			{
 				ret.Add(currentNode);
-				currentNode = cellInfo[currentNode].PreviousPos;
+				currentNode = prevNode;
+				prevNode = cellInfo[currentNode].PreviousPos;
 			}
 
 			ret.Add(currentNode);
@@ -214,21 +206,25 @@ static List<CPos> MakeBidiPath(IPathSearch a, IPathSearch b, CPos confluenceNode
 			var ret = new List<CPos>();
 
 			var q = confluenceNode;
+			var prevQ = ca[q].PreviousPos;
 			while (ca[q].PreviousPos != q)
 			{
 				ret.Add(q);
-				q = ca[q].PreviousPos;
+				q = prevQ;
+				prevQ = ca[q].PreviousPos;
 			}
 
 			ret.Add(q);
 
 			ret.Reverse();
 
 			q = confluenceNode;
-			while (cb[q].PreviousPos != q)
+			prevQ = cb[q].PreviousPos;
+			while (prevQ != q)
 			{
-				q = cb[q].PreviousPos;
+				q = prevQ;
 				ret.Add(q);
+				prevQ = cb[q].PreviousPos;
 			}
 
 			return ret;