Merge pull request #7430 from Rydra/upstream/pf-optimized

[Discussion PR] Complete refactor of Pathfinder
This commit is contained in:
Paul Chote
2015-03-03 19:50:25 +00:00
31 changed files with 1866 additions and 615 deletions

View File

@@ -12,9 +12,7 @@ using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using OpenRA;
using OpenRA.Primitives;
using OpenRA.Support;
using OpenRA.Mods.Common.Pathfinder;
using OpenRA.Traits;
namespace OpenRA.Mods.Common.Traits
@@ -22,222 +20,221 @@ namespace OpenRA.Mods.Common.Traits
[Desc("Calculates routes for mobile units based on the A* search algorithm.", " Attach this to the world actor.")]
public class PathFinderInfo : ITraitInfo
{
public object Create(ActorInitializer init) { return new PathFinder(init.World); }
public object Create(ActorInitializer init)
{
return new PathFinderCacheDecorator(new PathFinder(init.World), new PathCacheStorage(init.World));
}
}
public class PathFinder
public interface IPathFinder
{
/// <summary>
/// Calculates a path for the actor from source to destination
/// </summary>
/// <returns>A path from start to target</returns>
List<CPos> FindUnitPath(CPos source, CPos target, IActor self);
List<CPos> FindUnitPathToRange(CPos source, SubCell srcSub, WPos target, WRange range, IActor self);
/// <summary>
/// Calculates a path given a search specification
/// </summary>
List<CPos> FindPath(IPathSearch search);
/// <summary>
/// Calculates a path given two search specifications, and
/// then returns a path when both search intersect each other
/// TODO: This should eventually disappear
/// </summary>
List<CPos> FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest);
}
public class PathFinder : IPathFinder
{
const int MaxPathAge = 50; /* x 40ms ticks */
static readonly List<CPos> EmptyPath = new List<CPos>(0);
readonly IWorld world;
readonly World world;
public PathFinder(World world) { this.world = world; }
class CachedPath
public PathFinder(IWorld world)
{
public CPos From;
public CPos To;
public List<CPos> Result;
public int Tick;
public Actor Actor;
this.world = world;
}
List<CachedPath> cachedPaths = new List<CachedPath>();
public List<CPos> FindUnitPath(CPos from, CPos target, Actor self)
public List<CPos> FindUnitPath(CPos source, CPos target, IActor self)
{
using (new PerfSample("Pathfinder"))
var mi = self.Info.Traits.Get<IMobileInfo>();
// If a water-land transition is required, bail early
var domainIndex = world.WorldActor.TraitOrDefault<DomainIndex>();
if (domainIndex != null)
{
var cached = cachedPaths.FirstOrDefault(p => p.From == from && p.To == target && p.Actor == self);
if (cached != null)
{
Log.Write("debug", "Actor {0} asked for a path from {1} tick(s) ago", self.ActorID, world.WorldTick - cached.Tick);
if (world.WorldTick - cached.Tick > MaxPathAge)
cachedPaths.Remove(cached);
return new List<CPos>(cached.Result);
}
var mi = self.Info.Traits.Get<MobileInfo>();
// If a water-land transition is required, bail early
var domainIndex = self.World.WorldActor.TraitOrDefault<DomainIndex>();
if (domainIndex != null)
{
var passable = mi.GetMovementClass(world.TileSet);
if (!domainIndex.IsPassable(from, target, (uint)passable))
return EmptyPath;
}
var fromPoint = PathSearch.FromPoint(world, mi, self, target, from, true)
.WithIgnoredActor(self);
var fromPointReverse = PathSearch.FromPoint(world, mi, self, from, target, true)
.WithIgnoredActor(self)
.Reverse();
var pb = FindBidiPath(
fromPoint,
fromPointReverse);
CheckSanePath2(pb, from, target);
cachedPaths.RemoveAll(p => world.WorldTick - p.Tick > MaxPathAge);
cachedPaths.Add(new CachedPath { From = from, To = target, Actor = self, Result = pb, Tick = world.WorldTick });
return new List<CPos>(pb);
var passable = mi.GetMovementClass(world.TileSet);
if (!domainIndex.IsPassable(source, target, (uint)passable))
return EmptyPath;
}
var pb = FindBidiPath(
PathSearch.FromPoint(world, mi, self, target, source, true),
PathSearch.FromPoint(world, mi, self, source, target, true).Reverse());
CheckSanePath2(pb, source, target);
return pb;
}
public List<CPos> FindUnitPathToRange(CPos src, SubCell srcSub, WPos target, WRange range, Actor self)
public List<CPos> FindUnitPathToRange(CPos source, SubCell srcSub, WPos target, WRange range, IActor self)
{
using (new PerfSample("Pathfinder"))
var mi = self.Info.Traits.Get<MobileInfo>();
var targetCell = world.Map.CellContaining(target);
var rangeSquared = range.Range * range.Range;
// Correct for SubCell offset
target -= world.Map.OffsetOfSubCell(srcSub);
// Select only the tiles that are within range from the requested SubCell
// This assumes that the SubCell does not change during the path traversal
var tilesInRange = world.Map.FindTilesInCircle(targetCell, range.Range / 1024 + 1)
.Where(t => (world.Map.CenterOfCell(t) - target).LengthSquared <= rangeSquared
&& mi.CanEnterCell(self.World as World, self as Actor, t));
// See if there is any cell within range that does not involve a cross-domain request
// Really, we only need to check the circle perimeter, but it's not clear that would be a performance win
var domainIndex = world.WorldActor.TraitOrDefault<DomainIndex>();
if (domainIndex != null)
{
var mi = self.Info.Traits.Get<MobileInfo>();
var targetCell = self.World.Map.CellContaining(target);
var rangeSquared = range.Range * range.Range;
var passable = mi.GetMovementClass(world.TileSet);
tilesInRange = new List<CPos>(tilesInRange.Where(t => domainIndex.IsPassable(source, t, (uint)passable)));
if (!tilesInRange.Any())
return EmptyPath;
}
// Correct for SubCell offset
target -= self.World.Map.OffsetOfSubCell(srcSub);
var path = FindBidiPath(
PathSearch.FromPoints(world, mi, self, tilesInRange, source, true),
PathSearch.FromPoint(world, mi, self, source, targetCell, true).Reverse());
// Select only the tiles that are within range from the requested SubCell
// This assumes that the SubCell does not change during the path traversal
var tilesInRange = world.Map.FindTilesInCircle(targetCell, range.Range / 1024 + 1)
.Where(t => (world.Map.CenterOfCell(t) - target).LengthSquared <= rangeSquared &&
mi.CanEnterCell(self.World, self, t));
return path;
}
// See if there is any cell within range that does not involve a cross-domain request
// Really, we only need to check the circle perimeter, but it's not clear that would be a performance win
var domainIndex = self.World.WorldActor.TraitOrDefault<DomainIndex>();
if (domainIndex != null)
public List<CPos> FindPath(IPathSearch search)
{
var dbg = world.WorldActor.TraitOrDefault<PathfinderDebugOverlay>();
if (dbg != null && dbg.Visible)
search.Debug = true;
List<CPos> path = null;
while (!search.OpenQueue.Empty)
{
var p = search.Expand();
if (search.IsTarget(p))
{
var passable = mi.GetMovementClass(world.TileSet);
tilesInRange = new List<CPos>(tilesInRange.Where(t => domainIndex.IsPassable(src, t, (uint)passable)));
if (!tilesInRange.Any())
return EmptyPath;
path = MakePath(search.Graph, p);
break;
}
}
var path = FindBidiPath(
PathSearch.FromPoints(world, mi, self, tilesInRange, src, true),
PathSearch.FromPoint(world, mi, self, src, targetCell, true).Reverse());
if (dbg != null && dbg.Visible)
dbg.AddLayer(search.Considered, search.MaxCost, search.Owner);
search.Graph.Dispose();
if (path != null)
return path;
}
// no path exists
return EmptyPath;
}
public List<CPos> FindPath(PathSearch search)
// Searches from both ends toward each other. This is used to prevent blockings in case we find
// units in the middle of the path that prevent us to continue.
public List<CPos> FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest)
{
using (new PerfSample("Pathfinder"))
List<CPos> path = null;
var dbg = world.WorldActor.TraitOrDefault<PathfinderDebugOverlay>();
if (dbg != null && dbg.Visible)
{
using (search)
fromSrc.Debug = true;
fromDest.Debug = true;
}
while (!fromSrc.OpenQueue.Empty && !fromDest.OpenQueue.Empty)
{
// make some progress on the first search
var p = fromSrc.Expand();
if (fromDest.Graph[p].Status == CellStatus.Closed &&
fromDest.Graph[p].CostSoFar < int.MaxValue)
{
List<CPos> path = null;
while (!search.Queue.Empty)
{
var p = search.Expand(world);
if (search.Heuristic(p) == 0)
{
path = MakePath(search.CellInfo, p);
break;
}
}
var dbg = world.WorldActor.TraitOrDefault<PathfinderDebugOverlay>();
if (dbg != null)
dbg.AddLayer(search.Considered.Select(p => new Pair<CPos, int>(p, search.CellInfo[p].MinCost)), search.MaxCost, search.Owner);
if (path != null)
return path;
path = MakeBidiPath(fromSrc, fromDest, p);
break;
}
// no path exists
return EmptyPath;
// make some progress on the second search
var q = fromDest.Expand();
if (fromSrc.Graph[q].Status == CellStatus.Closed &&
fromSrc.Graph[q].CostSoFar < int.MaxValue)
{
path = MakeBidiPath(fromSrc, fromDest, q);
break;
}
}
if (dbg != null && dbg.Visible)
{
dbg.AddLayer(fromSrc.Considered, fromSrc.MaxCost, fromSrc.Owner);
dbg.AddLayer(fromDest.Considered, fromDest.MaxCost, fromDest.Owner);
}
fromSrc.Graph.Dispose();
fromDest.Graph.Dispose();
if (path != null)
return path;
return EmptyPath;
}
static List<CPos> MakePath(CellLayer<CellInfo> cellInfo, CPos destination)
// Build the path from the destination. When we find a node that has the same previous
// position than itself, that node is the source node.
static List<CPos> MakePath(IGraph<CellInfo> cellInfo, CPos destination)
{
var ret = new List<CPos>();
var pathNode = destination;
var currentNode = destination;
while (cellInfo[pathNode].Path != pathNode)
while (cellInfo[currentNode].PreviousPos != currentNode)
{
ret.Add(pathNode);
pathNode = cellInfo[pathNode].Path;
ret.Add(currentNode);
currentNode = cellInfo[currentNode].PreviousPos;
}
ret.Add(pathNode);
ret.Add(currentNode);
CheckSanePath(ret);
return ret;
}
// Searches from both ends toward each other
public List<CPos> FindBidiPath(PathSearch fromSrc, PathSearch fromDest)
static List<CPos> MakeBidiPath(IPathSearch a, IPathSearch b, CPos confluenceNode)
{
using (new PerfSample("Pathfinder"))
{
using (fromSrc)
using (fromDest)
{
List<CPos> path = null;
while (!fromSrc.Queue.Empty && !fromDest.Queue.Empty)
{
/* make some progress on the first search */
var p = fromSrc.Expand(world);
if (fromDest.CellInfo[p].Seen &&
fromDest.CellInfo[p].MinCost < float.PositiveInfinity)
{
path = MakeBidiPath(fromSrc, fromDest, p);
break;
}
/* make some progress on the second search */
var q = fromDest.Expand(world);
if (fromSrc.CellInfo[q].Seen &&
fromSrc.CellInfo[q].MinCost < float.PositiveInfinity)
{
path = MakeBidiPath(fromSrc, fromDest, q);
break;
}
}
var dbg = world.WorldActor.TraitOrDefault<PathfinderDebugOverlay>();
if (dbg != null)
{
dbg.AddLayer(fromSrc.Considered.Select(p => new Pair<CPos, int>(p, fromSrc.CellInfo[p].MinCost)), fromSrc.MaxCost, fromSrc.Owner);
dbg.AddLayer(fromDest.Considered.Select(p => new Pair<CPos, int>(p, fromDest.CellInfo[p].MinCost)), fromDest.MaxCost, fromDest.Owner);
}
if (path != null)
return path;
}
return EmptyPath;
}
}
static List<CPos> MakeBidiPath(PathSearch a, PathSearch b, CPos p)
{
var ca = a.CellInfo;
var cb = b.CellInfo;
var ca = a.Graph;
var cb = b.Graph;
var ret = new List<CPos>();
var q = p;
while (ca[q].Path != q)
var q = confluenceNode;
while (ca[q].PreviousPos != q)
{
ret.Add(q);
q = ca[q].Path;
q = ca[q].PreviousPos;
}
ret.Add(q);
ret.Reverse();
q = p;
while (cb[q].Path != q)
q = confluenceNode;
while (cb[q].PreviousPos != q)
{
q = cb[q].Path;
q = cb[q].PreviousPos;
ret.Add(q);
}
@@ -246,22 +243,22 @@ namespace OpenRA.Mods.Common.Traits
}
[Conditional("SANITY_CHECKS")]
static void CheckSanePath(List<CPos> path)
static void CheckSanePath(IList<CPos> path)
{
if (path.Count == 0)
return;
var prev = path[0];
for (var i = 0; i < path.Count; i++)
foreach (var cell in path)
{
var d = path[i] - prev;
var d = cell - prev;
if (Math.Abs(d.X) > 1 || Math.Abs(d.Y) > 1)
throw new InvalidOperationException("(PathFinder) path sanity check failed");
prev = path[i];
prev = cell;
}
}
[Conditional("SANITY_CHECKS")]
static void CheckSanePath2(List<CPos> path, CPos src, CPos dest)
static void CheckSanePath2(IList<CPos> path, CPos src, CPos dest)
{
if (path.Count == 0)
return;
@@ -272,35 +269,4 @@ namespace OpenRA.Mods.Common.Traits
throw new InvalidOperationException("(PathFinder) sanity check failed: doesn't come from src");
}
}
public struct CellInfo
{
public int MinCost;
public CPos Path;
public bool Seen;
public CellInfo(int minCost, CPos path, bool seen)
{
MinCost = minCost;
Path = path;
Seen = seen;
}
}
public struct PathDistance : IComparable<PathDistance>
{
public readonly int EstTotal;
public readonly CPos Location;
public PathDistance(int estTotal, CPos location)
{
EstTotal = estTotal;
Location = location;
}
public int CompareTo(PathDistance other)
{
return Math.Sign(EstTotal - other.EstTotal);
}
}
}

