diff --git a/CHANGELOG.md b/CHANGELOG.md index 7718cf5..07b1768 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,14 +14,13 @@ Jump to version: ### MLEM Additions - Added TokenizedString.Realign -- Added the ability to include special per-position directions in AStar pathfinding Improvements - Improved EnumHelper.GetValues signature to return an array - Allow using external gesture handling alongside InputHandler through ExternalGestureHandling - Discard old data when updating a StaticSpriteBatch - Multi-target net452, making MLEM compatible with MonoGame for consoles -- Allow retrieving the cost of a calculated path when using AStar +- **Allow retrieving the cost of a calculated path when using AStar** - **Drastically improved StaticSpriteBatch batching performance** Fixes diff --git a/Demos/PathfindingDemo.cs b/Demos/PathfindingDemo.cs index 716d33d..a8c89a7 100644 --- a/Demos/PathfindingDemo.cs +++ b/Demos/PathfindingDemo.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Linq; +using System.Threading.Tasks; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Graphics; using MLEM.Extensions; @@ -51,7 +52,7 @@ namespace Demos { // Now find a path from the top left to the bottom right corner and store it in a variable // If no path can be found after the maximum amount of tries (10000 by default), the pathfinder will abort and return no path (null) - var foundPath = await this.pathfinder.FindPathAsync(Point.Zero, new Point(49, 49)); + var foundPath = await Task.Run(() => this.pathfinder.FindPath(Point.Zero, new Point(49, 49))); this.path = foundPath != null ? foundPath.ToList() : null; // print out some info diff --git a/MLEM/Pathfinding/AStar.cs b/MLEM/Pathfinding/AStar.cs index 30ff7b1..ee22239 100644 --- a/MLEM/Pathfinding/AStar.cs +++ b/MLEM/Pathfinding/AStar.cs @@ -1,7 +1,6 @@ using System; using System.Collections.Generic; using System.Diagnostics; -using System.Threading.Tasks; namespace MLEM.Pathfinding { /// @@ -11,23 +10,6 @@ namespace MLEM.Pathfinding { /// The type of points used for this path public abstract class AStar { - /// - /// A value that represents an infinite path cost, or a cost for a location that cannot possibly be reached. - /// - [Obsolete("This field is deprecated. Use float.PositiveInfinity or float.MaxValue instead.")] - public const float InfiniteCost = float.PositiveInfinity; - - /// - /// The array of all directions that will be checked for path finding. - /// Note that this array is only used if is true. - /// - public readonly T[] AllDirections; - /// - /// The array of all adjacent directions that will be checked for path finding. - /// Note that this array is only used if is false. - /// - public readonly T[] AdjacentDirections; - /// /// The default cost function that determines the cost for each path finding position. /// @@ -41,13 +23,9 @@ namespace MLEM.Pathfinding { /// public int DefaultMaxTries; /// - /// Whether or not diagonal directions are considered while finding a path. + /// The default function. /// - public bool DefaultAllowDiagonals; - /// - /// The default function that determines a set of additional directions (or offsets) that should be tested for walkability, in addition to or . - /// - public GetSpecialDirections DefaultSpecialDirections; + public CollectAdditionalNeighbors DefaultAdditionalNeighbors; /// /// The amount of tries required for finding the last queried path @@ -61,26 +39,15 @@ namespace MLEM.Pathfinding { /// /// Creates a new A* pathfinder with the supplied default settings. /// - /// All directions that should be checked - /// All adjacent directions that should be checked /// The default function for cost determination of a path point - /// Whether or not diagonals should be allowed by default /// The default cost for a path point /// The default amount of tries before path finding is aborted - /// The default function that determines a set of additional directions (or offsets) that should be tested for walkability. - protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) { - this.AllDirections = allDirections; - this.AdjacentDirections = adjacentDirections; + /// The default function. + protected AStar(GetCost defaultCostFunction, float defaultCost, int defaultMaxTries, CollectAdditionalNeighbors defaultAdditionalNeighbors) { this.DefaultCostFunction = defaultCostFunction; this.DefaultCost = defaultCost; this.DefaultMaxTries = defaultMaxTries; - this.DefaultAllowDiagonals = defaultAllowDiagonals; - this.DefaultSpecialDirections = defaultSpecialDirections; - } - - /// - public Task> FindPathAsync(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) { - return Task.Run(() => this.FindPath(start, goal, costFunction, defaultCost, maxTries, allowDiagonals)); + this.DefaultAdditionalNeighbors = defaultAdditionalNeighbors; } /// @@ -91,45 +58,13 @@ namespace MLEM.Pathfinding { /// The function that determines the cost for each path point /// The default cost for each path point /// The maximum amount of tries before path finding is aborted - /// If diagonals should be looked at for path finding - /// An optional function that determines a set of additional directions (or offsets) that should be tested for walkability. + /// A function that determines a set of additional neighbors to be considered for a given point. /// A stack of path points, where the top item is the first point to go to, or null if no path was found. - public Stack FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) { - this.TryFindPath(start, goal, out var path, out _, costFunction, defaultCost, maxTries, allowDiagonals, specialDirections); + public Stack FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, CollectAdditionalNeighbors additionalNeighbors = null) { + this.TryFindPath(start, goal, out var path, out _, costFunction, defaultCost, maxTries, additionalNeighbors); return path; } - /// - public Task> FindPathAsync(T start, IEnumerable goals, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) { - return Task.Run(() => this.FindPath(start, goals, costFunction, defaultCost, maxTries, allowDiagonals)); - } - - /// - /// Tries to find paths between a position and a set of and returns the path that had the lowest overall cost. - /// Note that this method is only faster than a one-to-many pathfinding method like Dijkstra's algorithm in situations where the amount of possible is much lower than the total amount of possible positions, or the are relatively close to each other. - /// - /// The point to start path finding at - /// The set of points to try to find a path to - /// The function that determines the cost for each path point - /// The default cost for each path point - /// The maximum amount of tries before path finding is aborted - /// If diagonals should be looked at for path finding - /// An optional function that determines a set of additional directions (or offsets) that should be tested for walkability. - /// A stack of path points, where the top item is the first point to go to, or null if no paths were found. - public Stack FindPath(T start, IEnumerable goals, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) { - var lowestCost = float.PositiveInfinity; - Stack cheapestPath = null; - foreach (var goal in goals) { - if (!this.TryFindPath(start, goal, out var path, out var cost, costFunction, defaultCost, maxTries, allowDiagonals, specialDirections)) - continue; - if (cost < lowestCost) { - lowestCost = cost; - cheapestPath = path; - } - } - return cheapestPath; - } - /// /// Tries to find a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings. /// @@ -140,23 +75,22 @@ namespace MLEM.Pathfinding { /// The function that determines the cost for each path point /// The default cost for each path point /// The maximum amount of tries before path finding is aborted - /// If diagonals should be looked at for path finding - /// An optional function that determines a set of additional directions (or offsets) that should be tested for walkability. + /// A function that determines a set of additional neighbors to be considered for a given point. /// Whether a path was found. - public bool TryFindPath(T start, T goal, out Stack path, out float totalCost, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) { + public bool TryFindPath(T start, T goal, out Stack path, out float totalCost, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, CollectAdditionalNeighbors additionalNeighbors = null) { path = null; totalCost = float.PositiveInfinity; var stopwatch = Stopwatch.StartNew(); var getCost = costFunction ?? this.DefaultCostFunction; - var diags = allowDiagonals ?? this.DefaultAllowDiagonals; var tries = maxTries ?? this.DefaultMaxTries; var defCost = defaultCost ?? this.DefaultCost; - var special = specialDirections ?? this.DefaultSpecialDirections; + var additional = additionalNeighbors ?? this.DefaultAdditionalNeighbors; + var neighbors = new HashSet(); var open = new Dictionary>(); var closed = new Dictionary>(); - open.Add(start, new PathPoint(start, this.GetManhattanDistance(start, goal), null, 0, defCost)); + open.Add(start, new PathPoint(start, this.GetHeuristicDistance(start, goal), null, 0, defCost)); var count = 0; while (open.Count > 0) { @@ -171,17 +105,32 @@ namespace MLEM.Pathfinding { open.Remove(current.Pos); closed.Add(current.Pos, current); - if (current.Pos.Equals(goal)) { + if (EqualityComparer.Default.Equals(current.Pos, goal)) { path = AStar.CompilePath(current); totalCost = current.F; break; } - foreach (var dir in diags ? this.AllDirections : this.AdjacentDirections) - ExamineDirection(current, dir); - if (special != null) { - foreach (var dir in special(current.Pos)) - ExamineDirection(current, dir); + neighbors.Clear(); + this.CollectNeighbors(current.Pos, neighbors); + additional?.Invoke(current.Pos, neighbors); + + foreach (var neighborPos in neighbors) { + var cost = getCost(current.Pos, neighborPos); + if (!float.IsPositiveInfinity(cost) && cost < float.MaxValue && !closed.ContainsKey(neighborPos)) { + var neighbor = new PathPoint(neighborPos, this.GetHeuristicDistance(neighborPos, goal), current, cost, defCost); + // check if we already have a waypoint at this location with a worse path + if (open.TryGetValue(neighborPos, out var alreadyNeighbor)) { + if (neighbor.G < alreadyNeighbor.G) { + open.Remove(neighborPos); + } else { + // if the old waypoint is better, we don't add ours + continue; + } + } + // add the new neighbor as a possible waypoint + open.Add(neighborPos, neighbor); + } } count++; @@ -193,36 +142,23 @@ namespace MLEM.Pathfinding { this.LastTriesNeeded = count; this.LastTimeNeeded = stopwatch.Elapsed; return path != null; - - void ExamineDirection(PathPoint current, T dir) { - var neighborPos = this.AddPositions(current.Pos, dir); - var cost = getCost(current.Pos, neighborPos); - if (!float.IsPositiveInfinity(cost) && cost < float.MaxValue && !closed.ContainsKey(neighborPos)) { - var neighbor = new PathPoint(neighborPos, this.GetManhattanDistance(neighborPos, goal), current, cost, defCost); - // check if we already have a waypoint at this location with a worse path - if (open.TryGetValue(neighborPos, out var alreadyNeighbor)) { - if (neighbor.G < alreadyNeighbor.G) { - open.Remove(neighborPos); - } else { - // if the old waypoint is better, we don't add ours - return; - } - } - // add the new neighbor as a possible waypoint - open.Add(neighborPos, neighbor); - } - } } /// - /// A helper method to add two positions together. + /// This method should implement a heuristic that determines the total distance between the given position and the given second position . + /// Note that this is multiplied with the automatically, so no costs need to be considered in this method's return value. /// - protected abstract T AddPositions(T first, T second); + /// The start position. + /// The position to get the distance to. + /// The total distance between the two positions. + protected abstract float GetHeuristicDistance(T start, T position); /// - /// A helper method to get the Manhattan Distance between two points. + /// This method should populate a set of positions that are considered to the given . For example, this method might return directly adjacent positions, diagonal positions, or faraway positions that can be teleported to. /// - protected abstract float GetManhattanDistance(T first, T second); + /// The position whose neighbors to return. + /// The set to populate with neighbors. + protected abstract void CollectNeighbors(T position, ISet neighbors); private static Stack CompilePath(PathPoint current) { var path = new Stack(); @@ -234,20 +170,20 @@ namespace MLEM.Pathfinding { } /// - /// A cost function for a given path finding position. + /// A cost function for a given pair of neighboring path finding positions. /// If a path point should have the default cost, should be returned. /// If a path point should be unreachable, or should be returned. /// - /// The current position in the path - /// The position we're trying to reach from the current position + /// The current position in the path. + /// The neighboring position whose cost to check. public delegate float GetCost(T currPos, T nextPos); /// - /// A delegate used by and that determines a set of additional directions (or offsets) that should be tested for walkability. + /// A delegate that determines a set of additional to be considered for a given . /// - /// The current position in the path. - /// A set of additional directions (or offsets) that should be checked for walkability. If the given has no special directions, an empty should be returned. - public delegate IEnumerable GetSpecialDirections(T currPos); + /// The position whose neighbors to return. + /// The set to populate with neighbors. + public delegate void CollectAdditionalNeighbors(T position, ISet neighbors); } @@ -278,7 +214,7 @@ namespace MLEM.Pathfinding { /// Creates a new path point with the supplied settings. /// /// The point's position - /// The point's manhattan distance from the start point + /// The point's distance from the start point /// The point's parent /// The point's terrain cost, based on /// The default cost for a path point @@ -307,7 +243,7 @@ namespace MLEM.Pathfinding { /// Returns the hash code for this instance. /// A 32-bit signed integer that is the hash code for this instance. public override int GetHashCode() { - return this.Pos.GetHashCode(); + return EqualityComparer.Default.GetHashCode(this.Pos); } } diff --git a/MLEM/Pathfinding/AStar2.cs b/MLEM/Pathfinding/AStar2.cs index 89e45e8..7b92508 100644 --- a/MLEM/Pathfinding/AStar2.cs +++ b/MLEM/Pathfinding/AStar2.cs @@ -1,29 +1,36 @@ using System; -using System.Linq; +using System.Collections.Generic; using Microsoft.Xna.Framework; using MLEM.Misc; namespace MLEM.Pathfinding { /// - /// A 2-dimensional implementation of that uses for positions. + /// A 2-dimensional implementation of that uses for positions, and the manhattan distance as its heuristic. /// public class AStar2 : AStar { - private static readonly Point[] AdjacentDirs = Direction2Helper.Adjacent.Offsets().ToArray(); - private static readonly Point[] AllDirs = Direction2Helper.All.Offsets().ToArray(); + private readonly bool includeDiagonals; /// - public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) : - base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries, defaultSpecialDirections) {} - - /// - protected override Point AddPositions(Point first, Point second) { - return first + second; + public AStar2(GetCost defaultCostFunction, bool includeDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, CollectAdditionalNeighbors defaultAdditionalNeighbors = null) : + base(defaultCostFunction, defaultCost, defaultMaxTries, defaultAdditionalNeighbors) { + this.includeDiagonals = includeDiagonals; } /// - protected override float GetManhattanDistance(Point first, Point second) { - return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y); + protected override float GetHeuristicDistance(Point start, Point position) { + return Math.Abs(position.X - start.X) + Math.Abs(position.Y - start.Y); + } + + /// + protected override void CollectNeighbors(Point position, ISet neighbors) { + foreach (var dir in Direction2Helper.Adjacent) + neighbors.Add(position + dir.Offset()); + + if (this.includeDiagonals) { + foreach (var dir in Direction2Helper.Diagonals) + neighbors.Add(position + dir.Offset()); + } } } diff --git a/MLEM/Pathfinding/AStar3.cs b/MLEM/Pathfinding/AStar3.cs index 71f6108..7a489ef 100644 --- a/MLEM/Pathfinding/AStar3.cs +++ b/MLEM/Pathfinding/AStar3.cs @@ -4,47 +4,43 @@ using Microsoft.Xna.Framework; namespace MLEM.Pathfinding { /// - /// A 3-dimensional implementation of that uses for positions. + /// A 3-dimensional implementation of that uses for positions, and the manhattan distance as its heuristic. /// public class AStar3 : AStar { - private static readonly Vector3[] AdjacentDirs = { - new Vector3(1, 0, 0), - new Vector3(-1, 0, 0), - new Vector3(0, 1, 0), - new Vector3(0, -1, 0), - new Vector3(0, 0, 1), - new Vector3(0, 0, -1) - }; + private readonly bool includeDiagonals; - private static readonly Vector3[] AllDirs; + /// + public AStar3(GetCost defaultCostFunction, bool includeDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, CollectAdditionalNeighbors defaultAdditionalNeighbors = null) : + base(defaultCostFunction, defaultCost, defaultMaxTries, defaultAdditionalNeighbors) { + this.includeDiagonals = includeDiagonals; + } - static AStar3() { - var dirs = new List(); - for (var x = -1; x <= 1; x++) { - for (var y = -1; y <= 1; y++) { - for (var z = -1; z <= 1; z++) { - if (x == 0 && y == 0 && z == 0) - continue; - dirs.Add(new Vector3(x, y, z)); + /// + protected override float GetHeuristicDistance(Vector3 start, Vector3 position) { + return Math.Abs(position.X - start.X) + Math.Abs(position.Y - start.Y) + Math.Abs(position.Z - start.Z); + } + + /// + protected override void CollectNeighbors(Vector3 position, ISet neighbors) { + if (this.includeDiagonals) { + for (var x = -1; x <= 1; x++) { + for (var y = -1; y <= 1; y++) { + for (var z = -1; z <= 1; z++) { + if (x == 0 && y == 0 && z == 0) + continue; + neighbors.Add(position + new Vector3(x, y, z)); + } } } + } else { + neighbors.Add(position + new Vector3(1, 0, 0)); + neighbors.Add(position + new Vector3(-1, 0, 0)); + neighbors.Add(position + new Vector3(0, 1, 0)); + neighbors.Add(position + new Vector3(0, -1, 0)); + neighbors.Add(position + new Vector3(0, 0, 1)); + neighbors.Add(position + new Vector3(0, 0, -1)); } - AStar3.AllDirs = dirs.ToArray(); - } - - /// - public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) : - base(AStar3.AllDirs, AStar3.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries, defaultSpecialDirections) {} - - /// - protected override Vector3 AddPositions(Vector3 first, Vector3 second) { - return first + second; - } - - /// - protected override float GetManhattanDistance(Vector3 first, Vector3 second) { - return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y) + Math.Abs(second.Z - first.Z); } } diff --git a/Tests/PathfindingTests.cs b/Tests/PathfindingTests.cs index ae0f0fc..9fbc161 100644 --- a/Tests/PathfindingTests.cs +++ b/Tests/PathfindingTests.cs @@ -76,12 +76,14 @@ namespace Tests { Assert.IsNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, true)); // but if we define a link across the wall, it should work - Assert.IsNotNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, false, - p => p.X == 2 && p.Y == 2 ? new[] {new Point(-1, 2)} : Enumerable.Empty())); + Assert.IsNotNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, false, (p, n) => { + if (p.X == 2 && p.Y == 2) + n.Add(new Point(1, 4)); + })); } [Test] - public void TestMultiplePaths() { + public void TestCosts() { var area = new[] { "XXXXXXXX", "X 2 X", @@ -99,23 +101,19 @@ namespace Tests { pathfinder.TryFindPath(new Point(1, 1), goals[i], out _, out var cost); Assert.AreEqual(goalCosts[i], cost); } - - var path = pathfinder.FindPath(new Point(1, 1), goals).ToArray(); - var expected = new[] {new Point(1, 1), new Point(2, 1), new Point(3, 1), new Point(4, 1), new Point(5, 1), new Point(5, 2), new Point(5, 3), new Point(5, 4), new Point(5, 5)}; - Assert.AreEqual(path, expected); } - private static Stack FindPathInArea(Point start, Point end, IEnumerable area, bool allowDiagonals, AStar2.GetSpecialDirections getSpecialDirections = null) { - return PathfindingTests.CreatePathfinder(area, allowDiagonals, getSpecialDirections).FindPath(start, end); + private static Stack FindPathInArea(Point start, Point end, IEnumerable area, bool allowDiagonals, AStar2.CollectAdditionalNeighbors collectAdditionalNeighbors = null) { + return PathfindingTests.CreatePathfinder(area, allowDiagonals, collectAdditionalNeighbors).FindPath(start, end); } - private static AStar2 CreatePathfinder(IEnumerable area, bool allowDiagonals, AStar2.GetSpecialDirections getSpecialDirections = null) { + private static AStar2 CreatePathfinder(IEnumerable area, bool allowDiagonals, AStar2.CollectAdditionalNeighbors collectAdditionalNeighbors = null) { var costs = area.Select(s => s.Select(c => c switch { ' ' => 1, 'X' => float.PositiveInfinity, _ => (float) char.GetNumericValue(c) }).ToArray()).ToArray(); - return new AStar2((_, p2) => costs[p2.Y][p2.X], allowDiagonals, 1, 64, getSpecialDirections); + return new AStar2((_, p2) => costs[p2.Y][p2.X], allowDiagonals, 1, 64, collectAdditionalNeighbors); } }