- Introduced Unit Testing capabilities to the PathFinder trait and algorithm.

Introduced also a small Unit test project to prove it.

- Separated caching capabilities from PathFinder class to increase cohesion and maintainability.
Refactored the pathfinding algorithm by extracting methods based on responsibilities like
calculating costs and reordering functions. These changes should provide a in average a small increase in
pathfinding performance and maintainability.

- Optimized the pathfinder algorithm to reuse calculations like the
MovementCost and heuristics.

- Introduced base classes, IPathSearch and IPriorityQueue interfaces,
and restructured code to ease readability and testability

- Renamed the PathFinder related classes to more appropriate names. Made the
traits rely on the interface IPathfinder instead of concrete PathFinder
implementation.

- Massive performance improvements

- Solved error with harvesters' Heuristic

- Updated the heuristic to ease redability and adjustability. D can be
adjusted to offer best paths by decreasing and more performance by
increasing it

- Refactored the CellLayer<CellInfo> creation in its own Singleton class

- Extracted the graph abstraction onto an IGraph interface, making the
Pathfinder agnostic to the definition of world and terrain. This
abstraction can help in the future to be able to cache graphs for similar
classes and their costs, speeding up the pathfinder and being able to feed
the A* algorithm with different types of graphs like Hierarchical graphs

OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs

