PathFinder got some big changes (PathSearch)

OpenRa.Game/Game.cs

@@ -146,10 +146,6 @@ namespace OpenRa.Game
 		public static double RenderTime = 0.0;
 		public static double TickTime = 0.0;
 		public static double OreTime = 0.0;
-		public static double PathToPathTime = 0.0;
-		public static double NormalPathTime = 0.0;
-		public static int PathToPathCount = 0;
-		public static int NormalPathCount = 0;
 
 		public static Stopwatch sw;
 
@@ -162,10 +158,6 @@ namespace OpenRa.Game
 				using (new PerfSample("tick_time"))
 				{
 					sw.Reset();
-					PathToPathTime = 0;
-					NormalPathTime = 0;
-					PathToPathCount = 0;
-					NormalPathCount = 0;
 					lastTime += timestep;
 
 					if (orderManager.Tick())

OpenRa.Game/OpenRa.Game.csproj

@@ -34,13 +34,13 @@
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|x86' ">
     <DebugSymbols>true</DebugSymbols>
     <OutputPath>bin\x86\Debug\</OutputPath>
-    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <DefineConstants>TRACE;DEBUG;SANITY_CHECKS</DefineConstants>
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
     <DebugType>full</DebugType>
     <PlatformTarget>x86</PlatformTarget>
     <UseVSHostingProcess>false</UseVSHostingProcess>
     <ErrorReport>prompt</ErrorReport>
-    <Optimize>true</Optimize>
+    <Optimize>false</Optimize>
   </PropertyGroup>
   <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|x86' ">
     <OutputPath>bin\x86\Release\</OutputPath>
@@ -79,6 +79,7 @@
     <Compile Include="GameRules\TechTree.cs" />
     <Compile Include="OrderManager.cs" />
     <Compile Include="Ore.cs" />
+    <Compile Include="PathSearch.cs" />
     <Compile Include="Stopwatch.cs" />
     <Compile Include="Support\PerfHistory.cs" />
     <Compile Include="Traits\AcceptsOre.cs" />

OpenRa.Game/PathFinder.cs

@@ -27,22 +27,6 @@ namespace OpenRa.Game
 							: float.PositiveInfinity;
 		}
 
-		public List<int2> FindUnitPath(int2 src, int2 dest, UnitMovementType umt)
-		{
-			using (new PerfSample("find_unit_path"))
-				return FindUnitPath(src, DefaultEstimator(dest), umt);
-		}
-
-		public List<int2> FindUnitPathToRange(int2 src, int2 dest, UnitMovementType umt, int range)
-		{
-			var tilesInRange = Game.FindTilesInCircle(dest, range)
-				.Where(t => Game.IsCellBuildable(t, umt));
-
-			var path = FindUnitPath(tilesInRange, DefaultEstimator(src), umt);
-			path.Reverse();
-			return path;
-		}
-
 		bool IsBlocked(int2 from, UnitMovementType umt)
 		{
 			for (int v = -1; v < 2; v++)
@@ -57,103 +41,63 @@ namespace OpenRa.Game
 			return true;
 		}
 
