Path finding optimizations. Skip 15% of closed cells early.

Refactor. Remove IPathSearch, IPathGraph. Introduce PathCacheKey and PathQuery.

OpenRA.Game/CPos.cs

@@ -85,6 +85,11 @@ public MPos ToMPos(MapGridType gridType)
 			return new MPos(u, v);
 		}
 
+		public CPos AtLayer(byte layer)
+		{
+			return new CPos((Bits & 0xff) | layer);
+		}
+
 		#region Scripting interface
 
 		public LuaValue Add(LuaRuntime runtime, LuaValue left, LuaValue right)

OpenRA.Game/CacheStorage.cs

@@ -11,10 +11,10 @@
 
 namespace OpenRA
 {
-	public interface ICacheStorage<T>
+	public interface ICacheStorage<K, V>
 	{
-		void Remove(string key);
-		void Store(string key, T data);
-		T Retrieve(string key);
+		void Remove(in K key);
+		void Store(in K key, V data);
+		V Retrieve(in K key);
 	}
 }

OpenRA.Game/Map/CellLayer.cs

@@ -49,10 +49,17 @@ public static CellLayer<T> CreateInstance(Func<MPos, T> initialCellValueFactory,
 			return cellLayer;
 		}
 
-		// Resolve an array index from cell coordinates
-		int Index(CPos cell)
+		int Index(CPos pos)
 		{
-			return Index(cell.ToMPos(GridType));
+			// PERF: Inline CPos.ToMPos to avoid MPos allocation
+			var x = pos.X;
+			var y = pos.Y;
+			if (GridType == MapGridType.Rectangular)
+				return y * Size.Width + x;
+
+			var u = (x - y) / 2;
+			var v = x + y;
+			return v * Size.Width + u;
 		}
 
 		// Resolve an array index from map coordinates

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>
@@ -41,10 +43,11 @@ public void Add(T item)
 		{
 			var addLevel = level;
 			var addIndex = index;
+			T above;
 
-			while (addLevel >= 1 && comparer.Compare(Above(addLevel, addIndex), item) > 0)
+			while (addLevel >= 1 && comparer.Compare((above = Above(addLevel, addIndex)), item) > 0)
 			{
-				items[addLevel][addIndex] = Above(addLevel, addIndex);
+				items[addLevel][addIndex] = above;
 				--addLevel;
 				addIndex >>= 1;
 			}
@@ -91,6 +94,33 @@ public T Pop()
 			return ret;
 		}
 