@@ -0,0 +1,203 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using OpenRA.Mods.Common.Traits;
+using OpenRA.Primitives;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	public interface IPathSearch
+	{
+		string Id { get; }
+
+		/// <summary>
+		/// The Graph used by the A*
+		/// </summary>
+		IGraph<CellInfo> Graph { get; }
+
+		/// <summary>
+		/// The open queue where nodes that are worth to consider are stored by their estimator
+		/// </summary>
+		IPriorityQueue<CPos> OpenQueue { get; }
+
+		/// <summary>
+		/// Stores the analyzed nodes by the expand function
+		/// </summary>
+		IEnumerable<Pair<CPos, int>> Considered { get; }
+
+		bool Debug { get; set; }
+
+		Player Owner { get; }
+
+		int MaxCost { get; }
+
+		IPathSearch Reverse();
+
+		IPathSearch WithCustomBlocker(Func<CPos, bool> customBlock);
+
+		IPathSearch WithIgnoredActor(Actor b);
+
+		IPathSearch WithHeuristic(Func<CPos, int> h);
+
+		IPathSearch WithCustomCost(Func<CPos, int> w);
+
+		IPathSearch WithoutLaneBias();
+
+		IPathSearch FromPoint(CPos from);
+
+		/// <summary>
+		/// Decides whether a location is a target based on its estimate
+		/// (An estimate of 0 means that the location and the unit's goal
+		/// are the same. There could be multiple goals).
+		/// </summary>
+		/// <param name="location">The location to assess</param>
+		/// <returns>Whether the location is a target</returns>
+		bool IsTarget(CPos location);
+
+		CPos Expand();
+	}
+
+	public abstract class BasePathSearch : IPathSearch
+	{
+		public IGraph<CellInfo> Graph { get; set; }
+
+		// The Id of a Pathsearch is computed by its properties.
+		// So two PathSearch instances with the same parameters will
+		// Compute the same Id. This is used for caching purposes.
+		public string Id
+		{
+			get
+			{
+				if (string.IsNullOrEmpty(id))
+				{
+					var builder = new StringBuilder();
+					builder.Append(this.Graph.Actor.ActorID);
+					while (!startPoints.Empty)
+					{
+						var startpoint = startPoints.Pop();
+						builder.Append(startpoint.X);
+						builder.Append(startpoint.Y);
+						builder.Append(Graph[startpoint].EstimatedTotal);
+					}
+
+					builder.Append(Graph.InReverse);
+					if (Graph.IgnoredActor != null) builder.Append(Graph.IgnoredActor.ActorID);
+					builder.Append(Graph.LaneBias);
+
+					id = builder.ToString();
+				}
+
+				return id;
+			}
+		}
+
+		public IPriorityQueue<CPos> OpenQueue { get; protected set; }
+
+		public abstract IEnumerable<Pair<CPos, int>> Considered { get; }
+
+		public Player Owner { get { return this.Graph.Actor.Owner; } }
+		public int MaxCost { get; protected set; }
+		public bool Debug { get; set; }
+		string id;
+		protected Func<CPos, int> heuristic;
+
+		// This member is used to compute the ID of PathSearch.
+		// Essentially, it represents a collection of the initial
+		// points considered and their Heuristics to reach
+		// the target. It pretty match identifies, in conjunction of the Actor,
+		// a deterministic set of calculations
+		protected IPriorityQueue<CPos> startPoints;
+
+		protected BasePathSearch(IGraph<CellInfo> graph)
+		{
+			Graph = graph;
+			OpenQueue = new PriorityQueue<CPos>(new PositionComparer(Graph));
+			startPoints = new PriorityQueue<CPos>(new PositionComparer(Graph));
+			Debug = false;
+			MaxCost = 0;
+		}
+
+		/// <summary>
+		/// Default: Diagonal distance heuristic. More information:
+		/// http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
+		/// </summary>
+		/// <returns>A delegate that calculates the estimation for a node</returns>
+		protected static Func<CPos, int> DefaultEstimator(CPos destination)
+		{
+			return here =>
+			{
+				var diag = Math.Min(Math.Abs(here.X - destination.X), Math.Abs(here.Y - destination.Y));
+				var straight = Math.Abs(here.X - destination.X) + Math.Abs(here.Y - destination.Y);
+
+				// According to the information link, this is the shape of the function.
+				// We just extract factors to simplify.
+				// Possible simplification: var h = Constants.CellCost * (straight + (Constants.Sqrt2 - 2) * diag);
+				return Constants.CellCost * straight + (Constants.DiagonalCellCost - 2 * Constants.CellCost) * diag;
+			};
+		}
+
+		public IPathSearch Reverse()
+		{
+			Graph.InReverse = true;
+			return this;
+		}
+
+		public IPathSearch WithCustomBlocker(Func<CPos, bool> customBlock)
+		{
+			Graph.CustomBlock = customBlock;
+			return this;
+		}
+
+		public IPathSearch WithIgnoredActor(Actor b)
+		{
+			Graph.IgnoredActor = b;
+			return this;
+		}
+
+		public IPathSearch WithHeuristic(Func<CPos, int> h)
+		{
+			heuristic = h;
+			return this;
+		}
+
+		public IPathSearch WithCustomCost(Func<CPos, int> w)
+		{
+			Graph.CustomCost = w;
+			return this;
+		}
+
+		public IPathSearch WithoutLaneBias()
+		{
+			Graph.LaneBias = 0;
+			return this;
+		}
+
+		public IPathSearch FromPoint(CPos from)
+		{
+			if (this.Graph.World.Map.Contains(from))
+				AddInitialCell(from);
+
+			return this;
+		}
+
+		protected abstract void AddInitialCell(CPos cell);
+
+		public bool IsTarget(CPos location)
+		{
+			var locInfo = Graph[location];
+			return locInfo.EstimatedTotal - locInfo.CostSoFar == 0;
+		}
+
+		public abstract CPos Expand();
+	}
+}

OpenRA.Mods.Common/Pathfinder/CellInfo.cs

@@ -0,0 +1,78 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System;
+using System.Collections.Generic;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	/// <summary>
+	/// Describes the three states that a node in the graph can have.
+	/// Based on A* algorithm specification
+	/// </summary>
+	public enum CellStatus
+	{
+		Unvisited,
+		Open,
+		Closed
+	}
+
+	/// <summary>
+	/// Stores information about nodes in the pathfinding graph
+	/// </summary>
+	public struct CellInfo
+	{
+		/// <summary>
+		/// The cost to move from the start up to this node
+		/// </summary>
+		public readonly int CostSoFar;
+
+		/// <summary>
+		/// The estimation of how far is the node from our goal
+		/// </summary>
+		public readonly int EstimatedTotal;
+
+		/// <summary>
+		/// The previous node of this one that follows the shortest path
+		/// </summary>
+		public readonly CPos PreviousPos;
+
+		/// <summary>
+		/// The status of this node
+		/// </summary>
+		public readonly CellStatus Status;
+
+		public CellInfo(int costSoFar, int estimatedTotal, CPos previousPos, CellStatus status)
+		{
+			CostSoFar = costSoFar;
+			PreviousPos = previousPos;
+			Status = status;
+			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);
+		}
+	}
+}