+		public List<int2> FindUnitPath( int2 from, int2 target, UnitMovementType umt )
+		{
+			using( new PerfSample( "find_unit_path" ) )
+				return FindPath( PathSearch.FromPoint( from, target, umt, false ) );
+		}
+
+		public List<int2> FindUnitPathToRange( int2 src, int2 target, UnitMovementType umt, int range )
+		{
+			using( new PerfSample( "find_unit_path_multiple_src" ) )
+			{
+				var tilesInRange = Game.FindTilesInCircle( src, range )
+					.Where( t => Game.IsCellBuildable( t, umt ) );
+
+				var path = FindPath( PathSearch.FromPoints( tilesInRange, target, umt, false ));
+				path.Reverse();
+				return path;
+			}
+		}
+
 		public List<int2> FindPathToPath( int2 from, List<int2> path, UnitMovementType umt )
 		{
-			using (new PerfSample("find_path_to_path"))
+			if( IsBlocked( from, umt ) )
-			{
+				return new List<int2>();
-				if (IsBlocked(from, umt))
-					return new List<int2>();
 
-				CellInfo[,] cellInfo = null;
+			using( new PerfSample( "find_path_to_path" ) )
-				var queue = new PriorityQueue<PathDistance>();
+				return FindBidiPath(
-				var estimator = DefaultEstimator(from);
+					PathSearch.FromPath( path, from, umt, true ),
-
+					PathSearch.FromPoint( from, path[ 0 ], umt, true ) );
-				var cost = 0.0f;
-				var prev = path[0];
-				for (int i = 0; i < path.Count; i++)
-				{
-					var sl = path[i];
-					if ( /*i == 0 || */(Game.BuildingInfluence.CanMoveHere(path[i]) && Game.UnitInfluence.GetUnitAt(path[i]) == null))
-					{
-						queue.Add(new PathDistance(estimator(sl), sl));
-						if (cellInfo == null)
-							cellInfo = InitCellInfo();
-
-						cellInfo[sl.X, sl.Y] = new CellInfo(cost, prev, false);
-					}
-					var d = sl - prev;
-					cost += ((d.X * d.Y != 0) ? 1.414213563f : 1.0f) * passableCost[(int)umt][sl.X, sl.Y];
-					prev = sl;
-				}
-				if (queue.Empty) return new List<int2>();
-
-				var h2 = DefaultEstimator(path[0]);
-				var otherQueue = new PriorityQueue<PathDistance>();
-				otherQueue.Add(new PathDistance(h2(from), from));
-
-				var otherCellInfo = InitCellInfo();
-				otherCellInfo[from.X, from.Y] = new CellInfo( 0, from, false );
-				var ret = FindBidiPath(cellInfo, otherCellInfo, queue, otherQueue, estimator, h2, umt, true);
-				return ret;
-			}
 		}
 
-		public List<int2> FindUnitPath( int2 unitLocation, Func<int2,float> estimator, UnitMovementType umt )
-		{
-			return FindUnitPath( new[] { unitLocation }, estimator, umt );
-		}
 
-		public List<int2> FindUnitPath( IEnumerable<int2> startLocations, Func<int2, float> estimator, UnitMovementType umt )
-		{
-			var cellInfo = InitCellInfo();
-			var queue = new PriorityQueue<PathDistance>();
 
-			foreach (var sl in startLocations)
+		public List<int2> FindPath( PathSearch search )
-			{
-				queue.Add(new PathDistance(estimator(sl), sl));
-				cellInfo[sl.X, sl.Y].MinCost = 0;
-			}
-
-			return FindPath( cellInfo, queue, estimator, umt, false );
-		}
-
-		List<int2> FindPath( CellInfo[ , ] cellInfo, PriorityQueue<PathDistance> queue, Func<int2, float> estimator, UnitMovementType umt, bool checkForBlock )
 		{
 			int nodesExpanded = 0;
 			using (new PerfSample("find_path_inner"))
 			{
-				while (!queue.Empty)
+				while (!search.queue.Empty)
 				{
-					PathDistance p = queue.Pop();
+					var p = search.Expand( passableCost );
-					cellInfo[p.Location.X, p.Location.Y].Seen = true;
 
-					if (estimator(p.Location) == 0)
+					if (search.heuristic(p) == 0)
 					{
 						PerfHistory.Increment("nodes_expanded", nodesExpanded * .01);
-						return MakePath(cellInfo, p.Location);
+						return MakePath(search.cellInfo, p);
 					}
 
 					nodesExpanded++;
-
+					PerfHistory.Increment( "nodes_expanded", nodesExpanded * .01 );
-					ExpandNode(cellInfo, queue, p, umt, checkForBlock, estimator);
 				}
 
-				PerfHistory.Increment("nodes_expanded", nodesExpanded * .01);
 				// no path exists
 				return new List<int2>();
 			}
 		}
 