+		public bool TryPeek(out T item)
+		{
+			if (level == 0)
+			{
+				item = default(T);
+				return false;
+			}
+
+			item = At(0, 0);
+			return true;
+		}
+
+		public bool TryPop(out T item)
+		{
+			if (level == 0)
+			{
+				item = default(T);
+				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/Activities/FindAndDeliverResources.cs

@@ -180,42 +180,71 @@ public override bool Tick(Actor self)
 
 			var searchRadiusSquared = searchRadius * searchRadius;
 
-			var procPos = procLoc.HasValue ? (WPos?)self.World.Map.CenterOfCell(procLoc.Value) : null;
+			var map = self.World.Map;
+			var procPos = procLoc.HasValue ? (WPos?)map.CenterOfCell(procLoc.Value) : null;
 			var harvPos = self.CenterPosition;
 
 			// Find any harvestable resources:
 			List<CPos> path;
-			using (var search = PathSearch.Search(self.World, mobile.Locomotor, self, BlockedByActor.Stationary, loc =>
-					domainIndex.IsPassable(self.Location, loc, mobile.Locomotor) && harv.CanHarvestCell(self, loc) && claimLayer.CanClaimCell(self, loc))
-				.WithCustomCost(loc =>
+			Func<CPos, int> customCost;
+
+			if (procPos.HasValue && harvInfo.ResourceRefineryDirectionPenalty > 0)
+			{
+				customCost = loc =>
 				{
 					if ((loc - searchFromLoc).LengthSquared > searchRadiusSquared)
-						return int.MaxValue;
+						return PathGraph.CostForInvalidCell;
 
-					// Add a cost modifier to harvestable cells to prefer resources that are closer to the refinery.
+					// Add a cost modifier to harvestable cells to prefer resources
+					// that are closer to the refinery.
 					// This reduces the tendancy for harvesters to move in straight lines
-					if (procPos.HasValue && harvInfo.ResourceRefineryDirectionPenalty > 0 && harv.CanHarvestCell(self, loc))
+					if (harv.CanHarvestCell(self, loc))
 					{
-						var pos = self.World.Map.CenterOfCell(loc);
+						var pos = map.CenterOfCell(loc);
 
 						// Calculate harv-cell-refinery angle (cosine rule)
-						var a = harvPos - procPos.Value;
 						var b = pos - procPos.Value;
-						var c = pos - harvPos;
-
-						if (b != WVec.Zero && c != WVec.Zero)
+						if (b != WVec.Zero)
 						{
-							var cosA = (int)(512 * (b.LengthSquared + c.LengthSquared - a.LengthSquared) / b.Length / c.Length);
-
-							// Cost modifier varies between 0 and ResourceRefineryDirectionPenalty
-							return Math.Abs(harvInfo.ResourceRefineryDirectionPenalty / 2) + harvInfo.ResourceRefineryDirectionPenalty * cosA / 2048;
+							var c = pos - harvPos;
+							if (c != WVec.Zero)
+							{
+								var a = harvPos - procPos.Value;
+								var cosA = (int)(512 * (b.LengthSquared + c.LengthSquared - a.LengthSquared) / b.Length / c.Length);
+
+								// Cost modifier varies between 0 and ResourceRefineryDirectionPenalty
+								return Math.Abs(harvInfo.ResourceRefineryDirectionPenalty / 2) + harvInfo.ResourceRefineryDirectionPenalty * cosA / 2048;
+							}
 						}
 					}
 
 					return 0;
-				})
-				.FromPoint(searchFromLoc)
-				.FromPoint(self.Location))
+				};
+			}
+			else
+			{
+				customCost = loc =>
+				{
+					if ((loc - searchFromLoc).LengthSquared > searchRadiusSquared)
+						return PathGraph.CostForInvalidCell;
+					return 0;
+				};
+			}
+
+			var query = new PathQuery(
+				queryType: PathQueryType.ConditionUnidirectional,
+				world: self.World,
+				locomotor: mobile.Locomotor,
+				actor: self,
+				check: BlockedByActor.Stationary,
+				isGoal: loc => domainIndex.IsPassable(self.Location, loc, mobile.Locomotor)
+					&& harv.CanHarvestCell(self, loc)
+					&& claimLayer.CanClaimCell(self, loc),
+				customCost: customCost,
+				fromPositions: self.Location.Equals(searchFromLoc) ?
+					new CPos[] { self.Location } : new CPos[] { searchFromLoc, self.Location });
+
+			using (var search = new PathSearch(query))
 				path = mobile.Pathfinder.FindPath(search);
 
 			if (path.Count > 0)

OpenRA.Mods.Common/Activities/Move/Move.cs

@@ -58,9 +58,17 @@ public Move(Actor self, CPos destination, Color? targetLineColor = null)
 			getPath = check =>
 			{
 				List<CPos> path;
-				using (var search =
-					PathSearch.FromPoint(self.World, mobile.Locomotor, self, mobile.ToCell, destination, check)
-					.WithoutLaneBias())
+				var query = new PathQuery(
+					queryType: PathQueryType.PositionUnidirectional,
+					world: self.World,
+					locomotor: mobile.Locomotor,
+					actor: self,
+					laneBiasDisabled: true,
+					fromPosition: mobile.ToCell,
+					toPosition: destination,
+					check: check);
+
+				using (var search = new PathSearch(query))
 					path = mobile.Pathfinder.FindPath(search);
 				return path;
 			};

OpenRA.Mods.Common/Activities/Move/MoveAdjacentTo.cs

@@ -132,8 +132,27 @@ List<CPos> CalculatePathToTarget(Actor self, BlockedByActor check)
 			if (!searchCells.Any())
 				return NoPath;
 
-			using (var fromSrc = PathSearch.FromPoints(self.World, Mobile.Locomotor, self, searchCells, loc, check))
-			using (var fromDest = PathSearch.FromPoint(self.World, Mobile.Locomotor, self, loc, lastVisibleTargetLocation, check).Reverse())
+			var fromSrcQuery = new PathQuery(
+				queryType: PathQueryType.PositionBidirectional,
+				world: self.World,
+				locomotor: Mobile.Locomotor,
+				actor: self,
+				fromPositions: searchCells,
+				toPosition: loc,
+				check: check);
+
+			var fromDestQuery = new PathQuery(
+				queryType: PathQueryType.PositionBidirectional,
+				world: self.World,
+				locomotor: Mobile.Locomotor,
+				actor: self,
+				fromPosition: loc,
+				toPosition: lastVisibleTargetLocation,
+				check: check,
+				reverse: true);
+
+			using (var fromSrc = new PathSearch(fromSrcQuery))
+			using (var fromDest = new PathSearch(fromDestQuery))
 				return Mobile.Pathfinder.FindBidiPath(fromSrc, fromDest);
 		}