OpenRA.Mods.Common/Pathfinder/CellInfoLayerManager.cs

@@ -0,0 +1,111 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System.Collections.Generic;
+using System.Drawing;
+using OpenRA.Mods.Common.Traits;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	public interface ICellInfoLayerManager
+	{
+		/// <summary>
+		/// Gets a CellLayer of Nodes from the pool
+		/// </summary>
+		CellLayer<CellInfo> GetFromPool();
+
+		/// <summary>
+		/// Puts a CellLayer into the pool
+		/// </summary>
+		void PutBackIntoPool(CellLayer<CellInfo> ci);
+
+		/// <summary>
+		/// Creates (or obtains from the pool) a CellLayer given a map
+		/// </summary>
+		CellLayer<CellInfo> NewLayer(IMap map);
+	}
+
+	public sealed class CellInfoLayerManager : ICellInfoLayerManager
+	{
+		readonly Queue<CellLayer<CellInfo>> cellInfoPool = new Queue<CellLayer<CellInfo>>();
+		readonly object defaultCellInfoLayerSync = new object();
+		CellLayer<CellInfo> defaultCellInfoLayer;
+
+		static ICellInfoLayerManager instance = new CellInfoLayerManager();
+
+		public static ICellInfoLayerManager Instance
+		{
+			get
+			{
+				return instance;
+			}
+		}
+
+		public static void SetInstance(ICellInfoLayerManager cellInfoLayerManager)
+		{
+			instance = cellInfoLayerManager;
+		}
+
+		public CellLayer<CellInfo> GetFromPool()
+		{
+			lock (cellInfoPool)
+				return cellInfoPool.Dequeue();
+		}
+
+		public void PutBackIntoPool(CellLayer<CellInfo> ci)
+		{
+			lock (cellInfoPool)
+				cellInfoPool.Enqueue(ci);
+		}
+
+		public CellLayer<CellInfo> NewLayer(IMap map)
+		{
+			CellLayer<CellInfo> result = null;
+			var mapSize = new Size(map.MapSize.X, map.MapSize.Y);
+
+			// HACK: Uses a static cache so that double-ended searches (which have two PathSearch instances)
+			// can implicitly share data.  The PathFinder should allocate the CellInfo array and pass it
+			// explicitly to the things that need to share it.
+			while (cellInfoPool.Count > 0)
+			{
+				var cellInfo = GetFromPool();
+				if (cellInfo.Size != mapSize || cellInfo.Shape != map.TileShape)
+				{
+					Log.Write("debug", "Discarding old pooled CellInfo of wrong size.");
+					continue;
+				}
+
+				result = cellInfo;
+				break;
+			}
+
+			if (result == null)
+				result = new CellLayer<CellInfo>(map);
+
+			lock (defaultCellInfoLayerSync)
+			{
+				if (defaultCellInfoLayer == null ||
+				    defaultCellInfoLayer.Size != mapSize ||
+				    defaultCellInfoLayer.Shape != map.TileShape)
+				{
+					defaultCellInfoLayer =
+						CellLayer<CellInfo>.CreateInstance(
+							mpos => new CellInfo(int.MaxValue, int.MaxValue, mpos.ToCPos(map as Map), CellStatus.Unvisited),
+							new Size(map.MapSize.X, map.MapSize.Y),
+							map.TileShape);
+				}
+
+				result.CopyValuesFrom(defaultCellInfoLayer);
+			}
+
+			return result;
+		}
+	}
+}

OpenRA.Mods.Common/Pathfinder/Constants.cs

