Rework PriorityQueue for performance.

- Providing the comparer as a type argument that is a struct allows the calls to be devirtualised, leading to approx a 3x performance improvement.
- Use a single backing array, rather than a list of arrays.

OpenRA.Game/Primitives/PriorityQueue.cs

@@ -22,19 +22,39 @@ namespace OpenRA.Primitives
 		T Pop();
 	}
 
-	public class PriorityQueue<T> : IPriorityQueue<T>
+	/// <summary>
+	/// Represents a collection of items that have a priority.
+	/// On pop, the item with the lowest priority value is removed.
+	/// </summary>
+	public sealed class PriorityQueue<T, TComparer> : IPriorityQueue<T> where TComparer : struct, IComparer<T>
 	{
-		readonly List<T[]> items;
+		/// <summary>
-		readonly IComparer<T> comparer;
+		/// Compares two items to determine their priority.
-		int level, index;
+		/// PERF: Using a struct allows the calls to be devirtualized.
+		/// </summary>
+		readonly TComparer comparer;
 
-		public PriorityQueue()
+		/// <summary>
-			: this(Comparer<T>.Default) { }
+		/// A <a href="https://en.wikipedia.org/wiki/Binary_heap">binary min-heap</a> storing the items.
+		/// An array divided into sub arrays called levels. At each level the size of a level array doubles.
+		/// Elements at deeper levels always have higher priority values than elements nearer to the root.
+		/// </summary>
+		T[] items;
 
-		public PriorityQueue(IComparer<T> comparer)
+		/// <summary>
+		/// Index of deepest level.
+		/// </summary>
+		int level;
+
+		/// <summary>
+		/// Number of elements in the deepest level.
+		/// </summary>
+		int index;
+
+		public PriorityQueue(TComparer comparer)
 		{
-			items = new List<T[]> { new T[1] };
 			this.comparer = comparer;
+			items = new T[1];
 		}
 
 		public void Add(T item)
@@ -42,29 +62,37 @@ namespace OpenRA.Primitives
 			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);
+				var above = items[AboveIndex(addLevel, addIndex)];
-				--addLevel;
+				if (comparer.Compare(above, item) > 0)
-				addIndex >>= 1;
+				{
+					items[Index(addLevel, addIndex)] = above;
+					--addLevel;
+					addIndex >>= 1;
+				}
+				else
+					break;
 			}
 
-			items[addLevel][addIndex] = item;
+			items[Index(addLevel, addIndex)] = item;
 
 			if (++index >= 1 << level)
 			{
 				index = 0;
-				if (items.Count <= ++level)
+				var count = 2 * (1 << ++level);
-					items.Add(new T[1 << level]);
+				if (count - 1 >= items.Length)
+					Array.Resize(ref items, count);
 			}
 		}
 
 		public bool Empty => level == 0;
 
-		T At(int level, int index) { return items[level][index]; }
+		static int Index(int level, int index) { return (1 << level) - 1 + index; }
-		T Above(int level, int index) { return items[level - 1][index >> 1]; }
 
