From 6dc189b7d1de097e30301b3e382578b7600c0e70 Mon Sep 17 00:00:00 2001 From: RoosterDragon Date: Sat, 27 Nov 2021 12:23:08 +0000 Subject: [PATCH] Rearrange various API surfaces related to pathfinding. The existing APIs surfaces for pathfinding are in a wonky shape. We rearrange various responsibilities to better locations and simplify some abstractions that aren't providing value. - IPathSearch, BasePathSearch and PathSearch are combined into only PathSearch. Its role is now to run a search space over a graph, maintaining the open queue and evaluating the provided heuristic function. The builder-like methods (WithHeuristic, Reverse, FromPoint, etc) are removed in favour of optional parameters in static creation methods. This removes confusion between the builder-aspect and the search function itself. It also becomes responsible for applying the heuristic weight to the heuristic. This fixes an issue where an externally provided heuristic ignored the weighting adjustment, as previously the weight was baked into the default heuristic only. - Reduce the IGraph interface to the concepts of nodes and edges. Make it non-generic as it is specifically for pathfinding, and rename to IPathGraph accordingly. This is sufficient for a PathSearch to perform a search over any given IGraph. The various customization options are concrete properties of PathGraph only. - PathFinder does not need to deal with disposal of the search/graph, that is the caller's responsibility. - Remove CustomBlock from PathGraph as it was unused. - Remove FindUnitPathToRange as it was unused. - Use PathFinder.NoPath as the single helper to represent no/empty paths. --- .../Activities/FindAndDeliverResources.cs | 15 +- OpenRA.Mods.Common/Activities/Move/Move.cs | 15 +- .../Activities/Move/MoveAdjacentTo.cs | 8 +- .../Pathfinder/BasePathSearch.cs | 182 ---------------- OpenRA.Mods.Common/Pathfinder/IPathGraph.cs | 68 ++++++ OpenRA.Mods.Common/Pathfinder/PathGraph.cs | 153 +++++-------- OpenRA.Mods.Common/Pathfinder/PathSearch.cs | 203 +++++++++++++----- .../Traits/BotModules/HarvesterBotModule.cs | 12 +- OpenRA.Mods.Common/Traits/Harvester.cs | 7 +- OpenRA.Mods.Common/Traits/Mobile.cs | 6 +- OpenRA.Mods.Common/Traits/World/ActorMap.cs | 8 +- OpenRA.Mods.Common/Traits/World/Locomotor.cs | 8 +- OpenRA.Mods.Common/Traits/World/PathFinder.cs | 192 ++++++----------- 13 files changed, 379 insertions(+), 498 deletions(-) delete mode 100644 OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs create mode 100644 OpenRA.Mods.Common/Pathfinder/IPathGraph.cs diff --git a/OpenRA.Mods.Common/Activities/FindAndDeliverResources.cs b/OpenRA.Mods.Common/Activities/FindAndDeliverResources.cs index b9d715b2a6..7f729637eb 100644 --- a/OpenRA.Mods.Common/Activities/FindAndDeliverResources.cs +++ b/OpenRA.Mods.Common/Activities/FindAndDeliverResources.cs @@ -185,9 +185,14 @@ namespace OpenRA.Mods.Common.Activities // Find any harvestable resources: List 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 => + using (var search = PathSearch.ToTargetCellByPredicate( + self.World, mobile.Locomotor, self, new[] { searchFromLoc, self.Location }, + loc => + domainIndex.IsPassable(self.Location, loc, mobile.Locomotor) && + harv.CanHarvestCell(self, loc) && + claimLayer.CanClaimCell(self, loc), + BlockedByActor.Stationary, + loc => { if ((loc - searchFromLoc).LengthSquared > searchRadiusSquared) return PathGraph.PathCostForInvalidPath; @@ -216,9 +221,7 @@ namespace OpenRA.Mods.Common.Activities } return 0; - }) - .FromPoint(searchFromLoc) - .FromPoint(self.Location)) + })) path = mobile.Pathfinder.FindPath(search); if (path.Count > 0) diff --git a/OpenRA.Mods.Common/Activities/Move/Move.cs b/OpenRA.Mods.Common/Activities/Move/Move.cs index a861d2819e..1a447adaf8 100644 --- a/OpenRA.Mods.Common/Activities/Move/Move.cs +++ b/OpenRA.Mods.Common/Activities/Move/Move.cs @@ -22,8 +22,6 @@ namespace OpenRA.Mods.Common.Activities { public class Move : Activity { - static readonly List NoPath = new List(); - readonly Mobile mobile; readonly WDist nearEnough; readonly Func> getPath; @@ -52,7 +50,7 @@ namespace OpenRA.Mods.Common.Activities readonly bool evaluateNearestMovableCell; // Scriptable move order - // Ignores lane bias and nearby units + // Ignores lane bias public Move(Actor self, CPos destination, Color? targetLineColor = null) { // PERF: Because we can be sure that OccupiesSpace is Mobile here, we can save some performance by avoiding querying for the trait. @@ -60,12 +58,9 @@ namespace OpenRA.Mods.Common.Activities getPath = check => { - List path; - using (var search = - PathSearch.FromPoint(self.World, mobile.Locomotor, self, mobile.ToCell, destination, check) - .WithoutLaneBias()) - path = mobile.Pathfinder.FindPath(search); - return path; + using (var search = PathSearch.ToTargetCell( + self.World, mobile.Locomotor, self, mobile.ToCell, destination, check, laneBias: false)) + return mobile.Pathfinder.FindPath(search); }; this.destination = destination; @@ -82,7 +77,7 @@ namespace OpenRA.Mods.Common.Activities getPath = check => { if (!this.destination.HasValue) - return NoPath; + return PathFinder.NoPath; return mobile.Pathfinder.FindUnitPath(mobile.ToCell, this.destination.Value, self, ignoreActor, check); }; diff --git a/OpenRA.Mods.Common/Activities/Move/MoveAdjacentTo.cs b/OpenRA.Mods.Common/Activities/Move/MoveAdjacentTo.cs index 2a622c46d9..a3bcf19b31 100644 --- a/OpenRA.Mods.Common/Activities/Move/MoveAdjacentTo.cs +++ b/OpenRA.Mods.Common/Activities/Move/MoveAdjacentTo.cs @@ -21,8 +21,6 @@ namespace OpenRA.Mods.Common.Activities { public class MoveAdjacentTo : Activity { - static readonly List NoPath = new List(); - protected readonly Mobile Mobile; readonly DomainIndex domainIndex; readonly Color? targetLineColor; @@ -130,10 +128,10 @@ namespace OpenRA.Mods.Common.Activities } if (!searchCells.Any()) - return NoPath; + return PathFinder.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()) + using (var fromSrc = PathSearch.ToTargetCell(self.World, Mobile.Locomotor, self, searchCells, loc, check)) + using (var fromDest = PathSearch.ToTargetCell(self.World, Mobile.Locomotor, self, loc, lastVisibleTargetLocation, check, inReverse: true)) return Mobile.Pathfinder.FindBidiPath(fromSrc, fromDest); } diff --git a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs b/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs deleted file mode 100644 index ce3426b371..0000000000 --- a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs +++ /dev/null @@ -1,182 +0,0 @@ -#region Copyright & License Information -/* - * Copyright 2007-2021 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, either version 3 of - * the License, or (at your option) any later version. For more - * information, see COPYING. - */ -#endregion - -using System; -using System.Linq; -using OpenRA.Mods.Common.Traits; -using OpenRA.Primitives; - -namespace OpenRA.Mods.Common.Pathfinder -{ - public interface IPathSearch : IDisposable - { - /// - /// The Graph used by the A* - /// - IGraph Graph { get; } - - Player Owner { get; } - - IPathSearch Reverse(); - - IPathSearch WithCustomBlocker(Func customBlock); - - IPathSearch WithIgnoredActor(Actor b); - - IPathSearch WithHeuristic(Func h); - - IPathSearch WithHeuristicWeight(int percentage); - - IPathSearch WithCustomCost(Func w); - - IPathSearch WithoutLaneBias(); - - IPathSearch FromPoint(CPos from); - - /// - /// 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). - /// - /// The location to assess - /// Whether the location is a target - bool IsTarget(CPos location); - - bool CanExpand { get; } - CPos Expand(); - } - - public abstract class BasePathSearch : IPathSearch - { - public IGraph Graph { get; set; } - - protected IPriorityQueue OpenQueue { get; private set; } - - public Player Owner => Graph.Actor.Owner; - protected Func heuristic; - protected Func isGoal; - protected int heuristicWeightPercentage; - - // 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 readonly IPriorityQueue StartPoints; - - readonly short cellCost; - readonly int diagonalCellCost; - - protected BasePathSearch(IGraph graph) - { - Graph = graph; - OpenQueue = new PriorityQueue(GraphConnection.ConnectionCostComparer); - StartPoints = new PriorityQueue(GraphConnection.ConnectionCostComparer); - heuristicWeightPercentage = 100; - - // Determine the minimum possible cost for moving horizontally between cells based on terrain speeds. - // The minimum possible cost diagonally is then Sqrt(2) times more costly. - cellCost = ((Mobile)graph.Actor.OccupiesSpace).Info.LocomotorInfo.TerrainSpeeds.Values.Min(ti => ti.Cost); - diagonalCellCost = Exts.MultiplyBySqrtTwo(cellCost); - } - - /// - /// Default: Diagonal distance heuristic. More information: - /// http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html - /// - /// A delegate that calculates the estimation for a node - protected Func 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 (cellCost * straight + (diagonalCellCost - 2 * cellCost) * diag) * heuristicWeightPercentage / 100; - }; - } - - public IPathSearch Reverse() - { - Graph.InReverse = true; - return this; - } - - public IPathSearch WithCustomBlocker(Func customBlock) - { - Graph.CustomBlock = customBlock; - return this; - } - - public IPathSearch WithIgnoredActor(Actor b) - { - Graph.IgnoreActor = b; - return this; - } - - public IPathSearch WithHeuristic(Func h) - { - heuristic = h; - return this; - } - - public IPathSearch WithHeuristicWeight(int percentage) - { - heuristicWeightPercentage = percentage; - return this; - } - - public IPathSearch WithCustomCost(Func w) - { - Graph.CustomCost = w; - return this; - } - - public IPathSearch WithoutLaneBias() - { - Graph.LaneBias = 0; - return this; - } - - public IPathSearch FromPoint(CPos from) - { - if (Graph.World.Map.Contains(from)) - AddInitialCell(from); - - return this; - } - - protected abstract void AddInitialCell(CPos cell); - - public bool IsTarget(CPos location) - { - return isGoal(location); - } - - public bool CanExpand => !OpenQueue.Empty; - public abstract CPos Expand(); - - protected virtual void Dispose(bool disposing) - { - if (disposing) - Graph.Dispose(); - } - - public void Dispose() - { - Dispose(true); - GC.SuppressFinalize(this); - } - } -} diff --git a/OpenRA.Mods.Common/Pathfinder/IPathGraph.cs b/OpenRA.Mods.Common/Pathfinder/IPathGraph.cs new file mode 100644 index 0000000000..19715872be --- /dev/null +++ b/OpenRA.Mods.Common/Pathfinder/IPathGraph.cs @@ -0,0 +1,68 @@ +#region Copyright & License Information +/* + * Copyright 2007-2021 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, either version 3 of + * the License, or (at your option) any later version. For more + * information, see COPYING. + */ +#endregion + +using System; +using System.Collections.Generic; + +namespace OpenRA.Mods.Common.Pathfinder +{ + /// + /// Represents a pathfinding graph with nodes and edges. + /// Nodes are represented as cells, and pathfinding information + /// in the form of is attached to each one. + /// + public interface IPathGraph : IDisposable + { + /// + /// Given a source node, returns connections to all reachable destination nodes with their cost. + /// + List GetConnections(CPos source); + + /// + /// Gets or sets the pathfinding information for a given node. + /// + CellInfo this[CPos node] { get; set; } + } + + /// + /// Represents part of an edge in a graph, giving the cost to traverse to a node. + /// + public readonly struct GraphConnection + { + public static readonly CostComparer ConnectionCostComparer = CostComparer.Instance; + + public sealed class CostComparer : IComparer + { + public static readonly CostComparer Instance = new CostComparer(); + CostComparer() { } + public int Compare(GraphConnection x, GraphConnection y) + { + return x.Cost.CompareTo(y.Cost); + } + } + + public readonly CPos Destination; + public readonly int Cost; + + public GraphConnection(CPos destination, int cost) + { + if (cost < 0) + throw new ArgumentOutOfRangeException(nameof(cost), $"{nameof(cost)} cannot be negative"); + if (cost == PathGraph.PathCostForInvalidPath) + throw new ArgumentOutOfRangeException(nameof(cost), $"{nameof(cost)} cannot be used for an unreachable path"); + + Destination = destination; + Cost = cost; + } + + public override string ToString() => $"-> {Destination} = {Cost}"; + } +} diff --git a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs b/OpenRA.Mods.Common/Pathfinder/PathGraph.cs index d0ee88b82b..14da1c20b3 100644 --- a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs +++ b/OpenRA.Mods.Common/Pathfinder/PathGraph.cs @@ -18,100 +18,54 @@ using OpenRA.Traits; namespace OpenRA.Mods.Common.Pathfinder { /// - /// Represents a graph with nodes and edges + /// A dense pathfinding graph that supports a search over all cells within a map. + /// It implements the ability to cost and get connections for cells, and supports . /// - /// The type of node used in the graph - public interface IGraph : IDisposable - { - /// - /// Gets all the Connections for a given node in the graph - /// - List GetConnections(CPos position); - - /// - /// Retrieves an object given a node in the graph - /// - T this[CPos pos] { get; set; } - - Func CustomBlock { get; set; } - - Func CustomCost { get; set; } - - int LaneBias { get; set; } - - bool InReverse { get; set; } - - Actor IgnoreActor { get; set; } - - World World { get; } - - Actor Actor { get; } - } - - public readonly struct GraphConnection - { - public static readonly CostComparer ConnectionCostComparer = CostComparer.Instance; - - public sealed class CostComparer : IComparer - { - public static readonly CostComparer Instance = new CostComparer(); - CostComparer() { } - public int Compare(GraphConnection x, GraphConnection y) - { - return x.Cost.CompareTo(y.Cost); - } - } - - public readonly CPos Destination; - public readonly int Cost; - - public GraphConnection(CPos destination, int cost) - { - Destination = destination; - Cost = cost; - } - } - - sealed class PathGraph : IGraph + sealed class PathGraph : IPathGraph { public const int PathCostForInvalidPath = int.MaxValue; public const short MovementCostForUnreachableCell = short.MaxValue; + const int LaneBiasCost = 1; - public Actor Actor { get; private set; } - public World World { get; private set; } - public Func CustomBlock { get; set; } - public Func CustomCost { get; set; } - public int LaneBias { get; set; } - public bool InReverse { get; set; } - public Actor IgnoreActor { get; set; } - - readonly BlockedByActor checkConditions; + readonly ICustomMovementLayer[] customMovementLayers; + readonly int customMovementLayersEnabledForLocomotor; readonly Locomotor locomotor; - readonly CellInfoLayerPool.PooledCellInfoLayer pooledLayer; + readonly Actor actor; + readonly World world; + readonly BlockedByActor check; + readonly Func customCost; + readonly Actor ignoreActor; + readonly bool inReverse; + readonly bool laneBias; readonly bool checkTerrainHeight; + readonly CellInfoLayerPool.PooledCellInfoLayer pooledLayer; readonly CellLayer[] cellInfoForLayer; - public PathGraph(CellInfoLayerPool layerPool, Locomotor locomotor, Actor actor, World world, BlockedByActor check) + public PathGraph(CellInfoLayerPool layerPool, Locomotor locomotor, Actor actor, World world, BlockedByActor check, + Func customCost, Actor ignoreActor, bool inReverse, bool laneBias) { + customMovementLayers = world.GetCustomMovementLayers(); + customMovementLayersEnabledForLocomotor = customMovementLayers.Count(cml => cml != null && cml.EnabledForLocomotor(locomotor.Info)); this.locomotor = locomotor; + this.world = world; + this.actor = actor; + this.check = check; + this.customCost = customCost; + this.ignoreActor = ignoreActor; + this.inReverse = inReverse; + this.laneBias = laneBias; + checkTerrainHeight = world.Map.Grid.MaximumTerrainHeight > 0; // As we support a search over the whole map area, // use the pool to grab the CellInfos we need to track the graph state. // This allows us to avoid the cost of allocating large arrays constantly. // PERF: Avoid LINQ - var cmls = world.GetCustomMovementLayers(); pooledLayer = layerPool.Get(); - cellInfoForLayer = new CellLayer[cmls.Length]; + cellInfoForLayer = new CellLayer[customMovementLayers.Length]; cellInfoForLayer[0] = pooledLayer.GetLayer(); - foreach (var cml in cmls) + foreach (var cml in customMovementLayers) if (cml != null && cml.EnabledForLocomotor(locomotor.Info)) cellInfoForLayer[cml.Index] = pooledLayer.GetLayer(); - - World = world; - Actor = actor; - LaneBias = 1; - checkConditions = check; - checkTerrainHeight = world.Map.Grid.MaximumTerrainHeight > 0; } // Sets of neighbors for each incoming direction. These exclude the neighbors which are guaranteed @@ -156,35 +110,34 @@ namespace OpenRA.Mods.Common.Pathfinder public List GetConnections(CPos position) { var layer = position.Layer; - var info = cellInfoForLayer[layer]; - var previousNode = info[position].PreviousNode; + var info = this[position]; + var previousNode = info.PreviousNode; var dx = position.X - previousNode.X; var dy = position.Y - previousNode.Y; var index = dy * 3 + dx + 4; - var heightLayer = World.Map.Height; + var heightLayer = world.Map.Height; var directions = (checkTerrainHeight && layer == 0 && previousNode.Layer == 0 && heightLayer[position] != heightLayer[previousNode] ? DirectedNeighborsConservative : DirectedNeighbors)[index]; - var validNeighbors = new List(directions.Length + (layer == 0 ? cellInfoForLayer.Length : 1)); + var validNeighbors = new List(directions.Length + (layer == 0 ? customMovementLayersEnabledForLocomotor : 1)); for (var i = 0; i < directions.Length; i++) { var dir = directions[i]; var neighbor = position + dir; - var pathCost = GetPathCostToNode(position, neighbor, dir); - // PERF: Skip closed cells already, 15% of all cells - if (pathCost != PathCostForInvalidPath && info[neighbor].Status != CellStatus.Closed) + var pathCost = GetPathCostToNode(position, neighbor, dir); + if (pathCost != PathCostForInvalidPath && + this[neighbor].Status != CellStatus.Closed) validNeighbors.Add(new GraphConnection(neighbor, pathCost)); } - var cmls = World.GetCustomMovementLayers(); if (layer == 0) { - foreach (var cml in cmls) + foreach (var cml in customMovementLayers) { if (cml == null || !cml.EnabledForLocomotor(locomotor.Info)) continue; @@ -199,12 +152,12 @@ namespace OpenRA.Mods.Common.Pathfinder } else { - var layerPosition = new CPos(position.X, position.Y, 0); - var exitCost = cmls[layer].ExitMovementCost(locomotor.Info, layerPosition); + var groundPosition = new CPos(position.X, position.Y, 0); + var exitCost = customMovementLayers[layer].ExitMovementCost(locomotor.Info, groundPosition); if (exitCost != MovementCostForUnreachableCell && - CanEnterNode(position, layerPosition) && - this[layerPosition].Status != CellStatus.Closed) - validNeighbors.Add(new GraphConnection(layerPosition, exitCost)); + CanEnterNode(position, groundPosition) && + this[groundPosition].Status != CellStatus.Closed) + validNeighbors.Add(new GraphConnection(groundPosition, exitCost)); } return validNeighbors; @@ -213,14 +166,14 @@ namespace OpenRA.Mods.Common.Pathfinder bool CanEnterNode(CPos srcNode, CPos destNode) { return - locomotor.MovementCostToEnterCell(Actor, srcNode, destNode, checkConditions, IgnoreActor) + locomotor.MovementCostToEnterCell(actor, srcNode, destNode, check, ignoreActor) != MovementCostForUnreachableCell; } int GetPathCostToNode(CPos srcNode, CPos destNode, CVec direction) { - var movementCost = locomotor.MovementCostToEnterCell(Actor, srcNode, destNode, checkConditions, IgnoreActor); - if (movementCost != MovementCostForUnreachableCell && !(CustomBlock != null && CustomBlock(destNode))) + var movementCost = locomotor.MovementCostToEnterCell(actor, srcNode, destNode, check, ignoreActor); + if (movementCost != MovementCostForUnreachableCell) return CalculateCellPathCost(destNode, direction, movementCost); return PathCostForInvalidPath; @@ -232,26 +185,26 @@ namespace OpenRA.Mods.Common.Pathfinder ? Exts.MultiplyBySqrtTwo(movementCost) : movementCost; - if (CustomCost != null) + if (customCost != null) { - var customCost = CustomCost(neighborCPos); - if (customCost == PathCostForInvalidPath) + var customCellCost = customCost(neighborCPos); + if (customCellCost == PathCostForInvalidPath) return PathCostForInvalidPath; - cellCost += customCost; + cellCost += customCellCost; } // Directional bonuses for smoother flow! - if (LaneBias != 0) + if (laneBias) { - var ux = neighborCPos.X + (InReverse ? 1 : 0) & 1; - var uy = neighborCPos.Y + (InReverse ? 1 : 0) & 1; + 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; + cellCost += LaneBiasCost; if ((uy == 0 && direction.X < 0) || (uy == 1 && direction.X > 0)) - cellCost += LaneBias; + cellCost += LaneBiasCost; } return cellCost; diff --git a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs b/OpenRA.Mods.Common/Pathfinder/PathSearch.cs index fb66eed67a..c677b50241 100644 --- a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs +++ b/OpenRA.Mods.Common/Pathfinder/PathSearch.cs @@ -11,14 +11,29 @@ using System; using System.Collections.Generic; +using System.Linq; using System.Runtime.CompilerServices; using OpenRA.Mods.Common.Traits; +using OpenRA.Primitives; using OpenRA.Traits; namespace OpenRA.Mods.Common.Pathfinder { - public sealed class PathSearch : BasePathSearch + public sealed class PathSearch : IDisposable { + /// + /// When searching for paths, use a default weight of 125% to reduce + /// computation effort - even if this means paths may be sub-optimal. + /// + public const int DefaultHeuristicWeightPercentage = 125; + + /// + /// When searching for paths, use a lane bias to guide units into + /// "lanes" whilst moving, to promote smooth unit flow when groups of + /// units are moving. + /// + public const bool DefaultLaneBias = true; + // PERF: Maintain a pool of layers used for paths searches for each world. These searches are performed often // so we wish to avoid the high cost of initializing a new search space every time by reusing the old ones. static readonly ConditionalWeakTable LayerPoolTable = new ConditionalWeakTable(); @@ -29,46 +44,12 @@ namespace OpenRA.Mods.Common.Pathfinder return LayerPoolTable.GetValue(world, CreateLayerPool); } - PathSearch(IGraph graph) - : base(graph) + public static PathSearch ToTargetCellByPredicate( + World world, Locomotor locomotor, Actor self, IEnumerable froms, Func targetPredicate, BlockedByActor check, + Func customCost = null) { - } - - public static IPathSearch Search(World world, Locomotor locomotor, Actor self, BlockedByActor check, Func goalCondition) - { - var graph = new PathGraph(LayerPoolForWorld(world), locomotor, self, world, check); - return new PathSearch(graph) - { - isGoal = goalCondition, - heuristic = loc => 0 - }; - } - - public static IPathSearch FromPoint(World world, Locomotor locomotor, Actor self, CPos @from, CPos target, BlockedByActor check) - { - return FromPoints(world, locomotor, self, new[] { from }, target, check); - } - - public static IPathSearch FromPoints(World world, Locomotor locomotor, Actor self, IEnumerable froms, CPos target, BlockedByActor check) - { - var graph = new PathGraph(LayerPoolForWorld(world), locomotor, self, world, check); - var search = new PathSearch(graph); - search.heuristic = search.DefaultEstimator(target); - - // The search will aim for the shortest path by default, a weight of 100%. - // We can allow the search to find paths that aren't optimal by changing the weight. - // We provide a weight that limits the worst case length of the path, - // e.g. a weight of 110% will find a path no more than 10% longer than the shortest possible. - // The benefit of allowing the search to return suboptimal paths is faster computation time. - // The search can skip some areas of the search space, meaning it has less work to do. - // We allow paths up to 25% longer than the shortest, optimal path, to improve pathfinding time. - search.heuristicWeightPercentage = 125; - - search.isGoal = loc => - { - var locInfo = search.Graph[loc]; - return locInfo.EstimatedTotalCost - locInfo.CostSoFar == 0; - }; + var graph = new PathGraph(LayerPoolForWorld(world), locomotor, self, world, check, customCost, null, false, DefaultLaneBias); + var search = new PathSearch(graph, loc => 0, DefaultHeuristicWeightPercentage, targetPredicate); foreach (var sl in froms) if (world.Map.Contains(sl)) @@ -77,30 +58,131 @@ namespace OpenRA.Mods.Common.Pathfinder return search; } - protected override void AddInitialCell(CPos location) + public static PathSearch ToTargetCell( + World world, Locomotor locomotor, Actor self, CPos from, CPos target, BlockedByActor check, + Func customCost = null, + Actor ignoreActor = null, + bool inReverse = false, + bool laneBias = DefaultLaneBias) { - var cost = heuristic(location); - Graph[location] = new CellInfo(CellStatus.Open, 0, cost, location); - var connection = new GraphConnection(location, cost); - OpenQueue.Add(connection); - StartPoints.Add(connection); + return ToTargetCell(world, locomotor, self, new[] { from }, target, check, customCost, ignoreActor, inReverse, laneBias); + } + + public static PathSearch ToTargetCell( + World world, Locomotor locomotor, Actor self, IEnumerable froms, CPos target, BlockedByActor check, + Func customCost = null, + Actor ignoreActor = null, + bool inReverse = false, + bool laneBias = DefaultLaneBias, + Func heuristic = null, + int heuristicWeightPercentage = DefaultHeuristicWeightPercentage) + { + var graph = new PathGraph(LayerPoolForWorld(world), locomotor, self, world, check, customCost, ignoreActor, inReverse, laneBias); + + heuristic = heuristic ?? DefaultCostEstimator(locomotor, target); + var search = new PathSearch(graph, heuristic, heuristicWeightPercentage, loc => loc == target); + + foreach (var sl in froms) + if (world.Map.Contains(sl)) + search.AddInitialCell(sl); + + return search; } /// - /// This function analyzes the neighbors of the most promising node in the Pathfinding graph + /// Default: Diagonal distance heuristic. More information: + /// http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html + /// Layers are ignored and incur no additional cost. + /// + /// Locomotor used to provide terrain costs. + /// The cell for which costs are to be given by the estimation function. + /// A delegate that calculates the cost estimation between the and the given cell. + public static Func DefaultCostEstimator(Locomotor locomotor, CPos destination) + { + var estimator = DefaultCostEstimator(locomotor); + return here => estimator(here, destination); + } + + /// + /// Default: Diagonal distance heuristic. More information: + /// http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html + /// Layers are ignored and incur no additional cost. + /// + /// Locomotor used to provide terrain costs. + /// A delegate that calculates the cost estimation between the given cells. + public static Func DefaultCostEstimator(Locomotor locomotor) + { + // Determine the minimum possible cost for moving horizontally between cells based on terrain speeds. + // The minimum possible cost diagonally is then Sqrt(2) times more costly. + var cellCost = locomotor.Info.TerrainSpeeds.Values.Min(ti => ti.Cost); + var diagonalCellCost = Exts.MultiplyBySqrtTwo(cellCost); + return (here, destination) => + { + 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 cellCost * straight + (diagonalCellCost - 2 * cellCost) * diag; + }; + } + + public IPathGraph Graph { get; } + readonly Func heuristic; + readonly int heuristicWeightPercentage; + public Func TargetPredicate { get; set; } + readonly IPriorityQueue openQueue; + + /// + /// Initialize a new search. + /// + /// Graph over which the search is conducted. + /// Provides an estimation of the distance between the given cell and the target. + /// + /// The search will aim for the shortest path when given a weight of 100%. + /// We can allow the search to find paths that aren't optimal by changing the weight. + /// The weight limits the worst case length of the path, + /// e.g. a weight of 110% will find a path no more than 10% longer than the shortest possible. + /// The benefit of allowing the search to return suboptimal paths is faster computation time. + /// The search can skip some areas of the search space, meaning it has less work to do. + /// + /// Determines if the given cell is the target. + PathSearch(IPathGraph graph, Func heuristic, int heuristicWeightPercentage, Func targetPredicate) + { + Graph = graph; + this.heuristic = heuristic; + this.heuristicWeightPercentage = heuristicWeightPercentage; + TargetPredicate = targetPredicate; + openQueue = new PriorityQueue(GraphConnection.ConnectionCostComparer); + } + + void AddInitialCell(CPos location) + { + var estimatedCost = heuristic(location) * heuristicWeightPercentage / 100; + Graph[location] = new CellInfo(CellStatus.Open, 0, estimatedCost, location); + var connection = new GraphConnection(location, estimatedCost); + openQueue.Add(connection); + } + + /// + /// Determines if there are more reachable cells and the search can be continued. + /// If false, can no longer be called. + /// + public bool CanExpand => !openQueue.Empty; + + /// + /// This function analyzes the neighbors of the most promising node in the pathfinding graph /// using the A* algorithm (A-star) and returns that node /// /// The most promising node of the iteration - public override CPos Expand() + public CPos Expand() { - var currentMinNode = OpenQueue.Pop().Destination; + var currentMinNode = openQueue.Pop().Destination; var currentInfo = Graph[currentMinNode]; Graph[currentMinNode] = new CellInfo(CellStatus.Closed, currentInfo.CostSoFar, currentInfo.EstimatedTotalCost, currentInfo.PreviousNode); - if (Graph.CustomCost != null && Graph.CustomCost(currentMinNode) == PathGraph.PathCostForInvalidPath) - return currentMinNode; - foreach (var connection in Graph.GetConnections(currentMinNode)) { // Calculate the cost up to that point @@ -121,16 +203,29 @@ namespace OpenRA.Mods.Common.Pathfinder if (neighborInfo.Status == CellStatus.Open) estimatedRemainingCostToTarget = neighborInfo.EstimatedTotalCost - neighborInfo.CostSoFar; else - estimatedRemainingCostToTarget = heuristic(neighbor); + estimatedRemainingCostToTarget = heuristic(neighbor) * heuristicWeightPercentage / 100; var estimatedTotalCostToTarget = costSoFarToNeighbor + estimatedRemainingCostToTarget; Graph[neighbor] = new CellInfo(CellStatus.Open, costSoFarToNeighbor, estimatedTotalCostToTarget, currentMinNode); if (neighborInfo.Status != CellStatus.Open) - OpenQueue.Add(new GraphConnection(neighbor, estimatedTotalCostToTarget)); + openQueue.Add(new GraphConnection(neighbor, estimatedTotalCostToTarget)); } return currentMinNode; } + + /// + /// Determines if is the target of the search. + /// + public bool IsTarget(CPos location) + { + return TargetPredicate(location); + } + + public void Dispose() + { + Graph.Dispose(); + } } } diff --git a/OpenRA.Mods.Common/Traits/BotModules/HarvesterBotModule.cs b/OpenRA.Mods.Common/Traits/BotModules/HarvesterBotModule.cs index d5280bd041..7133ff8bfc 100644 --- a/OpenRA.Mods.Common/Traits/BotModules/HarvesterBotModule.cs +++ b/OpenRA.Mods.Common/Traits/BotModules/HarvesterBotModule.cs @@ -149,12 +149,14 @@ namespace OpenRA.Mods.Common.Traits harv.Harvester.CanHarvestCell(actor, cell) && claimLayer.CanClaimCell(actor, cell); - var path = pathfinder.FindPath( - PathSearch.Search(world, harv.Locomotor, actor, BlockedByActor.Stationary, isValidResource) - .WithCustomCost(loc => world.FindActorsInCircle(world.Map.CenterOfCell(loc), Info.HarvesterEnemyAvoidanceRadius) + List path; + using (var search = + PathSearch.ToTargetCellByPredicate( + world, harv.Locomotor, actor, new[] { actor.Location }, isValidResource, BlockedByActor.Stationary, + loc => world.FindActorsInCircle(world.Map.CenterOfCell(loc), Info.HarvesterEnemyAvoidanceRadius) .Where(u => !u.IsDead && actor.Owner.RelationshipWith(u.Owner) == PlayerRelationship.Enemy) - .Sum(u => Math.Max(WDist.Zero.Length, Info.HarvesterEnemyAvoidanceRadius.Length - (world.Map.CenterOfCell(loc) - u.CenterPosition).Length))) - .FromPoint(actor.Location)); + .Sum(u => Math.Max(WDist.Zero.Length, Info.HarvesterEnemyAvoidanceRadius.Length - (world.Map.CenterOfCell(loc) - u.CenterPosition).Length)))) + path = pathfinder.FindPath(search); if (path.Count == 0) return Target.Invalid; diff --git a/OpenRA.Mods.Common/Traits/Harvester.cs b/OpenRA.Mods.Common/Traits/Harvester.cs index 99545c3ddd..f0efcce42b 100644 --- a/OpenRA.Mods.Common/Traits/Harvester.cs +++ b/OpenRA.Mods.Common/Traits/Harvester.cs @@ -195,8 +195,9 @@ namespace OpenRA.Mods.Common.Traits // Start a search from each refinery's delivery location: List path; - using (var search = PathSearch.FromPoints(self.World, mobile.Locomotor, self, refineries.Select(r => r.Key), self.Location, BlockedByActor.None) - .WithCustomCost(location => + using (var search = PathSearch.ToTargetCell( + self.World, mobile.Locomotor, self, refineries.Select(r => r.Key), self.Location, BlockedByActor.None, + location => { if (!refineries.Contains(location)) return 0; @@ -212,7 +213,7 @@ namespace OpenRA.Mods.Common.Traits })) path = mobile.Pathfinder.FindPath(search); - if (path.Count != 0) + if (path.Count > 0) return refineries[path.Last()].First().Actor; return null; diff --git a/OpenRA.Mods.Common/Traits/Mobile.cs b/OpenRA.Mods.Common/Traits/Mobile.cs index ff959639df..9ff0476147 100644 --- a/OpenRA.Mods.Common/Traits/Mobile.cs +++ b/OpenRA.Mods.Common/Traits/Mobile.cs @@ -799,7 +799,6 @@ namespace OpenRA.Mods.Common.Traits notifyCrushed.Trait.WarnCrush(notifyCrushed.Actor, self, Info.LocomotorInfo.Crushes); } - public Activity ScriptedMove(CPos cell) { return new Move(self, cell); } public Activity MoveTo(Func> pathFunc) { return new Move(self, pathFunc); } Activity LocalMove(Actor self, WPos fromPos, WPos toPos, CPos cell) @@ -821,9 +820,8 @@ namespace OpenRA.Mods.Common.Traits return above; List path; - using (var search = PathSearch.Search(self.World, Locomotor, self, BlockedByActor.All, - loc => loc.Layer == 0 && CanEnterCell(loc)) - .FromPoint(self.Location)) + using (var search = PathSearch.ToTargetCellByPredicate( + self.World, Locomotor, self, new[] { self.Location }, loc => loc.Layer == 0 && CanEnterCell(loc), BlockedByActor.All)) path = Pathfinder.FindPath(search); if (path.Count > 0) diff --git a/OpenRA.Mods.Common/Traits/World/ActorMap.cs b/OpenRA.Mods.Common/Traits/World/ActorMap.cs index 60bc35c08c..616ffa32b8 100644 --- a/OpenRA.Mods.Common/Traits/World/ActorMap.cs +++ b/OpenRA.Mods.Common/Traits/World/ActorMap.cs @@ -211,15 +211,15 @@ namespace OpenRA.Mods.Common.Traits void INotifyCreated.Created(Actor self) { - var cmls = self.TraitsImplementing().ToList(); - if (cmls.Count == 0) + var customMovementLayers = self.TraitsImplementing().ToList(); + if (customMovementLayers.Count == 0) return; - var length = cmls.Max(cml => cml.Index) + 1; + var length = customMovementLayers.Max(cml => cml.Index) + 1; Array.Resize(ref CustomMovementLayers, length); Array.Resize(ref influence, length); - foreach (var cml in cmls) + foreach (var cml in customMovementLayers) { CustomMovementLayers[cml.Index] = cml; influence[cml.Index] = new CellLayer(self.World.Map); diff --git a/OpenRA.Mods.Common/Traits/World/Locomotor.cs b/OpenRA.Mods.Common/Traits/World/Locomotor.cs index a0cc08abc5..14115df411 100644 --- a/OpenRA.Mods.Common/Traits/World/Locomotor.cs +++ b/OpenRA.Mods.Common/Traits/World/Locomotor.cs @@ -379,10 +379,10 @@ namespace OpenRA.Mods.Common.Traits // This section needs to run after WorldLoaded() because we need to be sure that all types of ICustomMovementLayer have been initialized. w.AddFrameEndTask(_ => { - var cmls = world.GetCustomMovementLayers(); - Array.Resize(ref cellsCost, cmls.Length); - Array.Resize(ref blockingCache, cmls.Length); - foreach (var cml in cmls) + var customMovementLayers = world.GetCustomMovementLayers(); + Array.Resize(ref cellsCost, customMovementLayers.Length); + Array.Resize(ref blockingCache, customMovementLayers.Length); + foreach (var cml in customMovementLayers) { if (cml == null) continue; diff --git a/OpenRA.Mods.Common/Traits/World/PathFinder.cs b/OpenRA.Mods.Common/Traits/World/PathFinder.cs index 79876e1eed..54e059614f 100644 --- a/OpenRA.Mods.Common/Traits/World/PathFinder.cs +++ b/OpenRA.Mods.Common/Traits/World/PathFinder.cs @@ -10,7 +10,6 @@ #endregion using System.Collections.Generic; -using System.Linq; using OpenRA.Mods.Common.Pathfinder; using OpenRA.Traits; @@ -29,29 +28,28 @@ namespace OpenRA.Mods.Common.Traits public interface IPathFinder { /// - /// Calculates a path for the actor from source to destination + /// Calculates a path for the actor from source to target. + /// Returned path is *reversed* and given target to source. /// - /// A path from start to target List FindUnitPath(CPos source, CPos target, Actor self, Actor ignoreActor, BlockedByActor check); - List FindUnitPathToRange(CPos source, SubCell srcSub, WPos target, WDist range, Actor self, BlockedByActor check); + /// + /// Expands the path search until a path is found, and returns that path. + /// Returned path is *reversed* and given target to source. + /// + List FindPath(PathSearch search); /// - /// Calculates a path given a search specification + /// Expands both path searches until they intersect, and returns the path. + /// Returned path is from the source of the first search to the source of the second search. /// - List FindPath(IPathSearch search); - - /// - /// Calculates a path given two search specifications, and - /// then returns a path when both search intersect each other - /// TODO: This should eventually disappear - /// - List FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest); + List FindBidiPath(PathSearch fromSrc, PathSearch fromDest); } public class PathFinder : IPathFinder { - static readonly List EmptyPath = new List(0); + public static readonly List NoPath = new List(0); + readonly World world; DomainIndex domainIndex; bool cached; @@ -61,6 +59,10 @@ namespace OpenRA.Mods.Common.Traits this.world = world; } + /// + /// Calculates a path for the actor from source to target. + /// Returned path is *reversed* and given target to source. + /// public List FindUnitPath(CPos source, CPos target, Actor self, Actor ignoreActor, BlockedByActor check) { // PERF: Because we can be sure that OccupiesSpace is Mobile here, we can save some performance by avoiding querying for the trait. @@ -74,153 +76,99 @@ namespace OpenRA.Mods.Common.Traits // If a water-land transition is required, bail early if (domainIndex != null && !domainIndex.IsPassable(source, target, locomotor)) - return EmptyPath; + return NoPath; var distance = source - target; var canMoveFreely = locomotor.CanMoveFreelyInto(self, target, check, null); if (distance.LengthSquared < 3 && !canMoveFreely) - return new List { }; + return NoPath; if (source.Layer == target.Layer && distance.LengthSquared < 3 && canMoveFreely) return new List { target }; List pb; - - using (var fromSrc = PathSearch.FromPoint(world, locomotor, self, target, source, check).WithIgnoredActor(ignoreActor)) - using (var fromDest = PathSearch.FromPoint(world, locomotor, self, source, target, check).WithIgnoredActor(ignoreActor).Reverse()) + using (var fromSrc = PathSearch.ToTargetCell(world, locomotor, self, target, source, check, ignoreActor: ignoreActor)) + using (var fromDest = PathSearch.ToTargetCell(world, locomotor, self, source, target, check, ignoreActor: ignoreActor, inReverse: true)) pb = FindBidiPath(fromSrc, fromDest); return pb; } - public List FindUnitPathToRange(CPos source, SubCell srcSub, WPos target, WDist range, Actor self, BlockedByActor check) + /// + /// Expands the path search until a path is found, and returns that path. + /// Returned path is *reversed* and given target to source. + /// + public List FindPath(PathSearch search) { - if (!cached) - { - domainIndex = world.WorldActor.TraitOrDefault(); - cached = true; - } - - // PERF: Because we can be sure that OccupiesSpace is Mobile here, we can save some performance by avoiding querying for the trait. - var mobile = (Mobile)self.OccupiesSpace; - var locomotor = mobile.Locomotor; - - var targetCell = world.Map.CellContaining(target); - - // Correct for SubCell offset - target -= world.Map.Grid.OffsetOfSubCell(srcSub); - - var rangeLengthSquared = range.LengthSquared; - var map = world.Map; - - // 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 = map.FindTilesInCircle(targetCell, range.Length / 1024 + 1) - .Where(t => (map.CenterOfCell(t) - target).LengthSquared <= rangeLengthSquared - && mobile.Info.CanEnterCell(world, self, 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 - if (domainIndex != null) - { - tilesInRange = new List(tilesInRange.Where(t => domainIndex.IsPassable(source, t, locomotor))); - if (!tilesInRange.Any()) - return EmptyPath; - } - - using (var fromSrc = PathSearch.FromPoints(world, locomotor, self, tilesInRange, source, check)) - using (var fromDest = PathSearch.FromPoint(world, locomotor, self, source, targetCell, check).Reverse()) - return FindBidiPath(fromSrc, fromDest); - } - - public List FindPath(IPathSearch search) - { - List path = null; - while (search.CanExpand) { var p = search.Expand(); if (search.IsTarget(p)) - { - path = MakePath(search.Graph, p); - break; - } + return MakePath(search.Graph, p); } - search.Graph.Dispose(); - - if (path != null) - return path; - - // no path exists - return EmptyPath; + return NoPath; } - // 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 FindBidiPath(IPathSearch fromSrc, IPathSearch fromDest) - { - List path = null; - - while (fromSrc.CanExpand && fromDest.CanExpand) - { - // make some progress on the first search - var p = fromSrc.Expand(); - if (fromDest.Graph[p].Status == CellStatus.Closed && - fromDest.Graph[p].CostSoFar != PathGraph.PathCostForInvalidPath) - { - path = MakeBidiPath(fromSrc, fromDest, p); - break; - } - - // make some progress on the second search - var q = fromDest.Expand(); - if (fromSrc.Graph[q].Status == CellStatus.Closed && - fromSrc.Graph[q].CostSoFar != PathGraph.PathCostForInvalidPath) - { - path = MakeBidiPath(fromSrc, fromDest, q); - break; - } - } - - fromSrc.Graph.Dispose(); - fromDest.Graph.Dispose(); - - if (path != null) - return path; - - return EmptyPath; - } - - // 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 MakePath(IGraph 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 MakePath(IPathGraph graph, CPos destination) { var ret = new List(); var currentNode = destination; - while (cellInfo[currentNode].PreviousNode != currentNode) + while (graph[currentNode].PreviousNode != currentNode) { ret.Add(currentNode); - currentNode = cellInfo[currentNode].PreviousNode; + currentNode = graph[currentNode].PreviousNode; } ret.Add(currentNode); return ret; } - static List MakeBidiPath(IPathSearch a, IPathSearch b, CPos confluenceNode) + /// + /// Expands both path searches until they intersect, and returns the path. + /// Returned path is from the source of the first search to the source of the second search. + /// + public List FindBidiPath(PathSearch first, PathSearch second) { - var ca = a.Graph; - var cb = b.Graph; + while (first.CanExpand && second.CanExpand) + { + // make some progress on the first search + var p = first.Expand(); + var pInfo = second.Graph[p]; + if (pInfo.Status == CellStatus.Closed && + pInfo.CostSoFar != PathGraph.PathCostForInvalidPath) + return MakeBidiPath(first, second, p); + + // make some progress on the second search + var q = second.Expand(); + var qInfo = first.Graph[q]; + if (qInfo.Status == CellStatus.Closed && + qInfo.CostSoFar != PathGraph.PathCostForInvalidPath) + return MakeBidiPath(first, second, q); + } + + return NoPath; + } + + // Build the path from the destination of each search. + // When we find a node that has the same previous position than itself, that is the source of that search. + static List MakeBidiPath(PathSearch first, PathSearch second, CPos confluenceNode) + { + var ca = first.Graph; + var cb = second.Graph; var ret = new List(); var q = confluenceNode; - while (ca[q].PreviousNode != q) + var previous = ca[q].PreviousNode; + while (previous != q) { ret.Add(q); - q = ca[q].PreviousNode; + q = previous; + previous = ca[q].PreviousNode; } ret.Add(q); @@ -228,9 +176,11 @@ namespace OpenRA.Mods.Common.Traits ret.Reverse(); q = confluenceNode; - while (cb[q].PreviousNode != q) + previous = cb[q].PreviousNode; + while (previous != q) { - q = cb[q].PreviousNode; + q = previous; + previous = cb[q].PreviousNode; ret.Add(q); }