Fix issues preventing suboptimal path searches.

Two different issues were causing a path search to not explore cells in order of the cheapest estimated route first. This meant the search could sometimes miss a cheaper route and return a suboptimal path.

- PriorityQueue had a bug which would cause it to not check some elements when restoring the heap property of its internal data structure. Failing to do this would invalidate the heap property, meaning it would not longer return the items in correct priority order. Additional tests ensure this is covered.
- When a path search encountered the same cell again with a lower cost, it would not update the priority queue with the new cost. This meant the cell was not explored early enough as it was in the queue with its original, higher cost. Exploring other paths might close off surrounding cells, preventing the cell with the lower cost from progressing. Instead we now add a duplicate with the lower cost to ensure it gets explored at the right time. We remove the duplicate with the higher cost in CanExpand by checking for already Closed cells.

OpenRA.Game/Primitives/PriorityQueue.cs

@@ -102,7 +102,7 @@ namespace OpenRA.Primitives
 				return;
 			}
 
-			if (downLevel <= level && downIndex < index - 1 &&
+			if ((downLevel < level || (downLevel == level && downIndex < index - 1)) &&
 				comparer.Compare(At(downLevel, downIndex), At(downLevel, downIndex + 1)) >= 0)
 				++downIndex;
 

OpenRA.Mods.Common/Pathfinder/PathSearch.cs

@@ -153,7 +153,25 @@ namespace OpenRA.Mods.Common.Pathfinder
 		/// Determines if there are more reachable cells and the search can be continued.
 		/// If false, <see cref="Expand"/> can no longer be called.
 		/// </summary>
-		bool CanExpand => !openQueue.Empty;
+		bool CanExpand()
+		{
+			// Connections to a cell can appear more than once if a search discovers a lower cost route to the cell.
+			// The lower cost gets processed first and the cell will be Closed.
+			// When checking if we can expand, pop any Closed cells off the queue, so Expand will only see Open cells.
+			CellStatus status;
+			do
+			{
+				if (openQueue.Empty)
+					return false;
+
+				status = Graph[openQueue.Peek().Destination].Status;
+				if (status == CellStatus.Closed)
+					openQueue.Pop();
+			}
+			while (status == CellStatus.Closed);
+
+			return true;
+		}
 
 		/// <summary>
 		/// This function analyzes the neighbors of the most promising node in the pathfinding graph
@@ -191,8 +209,6 @@ namespace OpenRA.Mods.Common.Pathfinder
 
 				var estimatedTotalCostToTarget = costSoFarToNeighbor + estimatedRemainingCostToTarget;
 				Graph[neighbor] = new CellInfo(CellStatus.Open, costSoFarToNeighbor, estimatedTotalCostToTarget, currentMinNode);
-
-				if (neighborInfo.Status != CellStatus.Open)
 				openQueue.Add(new GraphConnection(neighbor, estimatedTotalCostToTarget));
 			}
 
