Cleaned up AStar code

CHANGELOG.md

@@ -19,8 +19,9 @@ 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
-- **Drastically improved StaticSpriteBatch batching performance**
 - Multi-target net452, making MLEM compatible with MonoGame for consoles
+- **Allow retrieving the cost of a calculated path when using AStar**
+- **Drastically improved StaticSpriteBatch batching performance**
 
 Fixes
 - Fixed TokenizedString handling trailing spaces incorrectly in the last line of non-left aligned text

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

MLEM/Pathfinding/AStar.cs

@@ -1,7 +1,6 @@
 using System;
 using System.Collections.Generic;
 using System.Diagnostics;
-using System.Threading.Tasks;
 
 namespace MLEM.Pathfinding {
     /// <summary>
@@ -11,21 +10,6 @@ namespace MLEM.Pathfinding {
     /// <typeparam name="T">The type of points used for this path</typeparam>
     public abstract class AStar<T> {
 
-        /// <summary>
-        /// A value that represents an infinite path cost, or a cost for a location that cannot possibly be reached.
-        /// </summary>
-        [Obsolete("This field is deprecated. Use float.PositiveInfinity or float.MaxValue instead.")]
-        public const float InfiniteCost = float.PositiveInfinity;
-        /// <summary>
-        /// The array of all directions that will be checked for path finding.
-        /// Note that this array is only used if <see cref="DefaultAllowDiagonals"/> is true.
-        /// </summary>
-        public readonly T[] AllDirections;
-        /// <summary>
-        /// The array of all adjacent directions that will be checked for path finding.
-        /// Note that this array is only used if <see cref="DefaultAllowDiagonals"/> is false.
-        /// </summary>
-        public readonly T[] AdjacentDirections;
         /// <summary>
         /// The default cost function that determines the cost for each path finding position.
         /// </summary>
@@ -39,9 +23,10 @@ namespace MLEM.Pathfinding {
         /// </summary>
         public int DefaultMaxTries;
         /// <summary>
-        /// Whether or not diagonal directions are considered while finding a path.
+        /// The default <see cref="CollectAdditionalNeighbors"/> function.
         /// </summary>
-        public bool DefaultAllowDiagonals;
+        public CollectAdditionalNeighbors DefaultAdditionalNeighbors;
+
         /// <summary>
         /// The amount of tries required for finding the last queried path
         /// </summary>
@@ -54,24 +39,15 @@ namespace MLEM.Pathfinding {
         /// <summary>
         /// Creates a new A* pathfinder with the supplied default settings.
         /// </summary>
-        /// <param name="allDirections">All directions that should be checked</param>
-        /// <param name="adjacentDirections">All adjacent directions that should be checked</param>
         /// <param name="defaultCostFunction">The default function for cost determination of a path point</param>
-        /// <param name="defaultAllowDiagonals">Whether or not diagonals should be allowed by default</param>
         /// <param name="defaultCost">The default cost for a path point</param>
         /// <param name="defaultMaxTries">The default amount of tries before path finding is aborted</param>
-        protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) {
-            this.AllDirections = allDirections;
-            this.AdjacentDirections = adjacentDirections;
+        /// <param name="defaultAdditionalNeighbors">The default <see cref="CollectAdditionalNeighbors"/> function.</param>
+        protected AStar(GetCost defaultCostFunction, float defaultCost, int defaultMaxTries, CollectAdditionalNeighbors defaultAdditionalNeighbors) {
             this.DefaultCostFunction = defaultCostFunction;
             this.DefaultCost = defaultCost;
             this.DefaultMaxTries = defaultMaxTries;
-            this.DefaultAllowDiagonals = defaultAllowDiagonals;
-        }
-
-        /// <inheritdoc cref="FindPath"/>
-        public Task<Stack<T>> 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;
         }
 
         /// <summary>
@@ -82,22 +58,41 @@ namespace MLEM.Pathfinding {
         /// <param name="costFunction">The function that determines the cost for each path point</param>
         /// <param name="defaultCost">The default cost for each path point</param>
         /// <param name="maxTries">The maximum amount of tries before path finding is aborted</param>
-        /// <param name="allowDiagonals">If diagonals should be looked at for path finding</param>
+        /// <param name="additionalNeighbors">A function that determines a set of additional neighbors to be considered for a given point.</param>
         /// <returns>A stack of path points, where the top item is the first point to go to, or null if no path was found.</returns>
-        public Stack<T> FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) {
-            var stopwatch = Stopwatch.StartNew();
+        public Stack<T> 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;
+        }
 
+        /// <summary>
+        /// Tries to find a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings.
+        /// </summary>
+        /// <param name="start">The point to start path finding at</param>
+        /// <param name="goal">The point to find a path to</param>
+        /// <param name="path">The path that was found, or <see langword="null"/> if no path was found.</param>
+        /// <param name="totalCost">The total cost that was calculated for the path, or <see cref="float.PositiveInfinity"/> if no path was found.</param>
+        /// <param name="costFunction">The function that determines the cost for each path point</param>
+        /// <param name="defaultCost">The default cost for each path point</param>
+        /// <param name="maxTries">The maximum amount of tries before path finding is aborted</param>
+        /// <param name="additionalNeighbors">A function that determines a set of additional neighbors to be considered for a given point.</param>
+        /// <returns>Whether a path was found.</returns>
+        public bool TryFindPath(T start, T goal, out Stack<T> 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 additional = additionalNeighbors ?? this.DefaultAdditionalNeighbors;
 
+            var neighbors = new HashSet<T>();
             var open = new Dictionary<T, PathPoint<T>>();
             var closed = new Dictionary<T, PathPoint<T>>();
-            open.Add(start, new PathPoint<T>(start, this.GetManhattanDistance(start, goal), null, 0, defCost));
+            open.Add(start, new PathPoint<T>(start, this.GetHeuristicDistance(start, goal), null, 0, defCost));
 
             var count = 0;
-            Stack<T> ret = null;
             while (open.Count > 0) {
                 PathPoint<T> current = null;
                 foreach (var point in open.Values) {
@@ -110,17 +105,20 @@ namespace MLEM.Pathfinding {
                 open.Remove(current.Pos);
                 closed.Add(current.Pos, current);
 
-                if (current.Pos.Equals(goal)) {
-                    ret = AStar<T>.CompilePath(current);
+                if (EqualityComparer<T>.Default.Equals(current.Pos, goal)) {
+                    path = AStar<T>.CompilePath(current);
+                    totalCost = current.F;
                     break;
                 }
 
-                var dirsUsed = diags ? this.AllDirections : this.AdjacentDirections;
-                foreach (var dir in dirsUsed) {
-                    var neighborPos = this.AddPositions(current.Pos, 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<T>(neighborPos, this.GetManhattanDistance(neighborPos, goal), current, cost, defCost);
+                        var neighbor = new PathPoint<T>(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) {
@@ -143,18 +141,24 @@ namespace MLEM.Pathfinding {
             stopwatch.Stop();
             this.LastTriesNeeded = count;
             this.LastTimeNeeded = stopwatch.Elapsed;
-            return ret;
+            return path != null;
         }
 
         /// <summary>
-        /// A helper method to add two positions together.
+        /// This method should implement a heuristic that determines the total distance between the given <paramref name="start"/> position and the given second position <paramref name="position"/>.
+        /// Note that this is multiplied with the <see cref="DefaultCost"/> automatically, so no costs need to be considered in this method's return value.
         /// </summary>
-        protected abstract T AddPositions(T first, T second);
+        /// <param name="start">The start position.</param>
+        /// <param name="position">The position to get the distance to.</param>
+        /// <returns>The total distance between the two positions.</returns>
+        protected abstract float GetHeuristicDistance(T start, T position);
 
         /// <summary>
-        /// A helper method to get the Manhattan Distance between two points.
+        /// This method should populate a set of positions that are considered <paramref name="neighbors"/> to the given <paramref name="position"/>. For example, this method might return directly adjacent positions, diagonal positions, or faraway positions that can be teleported to.
         /// </summary>
-        protected abstract float GetManhattanDistance(T first, T second);
+        /// <param name="position">The position whose neighbors to return.</param>
+        /// <param name="neighbors">The set to populate with neighbors.</param>
+        protected abstract void CollectNeighbors(T position, ISet<T> neighbors);
 
         private static Stack<T> CompilePath(PathPoint<T> current) {
             var path = new Stack<T>();
@@ -166,14 +170,21 @@ namespace MLEM.Pathfinding {
         }
 
         /// <summary>
-        /// 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, <see cref="AStar{T}.DefaultCost"/> should be returned.
         /// If a path point should be unreachable, <see cref="float.PositiveInfinity"/> or <see cref="float.MaxValue"/> should be returned.
         /// </summary>
-        /// <param name="currPos">The current position in the path</param>
-        /// <param name="nextPos">The position we're trying to reach from the current position</param>
+        /// <param name="currPos">The current position in the path.</param>
+        /// <param name="nextPos">The neighboring position whose cost to check.</param>
         public delegate float GetCost(T currPos, T nextPos);
 
+        /// <summary>
+        /// A delegate that determines a set of additional <paramref name="neighbors"/> to be considered for a given <paramref name="position"/>.
+        /// </summary>
+        /// <param name="position">The position whose neighbors to return.</param>
+        /// <param name="neighbors">The set to populate with neighbors.</param>
+        public delegate void CollectAdditionalNeighbors(T position, ISet<T> neighbors);
+
     }
 
     /// <summary>
@@ -203,7 +214,7 @@ namespace MLEM.Pathfinding {
         /// Creates a new path point with the supplied settings.
         /// </summary>
         /// <param name="pos">The point's position</param>
-        /// <param name="distance">The point's manhattan distance from the start point</param>
+        /// <param name="distance">The point's distance from the start point</param>
         /// <param name="parent">The point's parent</param>
         /// <param name="terrainCostForThisPos">The point's terrain cost, based on <see cref="AStar{T}.GetCost"/></param>
         /// <param name="defaultCost">The default cost for a path point</param>
@@ -232,7 +243,7 @@ namespace MLEM.Pathfinding {
         /// <summary>Returns the hash code for this instance.</summary>
         /// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
         public override int GetHashCode() {
-            return this.Pos.GetHashCode();
+            return EqualityComparer<T>.Default.GetHashCode(this.Pos);
         }
 
     }

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 {
     /// <summary>
-    /// A 2-dimensional implementation of <see cref="AStar{T}"/> that uses <see cref="Point"/> for positions.
+    /// A 2-dimensional implementation of <see cref="AStar{T}"/> that uses <see cref="Point"/> for positions, and the manhattan distance as its heuristic.
     /// </summary>
     public class AStar2 : AStar<Point> {
 
-        private static readonly Point[] AdjacentDirs = Direction2Helper.Adjacent.Offsets().ToArray();
-        private static readonly Point[] AllDirs = Direction2Helper.All.Offsets().ToArray();
+        private readonly bool includeDiagonals;
 
         /// <inheritdoc />
-        public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
-            base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {}
-
-        /// <inheritdoc />
-        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;
         }
 
         /// <inheritdoc />
-        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);
+        }
+
+        /// <inheritdoc />
+        protected override void CollectNeighbors(Point position, ISet<Point> 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());
+            }
         }
 
     }

MLEM/Pathfinding/AStar3.cs

@@ -4,47 +4,43 @@ using Microsoft.Xna.Framework;
 
 namespace MLEM.Pathfinding {
     /// <summary>
-    /// A 3-dimensional implementation of <see cref="AStar{T}"/> that uses <see cref="Vector3"/> for positions.
+    /// A 3-dimensional implementation of <see cref="AStar{T}"/> that uses <see cref="Vector3"/> for positions, and the manhattan distance as its heuristic.
     /// </summary>
     public class AStar3 : AStar<Vector3> {
 
-        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;
+        /// <inheritdoc />
+        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<Vector3>();
-            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));
+        /// <inheritdoc />
+        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);
+        }
+
+        /// <inheritdoc />
+        protected override void CollectNeighbors(Vector3 position, ISet<Vector3> 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();
-        }
-
-        /// <inheritdoc />
-        public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
-            base(AStar3.AllDirs, AStar3.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {}
-
-        /// <inheritdoc />
-        protected override Vector3 AddPositions(Vector3 first, Vector3 second) {
-            return first + second;
-        }
-
-        /// <inheritdoc />
-        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);
         }
 
     }

Tests/PathfindingTests.cs

@@ -60,14 +60,60 @@ namespace Tests {
             Assert.IsNotNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 3), area, true));
         }
 
-        private static Stack<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals) {
+        [Test]
+        public void TestSpecialDirections() {
+            var area = new[] {
+                "XXXX",
+                "X XX",
+                "X  X",
+                "XXXX",
+                "X  X",
+                "XXXX"
+            };
+
+            // both types of traditional pathfinding should get stuck
+            Assert.IsNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, false));
+            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, n) => {
+                if (p.X == 2 && p.Y == 2)
+                    n.Add(new Point(1, 4));
+            }));
+        }
+
+        [Test]
+        public void TestCosts() {
+            var area = new[] {
+                "XXXXXXXX",
+                "X  2  X",
+                "XXXXX X",
+                "X 53  X",
+                "X XXX X",
+                "X X X X",
+                "XXXXXXXX"
+            };
+            var pathfinder = PathfindingTests.CreatePathfinder(area, false);
+
+            var goals = new[] {new Point(1, 5), new Point(3, 5), new Point(5, 5)};
+            var goalCosts = new[] {19, float.PositiveInfinity, 9};
+            for (var i = 0; i < goals.Length; i++) {
+                pathfinder.TryFindPath(new Point(1, 1), goals[i], out _, out var cost);
+                Assert.AreEqual(goalCosts[i], cost);
+            }
+        }
+
+        private static Stack<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals, AStar2.CollectAdditionalNeighbors collectAdditionalNeighbors = null) {
+            return PathfindingTests.CreatePathfinder(area, allowDiagonals, collectAdditionalNeighbors).FindPath(start, end);
+        }
+
+        private static AStar2 CreatePathfinder(IEnumerable<string> 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();
-            var pathFinder = new AStar2((_, p2) => costs[p2.Y][p2.X], allowDiagonals);
-            return pathFinder.FindPath(start, end);
+            return new AStar2((_, p2) => costs[p2.Y][p2.X], allowDiagonals, 1, 64, collectAdditionalNeighbors);
         }
 
     }