@@ -0,0 +1,29 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	public static class Constants
+	{
+		/// <summary>
+		/// Min cost to arrive from once cell to an adjacent one
+		/// (125 according to runtime tests where we could assess the cost
+		/// a unit took to move one cell horizontally)
+		/// </summary>
+		public const int CellCost = 125;
+
+		/// <summary>
+		/// Min cost to arrive from once cell to a diagonal adjacent one
+		/// (125 * Sqrt(2) according to runtime tests where we could assess the cost
+		/// a unit took to move one cell diagonally)
+		/// </summary>
+		public const int DiagonalCellCost = 177;
+	}
+}

OpenRA.Mods.Common/Pathfinder/PathCacheStorage.cs

@@ -0,0 +1,75 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System.Collections.Generic;
+using System.Linq;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	public class PathCacheStorage : ICacheStorage<List<CPos>>
+	{
+		class CachedPath
+		{
+			public List<CPos> Result;
+			public int Tick;
+		}
+
+		const int MaxPathAge = 50;
+		readonly IWorld world;
+		Dictionary<string, CachedPath> cachedPaths = new Dictionary<string, CachedPath>(100);
+
+		public PathCacheStorage(IWorld world)
+		{
+			this.world = world;
+		}
+
+		public void Remove(string key)
+		{
+			cachedPaths.Remove(key);
+		}
+
+		public void Store(string key, List<CPos> data)
+		{
+			// Eventually clean up the cachedPaths dictionary
+			if (cachedPaths.Count >= 100)
+				foreach (var cachedPath in cachedPaths.Where(p => IsExpired(p.Value)).ToList())
+					cachedPaths.Remove(cachedPath.Key);
+
+			cachedPaths.Add(key, new CachedPath
+			{
+				Tick = world.WorldTick,
+				Result = data
+			});
+		}
+
+		public List<CPos> Retrieve(string key)
+		{
+			CachedPath cached;
+			if (cachedPaths.TryGetValue(key, out cached))
+			{
+				if (IsExpired(cached))
+				{
+					cachedPaths.Remove(key);
+					return null;
+				}
+
+				cached.Tick = world.WorldTick;
+				return cached.Result;
+			}
+
+			return null;
+		}
+
+		bool IsExpired(CachedPath path)
+		{
+			return world.WorldTick - path.Tick > MaxPathAge;
+		}
+	}
+}

OpenRA.Mods.Common/Pathfinder/PathFinderCacheDecorator.cs

@@ -0,0 +1,104 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System.Collections.Generic;
+using OpenRA.Mods.Common.Traits;
+using OpenRA.Support;
+using OpenRA.Traits;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	/// <summary>
+	/// A decorator used to cache the pathfinder (Decorator design pattern)
+	/// </summary>
+	public class PathFinderCacheDecorator : IPathFinder
+	{
+		readonly IPathFinder pathFinder;
+		readonly ICacheStorage<List<CPos>> cacheStorage;
+
+		public PathFinderCacheDecorator(IPathFinder pathFinder, ICacheStorage<List<CPos>> cacheStorage)
+		{
+			this.pathFinder = pathFinder;
+			this.cacheStorage = cacheStorage;
+		}
+
+		public List<CPos> FindUnitPath(CPos source, CPos target, IActor self)
+		{
+			using (new PerfSample("Pathfinder"))
+			{
+				var key = "FindUnitPath" + self.ActorID + source.X + source.Y + target.X + target.Y;
+				var cachedPath = cacheStorage.Retrieve(key);
+
+				if (cachedPath != null)
+					return cachedPath;
+
+				var pb = pathFinder.FindUnitPath(source, target, self);
+
+				cacheStorage.Store(key, pb);
+
+				return pb;
+			}
+		}
+
+		public List<CPos> FindUnitPathToRange(CPos source, SubCell srcSub, WPos target, WRange range, IActor self)
+		{
+			using (new PerfSample("Pathfinder"))
+			{
+				var key = "FindUnitPathToRange" + self.ActorID + source.X + source.Y + target.X + target.Y;
+				var cachedPath = cacheStorage.Retrieve(key);
+
+				if (cachedPath != null)
+					return cachedPath;
+
+				var pb = pathFinder.FindUnitPathToRange(source, srcSub, target, range, self);
+
+				cacheStorage.Store(key, pb);
+
+				return pb;
+			}
+		}
+
+		public List<CPos> FindPath(IPathSearch search)
+		{
+			using (new PerfSample("Pathfinder"))
+			{
+				var key = "FindPath" + search.Id;
+				var cachedPath = cacheStorage.Retrieve(key);
+
+				if (cachedPath != null)
+					return cachedPath;
+
+				var pb = pathFinder.FindPath(search);
+
+				cacheStorage.Store(key, pb);
+
+				return pb;
+			}
+		}
+
+		public List<CPos> FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest)
+		{
+			using (new PerfSample("Pathfinder"))
+			{
+				var key = "FindBidiPath" + fromSrc.Id + fromDest.Id;
+				var cachedPath = cacheStorage.Retrieve(key);
+
+				if (cachedPath != null)
+					return cachedPath;
+
+				var pb = pathFinder.FindBidiPath(fromSrc, fromDest);
+
+				cacheStorage.Store(key, pb);
+
+				return pb;
+			}
+		}
+	}
+}