-		T Last()
+		static int AboveIndex(int level, int index) { return (1 << (level - 1)) - 1 + (index >> 1); }
+
+		int IndexLast()
 		{
 			var lastLevel = level;
 			var lastIndex = index;
@@ -72,20 +100,21 @@ namespace OpenRA.Primitives
 			if (--lastIndex < 0)
 				lastIndex = (1 << --lastLevel) - 1;
 
-			return At(lastLevel, lastIndex);
+			return Index(lastLevel, lastIndex);
 		}
 
 		public T Peek()
 		{
 			if (level <= 0 && index <= 0)
 				throw new InvalidOperationException("PriorityQueue empty.");
-			return At(0, 0);
+
+			return items[Index(0, 0)];
 		}
 
 		public T Pop()
 		{
 			var ret = Peek();
-			BubbleInto(0, 0, Last());
+			BubbleInto(0, 0, items[IndexLast()]);
 			if (--index < 0)
 				index = (1 << --level) - 1;
 			return ret;
@@ -93,27 +122,38 @@ namespace OpenRA.Primitives
 
 		void BubbleInto(int intoLevel, int intoIndex, T val)
 		{
-			var downLevel = intoLevel + 1;
+			while (true)
-			var downIndex = intoIndex << 1;
-
-			if (downLevel > level || (downLevel == level && downIndex >= index))
 			{
-				items[intoLevel][intoIndex] = val;
+				var downLevel = intoLevel + 1;
-				return;
+				var downIndex = intoIndex << 1;
+
+				if (downLevel > level || (downLevel == level && downIndex >= index))
+				{
+					items[Index(intoLevel, intoIndex)] = val;
+					return;
+				}
+
+				var down = items[Index(downLevel, downIndex)];
+				if (downLevel < level || (downLevel == level && downIndex < index - 1))
+				{
+					var downRight = items[Index(downLevel, downIndex + 1)];
+					if (comparer.Compare(down, downRight) >= 0)
+					{
+						down = downRight;
+						++downIndex;
+					}
+				}
+
+				if (comparer.Compare(val, down) <= 0)
+				{
+					items[Index(intoLevel, intoIndex)] = val;
+					return;
+				}
+
+				items[Index(intoLevel, intoIndex)] = down;
+				intoLevel = downLevel;
+				intoIndex = downIndex;
 			}
-
-			if ((downLevel < level || (downLevel == level && downIndex < index - 1)) &&
-				comparer.Compare(At(downLevel, downIndex), At(downLevel, downIndex + 1)) >= 0)
-				++downIndex;
-
-			if (comparer.Compare(val, At(downLevel, downIndex)) <= 0)
-			{
-				items[intoLevel][intoIndex] = val;
-				return;
-			}
-
-			items[intoLevel][intoIndex] = At(downLevel, downIndex);
-			BubbleInto(downLevel, downIndex, val);
 		}
 	}
 }

OpenRA.Mods.Common/Pathfinder/IPathGraph.cs

@@ -77,12 +77,8 @@ namespace OpenRA.Mods.Common.Pathfinder
 	/// </summary>
 	public readonly struct GraphConnection
 	{
-		public static readonly CostComparer ConnectionCostComparer = CostComparer.Instance;
+		public readonly struct CostComparer : IComparer<GraphConnection>
-
-		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);

OpenRA.Mods.Common/Pathfinder/PathSearch.cs

@@ -170,7 +170,7 @@ namespace OpenRA.Mods.Common.Pathfinder
 			this.heuristicWeightPercentage = heuristicWeightPercentage;
 			TargetPredicate = targetPredicate;
 			this.recorder = recorder;
-			openQueue = new PriorityQueue<GraphConnection>(GraphConnection.ConnectionCostComparer);
+			openQueue = new Primitives.PriorityQueue<GraphConnection, GraphConnection.CostComparer>(default);
 		}
 
 		void AddInitialCell(CPos location, Func<CPos, int> customCost)

OpenRA.Test/OpenRA.Game/PriorityQueueTest.cs

@@ -10,10 +10,10 @@
 #endregion
 
 using System;
+using System.Collections.Generic;
 using System.Linq;
 using NUnit.Framework;
 using OpenRA.Mods.Common;
-using OpenRA.Primitives;
 using OpenRA.Support;
 
 namespace OpenRA.Test
@@ -21,6 +21,11 @@ namespace OpenRA.Test
 	[TestFixture]
 	class PriorityQueueTest
 	{
+		readonly struct Int32Comparer : IComparer<int>
+		{
+			public int Compare(int x, int y) => x.CompareTo(y);
+		}
+
 		[TestCase(1, 123)]
 		[TestCase(1, 1234)]
 		[TestCase(1, 12345)]
@@ -51,7 +56,7 @@ namespace OpenRA.Test
 			var values = Enumerable.Range(0, count);
 			var shuffledValues = values.Shuffle(mt).ToArray();
 
-			var queue = new PriorityQueue<int>();
+			var queue = new Primitives.PriorityQueue<int, Int32Comparer>(default);
 
 			Assert.IsTrue(queue.Empty, "New queue should start out empty.");
 			Assert.Throws<InvalidOperationException>(() => queue.Peek(), "Peeking at an empty queue should throw.");
@@ -95,7 +100,7 @@ namespace OpenRA.Test
 			var mt = new MersenneTwister(seed);
 			var shuffledValues = Enumerable.Range(0, count).Shuffle(mt).ToArray();
 
-			var queue = new PriorityQueue<int>();
+			var queue = new Primitives.PriorityQueue<int, Int32Comparer>(default);
 
 			Assert.IsTrue(queue.Empty, "New queue should start out empty.");
 			Assert.Throws<InvalidOperationException>(() => queue.Peek(), "Peeking at an empty queue should throw.");