-		static CellInfo[ , ] InitCellInfo()
+		static List<int2> MakePath( CellInfo[ , ] cellInfo, int2 destination )
-		{
-			var cellInfo = new CellInfo[ 128, 128 ];
-			for( int x = 0 ; x < 128 ; x++ )
-				for( int y = 0 ; y < 128 ; y++ )
-					cellInfo[ x, y ] = new CellInfo( float.PositiveInfinity, new int2( x, y ), false );
-			return cellInfo;
-		}
-
-		List<int2> MakePath( CellInfo[ , ] cellInfo, int2 destination )
 		{
 			List<int2> ret = new List<int2>();
 			int2 pathNode = destination;
@@ -165,112 +109,60 @@ namespace OpenRa.Game
 			}
 
 			ret.Add(pathNode);
-
+			CheckSanePath(ret);
 			return ret;
 		}
 
-		static Func<int2, float> DefaultEstimator(int2 destination)
-		{
-			return here =>
-			{
-				int2 d = ( here - destination ).Abs();
-				int diag = Math.Min( d.X, d.Y );
-				int straight = Math.Abs( d.X - d.Y );
-				return 1.5f * diag + straight;
-			};
-		}
 
-		void ExpandNode(CellInfo[,] ci, PriorityQueue<PathDistance> q, PathDistance p, UnitMovementType umt, bool checkForBlock, Func<int2, float> h)
-		{
-			foreach (int2 d in Util.directions)
-			{
-				int2 newHere = p.Location + d;
-
-				if (ci[newHere.X, newHere.Y].Seen)
-					continue;
-				if (passableCost[(int)umt][newHere.X, newHere.Y] == float.PositiveInfinity)
-					continue;
-				if (!Game.BuildingInfluence.CanMoveHere(newHere))
-					continue;
-				if (checkForBlock && Game.UnitInfluence.GetUnitAt(newHere) != null)
-					continue;
-				var est = h(newHere);
-				if (est == float.PositiveInfinity)
-					continue;
-
-				float cellCost = ((d.X * d.Y != 0) ? 1.414213563f : 1.0f) * passableCost[(int)umt][newHere.X, newHere.Y];
-				float newCost = ci[p.Location.X, p.Location.Y].MinCost + cellCost;
-
-				if (newCost >= ci[newHere.X, newHere.Y].MinCost)
-					continue;
-
-				ci[newHere.X, newHere.Y].Path = p.Location;
-				ci[newHere.X, newHere.Y].MinCost = newCost;
-
-				q.Add(new PathDistance(newCost + est, newHere));
-			}
-		}
 
 		List<int2> FindBidiPath(			/* searches from both ends toward each other */
-			CellInfo[,] ca, 
+			PathSearch fromSrc,
-			CellInfo[,] cb,
+			PathSearch fromDest)
-			PriorityQueue<PathDistance> qa,
-			PriorityQueue<PathDistance> qb,
-			Func<int2, float> ha,
-			Func<int2, float> hb,
-			UnitMovementType umt,
-			bool checkForBlocked)
 		{
-			while (!qa.Empty && !qb.Empty)
+			while (!fromSrc.queue.Empty && !fromDest.queue.Empty)
 			{
-				{		/* make some progress on the first search */
+				/* make some progress on the first search */
-					var p = qa.Pop();
+				var p = fromSrc.Expand( passableCost );
-					ca[p.Location.X, p.Location.Y].Seen = true;
 				
-					if (cb[p.Location.X, p.Location.Y].MinCost < float.PositiveInfinity)
+				if (fromDest.cellInfo[p.X, p.Y].MinCost < float.PositiveInfinity)
-						return MakeBidiPath(ca, cb, p.Location);
+					return MakeBidiPath(fromSrc, fromDest, p);
-					else
-						ExpandNode(ca, qa, p, umt, checkForBlocked, ha);
-				}
 
-				{		/* make some progress on the second search */
+				/* make some progress on the second search */
-					var p = qb.Pop();
+				fromDest.Expand( passableCost );
-					cb[p.Location.X, p.Location.Y].Seen = true;
-
-					//if (ca[p.Location.X, p.Location.Y].MinCost < float.PositiveInfinity)
-					//	return MakeBidiPath(ca, cb, p.Location);
-					//else
-						ExpandNode(cb, qb, p, umt, checkForBlocked, hb);
-				}
 			}
 
 			return new List<int2>();
 		}
 