OpenRA.Mods.Common/Pathfinder/PathGraph.cs

@@ -0,0 +1,198 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System;
+using System.Collections.Generic;
+using OpenRA.Mods.Common.Traits;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	/// <summary>
+	/// Represents a graph with nodes and edges
+	/// </summary>
+	/// <typeparam name="T">The type of node used in the graph</typeparam>
+	public interface IGraph<T> : IDisposable
+	{
+		/// <summary>
+		/// Gets all the Connections for a given node in the graph
+		/// </summary>
+		ICollection<GraphConnection> GetConnections(CPos position);
+
+		/// <summary>
+		/// Retrieves an object given a node in the graph
+		/// </summary>
+		T this[CPos pos] { get; set; }
+
+		Func<CPos, bool> CustomBlock { get; set; }
+
+		Func<CPos, int> CustomCost { get; set; }
+
+		int LaneBias { get; set; }
+
+		bool InReverse { get; set; }
+
+		IActor IgnoredActor { get; set; }
+
+		IWorld World { get; }
+
+		IActor Actor { get; }
+	}
+
+	public struct GraphConnection
+	{
+		public readonly CPos Source;
+		public readonly CPos Destination;
+		public readonly int Cost;
+
+		public GraphConnection(CPos source, CPos destination, int cost)
+		{
+			Source = source;
+			Destination = destination;
+			Cost = cost;
+		}
+	}
+
+	public class PathGraph : IGraph<CellInfo>
+	{
+		public IActor Actor { get; private set; }
+		public IWorld World { get; private set; }
+		public Func<CPos, bool> CustomBlock { get; set; }
+		public Func<CPos, int> CustomCost { get; set; }
+		public int LaneBias { get; set; }
+		public bool InReverse { get; set; }
+		public IActor IgnoredActor { get; set; }
+
+		readonly CellConditions checkConditions;
+		readonly IMobileInfo mobileInfo;
+		CellLayer<CellInfo> cellInfo;
+
+		public const int InvalidNode = int.MaxValue;
+
+		public PathGraph(CellLayer<CellInfo> cellInfo, IMobileInfo mobileInfo, IActor actor, IWorld world, bool checkForBlocked)
+		{
+			this.cellInfo = cellInfo;
+			World = world;
+			this.mobileInfo = mobileInfo;
+			Actor = actor;
+			LaneBias = 1;
+			checkConditions = checkForBlocked ? CellConditions.TransientActors : CellConditions.None;
+		}
+
+		// Sets of neighbors for each incoming direction. These exclude the neighbors which are guaranteed
+		// to be reached more cheaply by a path through our parent cell which does not include the current cell.
+		// For horizontal/vertical directions, the set is the three cells 'ahead'. For diagonal directions, the set
+		// is the three cells ahead, plus the two cells to the side, which we cannot exclude without knowing if
+		// the cell directly between them and our parent is passable.
+		static readonly CVec[][] DirectedNeighbors = {
+			new[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1), new CVec(-1, 0), new CVec(-1, 1) },
+			new[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1) },
+			new[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1), new CVec(1, 0), new CVec(1, 1) },
+			new[] { new CVec(-1, -1), new CVec(-1, 0), new CVec(-1, 1) },
+			CVec.Directions,
+			new[] { new CVec(1, -1), new CVec(1, 0), new CVec(1, 1) },
+			new[] { new CVec(-1, -1), new CVec(-1, 0), new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
+			new[] { new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
+			new[] { new CVec(1, -1), new CVec(1, 0), new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
+		};
+
+		public ICollection<GraphConnection> GetConnections(CPos position)
+		{
+			var previousPos = cellInfo[position].PreviousPos;
+
+			var dx = position.X - previousPos.X;
+			var dy = position.Y - previousPos.Y;
+			var index = dy * 3 + dx + 4;
+
+			var validNeighbors = new LinkedList<GraphConnection>();
+			var directions = DirectedNeighbors[index];
+			for (var i = 0; i < directions.Length; i++)
+			{
+				var neighbor = position + directions[i];
+				var movementCost = GetCostToNode(neighbor, directions[i]);
+				if (movementCost != InvalidNode)
+					validNeighbors.AddLast(new GraphConnection(position, neighbor, movementCost));
+			}
+
+			return validNeighbors;
+		}
+
+		int GetCostToNode(CPos destNode, CVec direction)
+		{
+			int movementCost;
+			if (mobileInfo.CanEnterCell(
+				World as World,
+				Actor as Actor,
+				destNode,
+				out movementCost,
+				IgnoredActor as Actor,
+				checkConditions) && !(CustomBlock != null && CustomBlock(destNode)))
+			{
+				return CalculateCellCost(destNode, direction, movementCost);
+			}
+
+			return InvalidNode;
+		}
+
+		int CalculateCellCost(CPos neighborCPos, CVec direction, int movementCost)
+		{
+			var cellCost = movementCost;
+
+			if (direction.X * direction.Y != 0)
+				cellCost = (cellCost * 34) / 24;
+
+			if (CustomCost != null)
+				cellCost += CustomCost(neighborCPos);
+
+			// directional bonuses for smoother flow!
+			if (LaneBias != 0)
+			{
+				var ux = neighborCPos.X + (InReverse ? 1 : 0) & 1;
+				var uy = neighborCPos.Y + (InReverse ? 1 : 0) & 1;
+
+				if ((ux == 0 && direction.Y < 0) || (ux == 1 && direction.Y > 0))
+					cellCost += LaneBias;
+
+				if ((uy == 0 && direction.X < 0) || (uy == 1 && direction.X > 0))
+					cellCost += LaneBias;
+			}
+
+			return cellCost;
+		}
+
+		bool disposed;
+		public void Dispose()
+		{
+			if (disposed)
+				return;
+
+			disposed = true;
+
+			CellInfoLayerManager.Instance.PutBackIntoPool(cellInfo);
+			cellInfo = null;
+
+			GC.SuppressFinalize(this);
+		}
+
+		~PathGraph() { Dispose(); }
+
+		public CellInfo this[CPos pos]
+		{
+			get
+			{
+				return cellInfo[pos];
+			}
+
+			set
+			{
+				cellInfo[pos] = value;
+			}
+		}
+	}
+}

