From ac55c5bf0966a369ef52ecb0233bceda160b6844 Mon Sep 17 00:00:00 2001 From: RoosterDragon Date: Thu, 20 Aug 2015 23:36:44 +0100 Subject: [PATCH] Fix pathfinding using PriorityQueue incorrectly. By providing a comparer that could change over time (as estimated costs on the graph were updated), this meant the priority queue could have its heap property invalidated and thus not maintain a correct ordering. Instead we store elements into the queue with their estimations at the time. This preserves the heap property and thus ensures the queue returns properly ordered results, although it may contain out of date estimations. This also improves performance. The fixed comparer need not perform expensive lookups into the graph, but can instead use the readily available value. This speeds up adds and removes on the queue significantly. --- .../Pathfinder/BasePathSearch.cs | 8 ++++---- OpenRA.Mods.Common/Pathfinder/CellInfo.cs | 18 ------------------ OpenRA.Mods.Common/Pathfinder/PathGraph.cs | 12 ++++++++++++ OpenRA.Mods.Common/Pathfinder/PathSearch.cs | 15 +++++++++------ 4 files changed, 25 insertions(+), 28 deletions(-) diff --git a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs b/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs index 8fba8f3e09..fab51285a3 100644 --- a/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs +++ b/OpenRA.Mods.Common/Pathfinder/BasePathSearch.cs @@ -65,7 +65,7 @@ namespace OpenRA.Mods.Common.Pathfinder { public IGraph Graph { get; set; } - protected IPriorityQueue OpenQueue { get; private set; } + protected IPriorityQueue OpenQueue { get; private set; } public abstract IEnumerable> Considered { get; } @@ -80,13 +80,13 @@ namespace OpenRA.Mods.Common.Pathfinder // 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; + protected readonly IPriorityQueue StartPoints; protected BasePathSearch(IGraph graph) { Graph = graph; - OpenQueue = new PriorityQueue(new PositionComparer(Graph)); - startPoints = new PriorityQueue(new PositionComparer(Graph)); + OpenQueue = new PriorityQueue(GraphConnection.ConnectionCostComparer); + StartPoints = new PriorityQueue(GraphConnection.ConnectionCostComparer); Debug = false; MaxCost = 0; } diff --git a/OpenRA.Mods.Common/Pathfinder/CellInfo.cs b/OpenRA.Mods.Common/Pathfinder/CellInfo.cs index 57f3e9329e..a8112bc343 100644 --- a/OpenRA.Mods.Common/Pathfinder/CellInfo.cs +++ b/OpenRA.Mods.Common/Pathfinder/CellInfo.cs @@ -57,22 +57,4 @@ namespace OpenRA.Mods.Common.Pathfinder EstimatedTotal = estimatedTotal; } } - - /// - /// Compares two nodes according to their estimations - /// - public class PositionComparer : IComparer - { - readonly IGraph graph; - - public PositionComparer(IGraph graph) - { - this.graph = graph; - } - - public int Compare(CPos x, CPos y) - { - return Math.Sign(graph[x].EstimatedTotal - graph[y].EstimatedTotal); - } - } } diff --git a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs b/OpenRA.Mods.Common/Pathfinder/PathGraph.cs index 3204b5282e..268e1331c2 100644 --- a/OpenRA.Mods.Common/Pathfinder/PathGraph.cs +++ b/OpenRA.Mods.Common/Pathfinder/PathGraph.cs @@ -47,6 +47,18 @@ namespace OpenRA.Mods.Common.Pathfinder public 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; diff --git a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs b/OpenRA.Mods.Common/Pathfinder/PathSearch.cs index 254317d030..c4f5355f7d 100644 --- a/OpenRA.Mods.Common/Pathfinder/PathSearch.cs +++ b/OpenRA.Mods.Common/Pathfinder/PathSearch.cs @@ -84,9 +84,11 @@ namespace OpenRA.Mods.Common.Pathfinder protected override void AddInitialCell(CPos location) { - Graph[location] = new CellInfo(0, heuristic(location), location, CellStatus.Open); - OpenQueue.Add(location); - startPoints.Add(location); + var cost = heuristic(location); + Graph[location] = new CellInfo(0, cost, location, CellStatus.Open); + var connection = new GraphConnection(location, cost); + OpenQueue.Add(connection); + StartPoints.Add(connection); considered.AddLast(new Pair(location, 0)); } @@ -99,7 +101,7 @@ namespace OpenRA.Mods.Common.Pathfinder /// The most promising node of the iteration public override CPos Expand() { - var currentMinNode = OpenQueue.Pop(); + var currentMinNode = OpenQueue.Pop().Destination; var currentCell = Graph[currentMinNode]; Graph[currentMinNode] = new CellInfo(currentCell.CostSoFar, currentCell.EstimatedTotal, currentCell.PreviousPos, CellStatus.Closed); @@ -128,10 +130,11 @@ namespace OpenRA.Mods.Common.Pathfinder else hCost = heuristic(neighborCPos); - Graph[neighborCPos] = new CellInfo(gCost, gCost + hCost, currentMinNode, CellStatus.Open); + var estimatedCost = gCost + hCost; + Graph[neighborCPos] = new CellInfo(gCost, estimatedCost, currentMinNode, CellStatus.Open); if (neighborCell.Status != CellStatus.Open) - OpenQueue.Add(neighborCPos); + OpenQueue.Add(new GraphConnection(neighborCPos, estimatedCost)); if (Debug) {