View File

@@ -1,362 +0,0 @@
#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.Drawing;
using System.Linq;
using OpenRA;
using OpenRA.Primitives;
namespace OpenRA.Mods.Common.Traits
{
public sealed class PathSearch : IDisposable
{
public CellLayer<CellInfo> CellInfo;
public PriorityQueue<PathDistance> Queue;
public Func<CPos, int> Heuristic;
public bool CheckForBlocked;
public Actor IgnoredActor;
public bool InReverse;
public HashSet<CPos> Considered;
public Player Owner { get { return self.Owner; } }
public int MaxCost;
Actor self;
MobileInfo mobileInfo;
Func<CPos, int> customCost;
Func<CPos, bool> customBlock;
int laneBias = 1;
public PathSearch(World world, MobileInfo mobileInfo, Actor self)
{
this.self = self;
CellInfo = InitCellInfo();
this.mobileInfo = mobileInfo;
this.self = self;
customCost = null;
Queue = new PriorityQueue<PathDistance>();
Considered = new HashSet<CPos>();
MaxCost = 0;
}
public static PathSearch Search(World world, MobileInfo mi, Actor self, bool checkForBlocked)
{
var search = new PathSearch(world, mi, self)
{
CheckForBlocked = checkForBlocked
};
return search;
}
public static PathSearch FromPoint(World world, MobileInfo mi, Actor self, CPos from, CPos target, bool checkForBlocked)
{
var search = new PathSearch(world, mi, self)
{
Heuristic = DefaultEstimator(target),
CheckForBlocked = checkForBlocked
};
search.AddInitialCell(from);
return search;
}
public static PathSearch FromPoints(World world, MobileInfo mi, Actor self, IEnumerable<CPos> froms, CPos target, bool checkForBlocked)
{
var search = new PathSearch(world, mi, self)
{
Heuristic = DefaultEstimator(target),
CheckForBlocked = checkForBlocked
};
foreach (var sl in froms)
search.AddInitialCell(sl);
return search;
}
public 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);
// HACK: this relies on fp and cell-size assumptions.
var h = (3400 * diag / 24) + 100 * (straight - (2 * diag));
return (int)(h * 1.001);
};
}
public PathSearch Reverse()
{
InReverse = true;
return this;
}
public PathSearch WithCustomBlocker(Func<CPos, bool> customBlock)
{
this.customBlock = customBlock;
return this;
}
public PathSearch WithIgnoredActor(Actor b)
{
IgnoredActor = b;
return this;
}
public PathSearch WithHeuristic(Func<CPos, int> h)
{
Heuristic = h;
return this;
}
public PathSearch WithCustomCost(Func<CPos, int> w)
{
customCost = w;
return this;
}
public PathSearch WithoutLaneBias()
{
laneBias = 0;
return this;
}
public PathSearch FromPoint(CPos from)
{
AddInitialCell(from);
return this;
}
// 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 CVec[][] directedNeighbors = {
new CVec[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1), new CVec(-1, 0), new CVec(-1, 1) },
new CVec[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1) },
new CVec[] { new CVec(-1, -1), new CVec(0, -1), new CVec(1, -1), new CVec(1, 0), new CVec(1, 1) },
new CVec[] { new CVec(-1, -1), new CVec(-1, 0), new CVec(-1, 1) },
CVec.Directions,
new CVec[] { new CVec(1, -1), new CVec(1, 0), new CVec(1, 1) },
new CVec[] { new CVec(-1, -1), new CVec(-1, 0), new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
new CVec[] { new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
new CVec[] { new CVec(1, -1), new CVec(1, 0), new CVec(-1, 1), new CVec(0, 1), new CVec(1, 1) },
};
static CVec[] GetNeighbors(CPos p, CPos prev)
{
var dx = p.X - prev.X;
var dy = p.Y - prev.Y;
var index = dy * 3 + dx + 4;
return directedNeighbors[index];
}
public CPos Expand(World world)
{
var p = Queue.Pop();
while (CellInfo[p.Location].Seen)
{
if (Queue.Empty)
return p.Location;
p = Queue.Pop();
}
var pCell = CellInfo[p.Location];
pCell.Seen = true;
CellInfo[p.Location] = pCell;
var thisCost = mobileInfo.MovementCostForCell(world, p.Location);
if (thisCost == int.MaxValue)
return p.Location;
if (customCost != null)
{
var c = customCost(p.Location);
if (c == int.MaxValue)
return p.Location;
}
// This current cell is ok; check useful immediate directions:
Considered.Add(p.Location);
var directions = GetNeighbors(p.Location, pCell.Path);
for (var i = 0; i < directions.Length; ++i)
{
var d = directions[i];
var newHere = p.Location + d;
// Is this direction flat-out unusable or already seen?
if (!world.Map.Contains(newHere))
continue;
if (CellInfo[newHere].Seen)
continue;
// Now we may seriously consider this direction using heuristics:
var costHere = mobileInfo.MovementCostForCell(world, newHere);
if (costHere == int.MaxValue)
continue;
if (!mobileInfo.CanEnterCell(world, self, newHere, IgnoredActor, CheckForBlocked ? CellConditions.TransientActors : CellConditions.None))
continue;
if (customBlock != null && customBlock(newHere))
continue;
var est = Heuristic(newHere);
if (est == int.MaxValue)
continue;
var cellCost = costHere;
if (d.X * d.Y != 0)
cellCost = (cellCost * 34) / 24;
var userCost = 0;
if (customCost != null)
{
userCost = customCost(newHere);
cellCost += userCost;
}
// directional bonuses for smoother flow!
if (laneBias != 0)
{
var ux = newHere.X + (InReverse ? 1 : 0) & 1;
var uy = newHere.Y + (InReverse ? 1 : 0) & 1;
if (ux == 0 && d.Y < 0)
cellCost += laneBias;
else if (ux == 1 && d.Y > 0)
cellCost += laneBias;
if (uy == 0 && d.X < 0)
cellCost += laneBias;
else if (uy == 1 && d.X > 0)
cellCost += laneBias;
}
var newCost = CellInfo[p.Location].MinCost + cellCost;
// Cost is even higher; next direction:
if (newCost > CellInfo[newHere].MinCost)
continue;
var hereCell = CellInfo[newHere];
hereCell.Path = p.Location;
hereCell.MinCost = newCost;
CellInfo[newHere] = hereCell;
Queue.Add(new PathDistance(newCost + est, newHere));
if (newCost > MaxCost)
MaxCost = newCost;
Considered.Add(newHere);
}
return p.Location;
}
public void AddInitialCell(CPos location)
{
if (!self.World.Map.Contains(location))
return;
CellInfo[location] = new CellInfo(0, location, false);
Queue.Add(new PathDistance(Heuristic(location), location));
}
static readonly Queue<CellLayer<CellInfo>> CellInfoPool = new Queue<CellLayer<CellInfo>>();
static readonly object DefaultCellInfoLayerSync = new object();
static CellLayer<CellInfo> defaultCellInfoLayer;
static CellLayer<CellInfo> GetFromPool()
{
lock (CellInfoPool)
return CellInfoPool.Dequeue();
}
static void PutBackIntoPool(CellLayer<CellInfo> ci)
{
lock (CellInfoPool)
CellInfoPool.Enqueue(ci);
}
CellLayer<CellInfo> InitCellInfo()
{
CellLayer<CellInfo> result = null;
var map = self.World.Map;
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 = new CellLayer<CellInfo>(map);
for (var v = 0; v < mapSize.Height; v++)
for (var u = 0; u < mapSize.Width; u++)
defaultCellInfoLayer[new MPos(u, v)] = new CellInfo(int.MaxValue, new MPos(u, v).ToCPos(map), false);
}
result.CopyValuesFrom(defaultCellInfoLayer);
}
return result;
}
bool disposed;
public void Dispose()
{
if (disposed)
return;
disposed = true;
PutBackIntoPool(CellInfo);
CellInfo = null;
GC.SuppressFinalize(this);
}
~PathSearch() { Dispose(); }
}
}

View File

@@ -11,7 +11,6 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using OpenRA;
using OpenRA.Graphics;
using OpenRA.Primitives;
using OpenRA.Traits;