OpenRA.Mods.Common/Pathfinder/PathSearch.cs

@@ -0,0 +1,129 @@
+#region Copyright & License Information
+/*
+ * Copyright 2007-2015 The OpenRA Developers (see AUTHORS)
+ * This file is part of OpenRA, which is free software. It is made
+ * available to you under the terms of the GNU General Public License
+ * as published by the Free Software Foundation. For more information,
+ * see COPYING.
+ */
+#endregion
+
+using System.Collections.Generic;
+using System.Linq;
+using OpenRA.Mods.Common.Traits;
+using OpenRA.Primitives;
+
+namespace OpenRA.Mods.Common.Pathfinder
+{
+	public sealed class PathSearch : BasePathSearch
+	{
+		public override IEnumerable<Pair<CPos, int>> Considered
+		{
+			get { return considered; }
+		}
+
+		LinkedList<Pair<CPos, int>> considered;
+
+		#region Constructors
+
+		private PathSearch(IGraph<CellInfo> graph)
+			: base(graph)
+		{
+			considered = new LinkedList<Pair<CPos, int>>();
+		}
+
+		public static IPathSearch Search(IWorld world, IMobileInfo mi, IActor self, bool checkForBlocked)
+		{
+			var graph = new PathGraph(CellInfoLayerManager.Instance.NewLayer(world.Map), mi, self, world, checkForBlocked);
+			return new PathSearch(graph);
+		}
+
+		public static IPathSearch FromPoint(IWorld world, IMobileInfo mi, IActor self, CPos from, CPos target, bool checkForBlocked)
+		{
+			var graph = new PathGraph(CellInfoLayerManager.Instance.NewLayer(world.Map), mi, self, world, checkForBlocked);
+			var search = new PathSearch(graph)
+			{
+				heuristic = DefaultEstimator(target)
+			};
+
+			if (world.Map.Contains(from))
+				search.AddInitialCell(from);
+
+			return search;
+		}
+
+		public static IPathSearch FromPoints(IWorld world, IMobileInfo mi, IActor self, IEnumerable<CPos> froms, CPos target, bool checkForBlocked)
+		{
+			var graph = new PathGraph(CellInfoLayerManager.Instance.NewLayer(world.Map), mi, self, world, checkForBlocked);
+			var search = new PathSearch(graph)
+			{
+				heuristic = DefaultEstimator(target)
+			};
+
+			foreach (var sl in froms.Where(sl => world.Map.Contains(sl)))
+				search.AddInitialCell(sl);
+
+			return search;
+		}
+
+		protected override void AddInitialCell(CPos location)
+		{
+			Graph[location] = new CellInfo(0, heuristic(location), location, CellStatus.Open);
+			OpenQueue.Add(location);
+			startPoints.Add(location);
+			considered.AddLast(new Pair<CPos, int>(location, 0));
+		}
+
+		#endregion
+
+		/// <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
+		/// </summary>
+		/// <returns>The most promising node of the iteration</returns>
+		public override CPos Expand()
+		{
+			var currentMinNode = OpenQueue.Pop();
+
+			var currentCell = Graph[currentMinNode];
+			Graph[currentMinNode] = new CellInfo(currentCell.CostSoFar, currentCell.EstimatedTotal, currentCell.PreviousPos, CellStatus.Closed);
+
+			foreach (var connection in Graph.GetConnections(currentMinNode))
+			{
+				// Calculate the cost up to that point
+				var gCost = currentCell.CostSoFar + connection.Cost;
+
+				var neighborCPos = connection.Destination;
+				var neighborCell = Graph[neighborCPos];
+
+				// Cost is even higher; next direction:
+				if (gCost >= neighborCell.CostSoFar)
+					continue;
+
+				// Now we may seriously consider this direction using heuristics. If the cell has
+				// already been processed, we can reuse the result (just the difference between the
+				// estimated total and the cost so far
+				int hCost;
+				if (neighborCell.Status == CellStatus.Open || neighborCell.Status == CellStatus.Closed)
+					hCost = neighborCell.EstimatedTotal - neighborCell.CostSoFar;
+				else
+					hCost = heuristic(neighborCPos);
+
+				Graph[neighborCPos] = new CellInfo(gCost, gCost + hCost, currentMinNode, CellStatus.Open);
+
+				if (neighborCell.Status != CellStatus.Open)
+					OpenQueue.Add(neighborCPos);
+
+				if (Debug)
+				{
+					if (gCost > MaxCost)
+						MaxCost = gCost;
+
+					considered.AddLast(new Pair<CPos, int>(neighborCPos, gCost));
+				}
+			}
+
+			return currentMinNode;
+		}
+	}
+}