@@ -205,7 +221,7 @@ namespace OpenRA.Mods.Common.Pathfinder
 		/// </summary>
 		public List<CPos> FindPath()
 		{
-			while (CanExpand)
+			while (CanExpand())
 			{
 				var p = Expand();
 				if (TargetPredicate(p))
@@ -238,7 +254,7 @@ namespace OpenRA.Mods.Common.Pathfinder
 		/// </summary>
 		public static List<CPos> FindBidiPath(PathSearch first, PathSearch second)
 		{
-			while (first.CanExpand && second.CanExpand)
+			while (first.CanExpand() && second.CanExpand())
 			{
 				// make some progress on the first search
 				var p = first.Expand();

OpenRA.Test/OpenRA.Game/PriorityQueueTest.cs

@@ -10,30 +10,133 @@
 #endregion
 
 using System;
+using System.Linq;
 using NUnit.Framework;
+using OpenRA.Mods.Common;
 using OpenRA.Primitives;
+using OpenRA.Support;
 
 namespace OpenRA.Test
 {
 	[TestFixture]
 	class PriorityQueueTest
 	{
-		[TestCase(TestName = "PriorityQueue maintains invariants when adding and removing items.")]
+		[TestCase(1, 123)]
-		public void PriorityQueueGeneralTest()
+		[TestCase(1, 1234)]
+		[TestCase(1, 12345)]
+		[TestCase(2, 123)]
+		[TestCase(2, 1234)]
+		[TestCase(2, 12345)]
+		[TestCase(10, 123)]
+		[TestCase(10, 1234)]
+		[TestCase(10, 12345)]
+		[TestCase(15, 123)]
+		[TestCase(15, 1234)]
+		[TestCase(15, 12345)]
+		[TestCase(16, 123)]
+		[TestCase(16, 1234)]
+		[TestCase(16, 12345)]
+		[TestCase(17, 123)]
+		[TestCase(17, 1234)]
+		[TestCase(17, 12345)]
+		[TestCase(100, 123)]
+		[TestCase(100, 1234)]
+		[TestCase(100, 12345)]
+		[TestCase(1000, 123)]
+		[TestCase(1000, 1234)]
+		[TestCase(1000, 12345)]
+		public void PriorityQueueAddThenRemoveTest(int count, int seed)
 		{
+			var mt = new MersenneTwister(seed);
+			var values = Enumerable.Range(0, count);
+			var shuffledValues = values.Shuffle(mt).ToArray();
+
 			var queue = new PriorityQueue<int>();
 
 			Assert.IsTrue(queue.Empty, "New queue should start out empty.");
 			Assert.Throws<InvalidOperationException>(() => queue.Peek(), "Peeking at an empty queue should throw.");
 			Assert.Throws<InvalidOperationException>(() => queue.Pop(), "Popping an empty queue should throw.");
 
-			foreach (var value in new[] { 4, 3, 5, 1, 2 })
+			foreach (var value in shuffledValues)
 			{
 				queue.Add(value);
 				Assert.IsFalse(queue.Empty, "Queue should not be empty - items have been added.");
 			}
 
-			foreach (var value in new[] { 1, 2, 3, 4, 5 })
+			foreach (var value in values)
+			{
+				Assert.AreEqual(value, queue.Peek(), "Peek returned the wrong item - should be in order.");
+				Assert.IsFalse(queue.Empty, "Queue should not be empty yet.");
+				Assert.AreEqual(value, queue.Pop(), "Pop returned the wrong item - should be in order.");
+			}
+
+			Assert.IsTrue(queue.Empty, "Queue should now be empty.");
+			Assert.Throws<InvalidOperationException>(() => queue.Peek(), "Peeking at an empty queue should throw.");
+			Assert.Throws<InvalidOperationException>(() => queue.Pop(), "Popping an empty queue should throw.");
+		}
+
+		[TestCase(15, 123)]
+		[TestCase(15, 1234)]
+		[TestCase(15, 12345)]
+		[TestCase(16, 123)]
+		[TestCase(16, 1234)]
+		[TestCase(16, 12345)]
+		[TestCase(17, 123)]
+		[TestCase(17, 1234)]
+		[TestCase(17, 12345)]
+		[TestCase(100, 123)]
+		[TestCase(100, 1234)]
+		[TestCase(100, 12345)]
+		[TestCase(1000, 123)]
+		[TestCase(1000, 1234)]
+		[TestCase(1000, 12345)]
+		public void PriorityQueueAddAndRemoveInterleavedTest(int count, int seed)
+		{
+			var mt = new MersenneTwister(seed);
+			var shuffledValues = Enumerable.Range(0, count).Shuffle(mt).ToArray();
+
+			var queue = new PriorityQueue<int>();
+
+			Assert.IsTrue(queue.Empty, "New queue should start out empty.");
+			Assert.Throws<InvalidOperationException>(() => queue.Peek(), "Peeking at an empty queue should throw.");
+			Assert.Throws<InvalidOperationException>(() => queue.Pop(), "Popping an empty queue should throw.");
+
+			foreach (var value in shuffledValues.Take(10))
+			{
+				queue.Add(value);
+				Assert.IsFalse(queue.Empty, "Queue should not be empty - items have been added.");
+			}
+
+			foreach (var value in shuffledValues.Take(10).OrderBy(x => x).Take(5))
+			{
+				Assert.AreEqual(value, queue.Peek(), "Peek returned the wrong item - should be in order.");
+				Assert.IsFalse(queue.Empty, "Queue should not be empty yet.");
+				Assert.AreEqual(value, queue.Pop(), "Pop returned the wrong item - should be in order.");
+			}
+
+			foreach (var value in shuffledValues.Skip(10).Take(5))
+			{
+				queue.Add(value);
+				Assert.IsFalse(queue.Empty, "Queue should not be empty - items have been added.");
+			}
+
+			foreach (var value in shuffledValues.Take(10).OrderBy(x => x).Skip(5)
+				.Concat(shuffledValues.Skip(10).Take(5)).OrderBy(x => x).Take(5))
+			{
+				Assert.AreEqual(value, queue.Peek(), "Peek returned the wrong item - should be in order.");
+				Assert.IsFalse(queue.Empty, "Queue should not be empty yet.");
+				Assert.AreEqual(value, queue.Pop(), "Pop returned the wrong item - should be in order.");
+			}
+
+			foreach (var value in shuffledValues.Skip(15))
+			{
+				queue.Add(value);
+				Assert.IsFalse(queue.Empty, "Queue should not be empty - items have been added.");
+			}
+
+			foreach (var value in shuffledValues.Take(10).OrderBy(x => x).Skip(5)
+				.Concat(shuffledValues.Skip(10).Take(5)).OrderBy(x => x).Skip(5)
+				.Concat(shuffledValues.Skip(15)).OrderBy(x => x))
 			{
 				Assert.AreEqual(value, queue.Peek(), "Peek returned the wrong item - should be in order.");
 				Assert.IsFalse(queue.Empty, "Queue should not be empty yet.");