-		static List<int2> MakeBidiPath(CellInfo[,] ca, CellInfo[,] cb, int2 p)
+		static List<int2> MakeBidiPath(PathSearch a, PathSearch b, int2 p)
 		{
-			var a = new List<int2>();
+			var ca = a.cellInfo;
+			var cb = b.cellInfo;
+
+			var ret = new List<int2>();
 
 			var q = p;
 			while (ca[q.X, q.Y].Path != q)
 			{
-				a.Add( q );
+				ret.Add( q );
 				q = ca[ q.X, q.Y ].Path;
 			}
 
-			a.Reverse();
+			ret.Reverse();
 
 			q = p;
 			while (cb[q.X, q.Y].Path != q)
 			{
 				q = cb[q.X, q.Y].Path;
-				a.Add(q);
+				ret.Add(q);
 			}
 
-			CheckSanePath( a );
+			CheckSanePath( ret );
-			return a;
+			return ret;
 		}
 
+
+
 		[System.Diagnostics.Conditional( "SANITY_CHECKS" )]
 		static void CheckSanePath( List<int2> path )
 		{

OpenRa.Game/PathSearch.cs

@@ -0,0 +1,142 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using OpenRa.Game.Graphics;
+using IjwFramework.Collections;
+
+namespace OpenRa.Game
+{
+	class PathSearch
+	{
+		public CellInfo[ , ] cellInfo;
+		public PriorityQueue<PathDistance> queue;
+		public Func<int2, float> heuristic;
+		public UnitMovementType umt;
+		public bool checkForBlocked;
+
+		public PathSearch()
+		{
+			cellInfo = InitCellInfo();
+			queue = new PriorityQueue<PathDistance>();
+		}
+
+		public int2 Expand( float[][ , ] passableCost )
+		{
+			var p = queue.Pop();
+			cellInfo[ p.Location.X, p.Location.Y ].Seen = true;
+
+			foreach( int2 d in Util.directions )
+			{
+				int2 newHere = p.Location + d;
+
+				if( cellInfo[ newHere.X, newHere.Y ].Seen )
+					continue;
+				if( passableCost[ (int)umt ][ newHere.X, newHere.Y ] == float.PositiveInfinity )
+					continue;
+				if( !Game.BuildingInfluence.CanMoveHere( newHere ) )
+					continue;
+				if( checkForBlocked && Game.UnitInfluence.GetUnitAt( newHere ) != null )
+					continue;
+				var est = heuristic( newHere );
+				if( est == float.PositiveInfinity )
+					continue;
+
+				float cellCost = ( ( d.X * d.Y != 0 ) ? 1.414213563f : 1.0f ) * passableCost[ (int)umt ][ newHere.X, newHere.Y ];
+				float newCost = cellInfo[ p.Location.X, p.Location.Y ].MinCost + cellCost;
+
+				if( newCost >= cellInfo[ newHere.X, newHere.Y ].MinCost )
+					continue;
+
+				cellInfo[ newHere.X, newHere.Y ].Path = p.Location;
+				cellInfo[ newHere.X, newHere.Y ].MinCost = newCost;
+
+				queue.Add( new PathDistance( newCost + est, newHere ) );
+			}
+			return p.Location;
+		}
+
+		public void AddInitialCell( int2 location )
+		{
+			cellInfo[ location.X, location.Y ] = new CellInfo( 0, location, false );
+			queue.Add( new PathDistance( heuristic( location ), location ) );
+		}
+
+	
+		
+		
+		
+		
+		
+		public static PathSearch FromPoint( int2 from, int2 target, UnitMovementType umt, bool checkForBlocked )
+		{
+			var search = new PathSearch {
+				heuristic = DefaultEstimator( target ),
+				umt = umt,
+				checkForBlocked = checkForBlocked };
+
+			search.AddInitialCell( from );
+
+			return search;
+		}
+
+		public static PathSearch FromPoints( IEnumerable<int2> froms, int2 target, UnitMovementType umt, bool checkForBlocked )
+		{
+			var search = new PathSearch { 
+				heuristic = DefaultEstimator( target ),
+				umt = umt,
+				checkForBlocked = checkForBlocked };
+
+			foreach( var sl in froms )
+				search.AddInitialCell( sl );
+
+			return search;
+		}
+
+		public static PathSearch FromPath( List<int2> path, int2 target, UnitMovementType umt, bool checkForBlocked )
+		{
+			var search = new PathSearch { 
+				heuristic = DefaultEstimator( target ),
+				umt = umt,
+				checkForBlocked = checkForBlocked };
+
+			var cost = 0.0f;
+			var prev = path[ 0 ];
+			for( int i = 0 ; i < path.Count ; i++ )
+			{
+				var sl = path[ i ];
+				if( Game.BuildingInfluence.CanMoveHere( path[ i ] ) && Game.UnitInfluence.GetUnitAt( path[ i ] ) == null )
+					search.AddInitialCell( sl );
+
+				var d = sl - prev;
+				cost += ( ( d.X * d.Y != 0 ) ? 1.414213563f : 1.0f );// *passableCost[ (int)umt ][ sl.X, sl.Y ];
+				prev = sl;
+			}
+			return search;
+		}
+
+
+
+
+
+		static CellInfo[ , ] InitCellInfo()
+		{
+			var cellInfo = new CellInfo[ 128, 128 ];
+			for( int x = 0 ; x < 128 ; x++ )
+				for( int y = 0 ; y < 128 ; y++ )
+					cellInfo[ x, y ] = new CellInfo( float.PositiveInfinity, new int2( x, y ), false );
+			return cellInfo;
+		}
+
+		static Func<int2, float> DefaultEstimator( int2 destination )
+		{
+			return here =>
+			{
+				int2 d = ( here - destination ).Abs();
+				int diag = Math.Min( d.X, d.Y );
+				int straight = Math.Abs( d.X - d.Y );
+				return 1.5f * diag + straight;
+			};
+		}
+	}
+}

OpenRa.Game/Traits/Activities/Harvest.cs

@@ -75,20 +75,25 @@ namespace OpenRa.Game.Traits.Activities
 			/* find a nearby patch */
 			/* todo: add the queries we need to support this! */
 
-			var path = Game.PathFinder.FindUnitPath( self.Location, loc =>
+			var search = new PathSearch
-				{
+			{
-					if( Game.UnitInfluence.GetUnitAt( loc ) != null ) return float.PositiveInfinity;
+				heuristic = loc => ( Game.map.ContainsResource( loc ) ? 0 : 1 ),
-					return Game.map.ContainsResource( loc ) ? 0 : 1;
+				umt = UnitMovementType.Wheel,
-				}, UnitMovementType.Wheel )
+				checkForBlocked = true
+			};
+			search.AddInitialCell( self.Location );
+
+			var path = Game.PathFinder.FindPath( search )
 				.TakeWhile( a => a != self.Location )
 				.ToList();
+
 			if( path.Count != 0 )
 			{
 				mobile.QueueActivity( new Move( path ) );
 				mobile.QueueActivity( new Harvest() );
 			}
 
-			mobile.InternalSetActivity(NextActivity);
+			mobile.InternalSetActivity( NextActivity );
 		}
 
 		public void Cancel(Actor self, Mobile mobile)