Added the ability to include special per-position directions in AStar pathfinding

CHANGELOG.md

@@ -14,6 +14,7 @@ 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

MLEM/Pathfinding/AStar.cs

@@ -16,6 +16,7 @@ namespace MLEM.Pathfinding {
         /// </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.
@@ -26,6 +27,7 @@ namespace MLEM.Pathfinding {
         /// 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>
@@ -42,6 +44,11 @@ namespace MLEM.Pathfinding {
         /// Whether or not diagonal directions are considered while finding a path.
         /// </summary>
         public bool DefaultAllowDiagonals;
+        /// <summary>
+        /// The default function that determines a set of additional directions (or offsets) that should be tested for walkability, in addition to <see cref="AllDirections"/> or <see cref="AdjacentDirections"/>.
+        /// </summary>
+        public GetSpecialDirections DefaultSpecialDirections;
+
         /// <summary>
         /// The amount of tries required for finding the last queried path
         /// </summary>
@@ -60,13 +67,15 @@ namespace MLEM.Pathfinding {
         /// <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) {
+        /// <param name="defaultSpecialDirections">The default function that determines a set of additional directions (or offsets) that should be tested for walkability.</param>
+        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;
             this.DefaultCostFunction = defaultCostFunction;
             this.DefaultCost = defaultCost;
             this.DefaultMaxTries = defaultMaxTries;
             this.DefaultAllowDiagonals = defaultAllowDiagonals;
+            this.DefaultSpecialDirections = defaultSpecialDirections;
         }
 
         /// <inheritdoc cref="FindPath"/>
@@ -83,14 +92,16 @@ namespace MLEM.Pathfinding {
         /// <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="specialDirections">An optional function that determines a set of additional directions (or offsets) that should be tested for walkability.</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) {
+        public Stack<T> FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) {
             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 open = new Dictionary<T, PathPoint<T>>();
             var closed = new Dictionary<T, PathPoint<T>>();
@@ -115,24 +126,11 @@ namespace MLEM.Pathfinding {
                     break;
                 }
 
-                var dirsUsed = diags ? this.AllDirections : this.AdjacentDirections;
+                foreach (var dir in diags ? this.AllDirections : this.AdjacentDirections)
-                foreach (var dir in dirsUsed) {
+                    ExamineDirection(current, dir);
-                    var neighborPos = this.AddPositions(current.Pos, dir);
+                if (special != null) {
-                    var cost = getCost(current.Pos, neighborPos);
+                    foreach (var dir in special(current.Pos))
-                    if (!float.IsPositiveInfinity(cost) && cost < float.MaxValue && !closed.ContainsKey(neighborPos)) {
+                        ExamineDirection(current, dir);
-                        var neighbor = new PathPoint<T>(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
-                                continue;
-                            }
-                        }
-                        // add the new neighbor as a possible waypoint
-                        open.Add(neighborPos, neighbor);
-                    }
                 }
 
                 count++;
@@ -144,6 +142,25 @@ namespace MLEM.Pathfinding {
             this.LastTriesNeeded = count;
             this.LastTimeNeeded = stopwatch.Elapsed;
             return ret;
+
+            void ExamineDirection(PathPoint<T> 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<T>(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);
+                }
+            }
         }
 
         /// <summary>
@@ -174,6 +191,13 @@ namespace MLEM.Pathfinding {
         /// <param name="nextPos">The position we're trying to reach from the current position</param>
         public delegate float GetCost(T currPos, T nextPos);
 
+        /// <summary>
+        /// A delegate used by <see cref="AStar{T}.DefaultSpecialDirections"/> and <see cref="AStar{T}.FindPath"/> that determines a set of additional directions (or offsets) that should be tested for walkability.
+        /// </summary>
+        /// <param name="currPos">The current position in the path.</param>
+        /// <returns>A set of additional directions (or offsets) that should be checked for walkability. If the given <paramref name="currPos"/> has no special directions, an empty <see cref="IEnumerable{T}"/> should be returned.</returns>
+        public delegate IEnumerable<T> GetSpecialDirections(T currPos);
+
     }
 
     /// <summary>

MLEM/Pathfinding/AStar2.cs

@@ -13,8 +13,8 @@ namespace MLEM.Pathfinding {
         private static readonly Point[] AllDirs = Direction2Helper.All.Offsets().ToArray();
 
         /// <inheritdoc />
-        public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
+        public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) :
-            base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {}
+            base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries, defaultSpecialDirections) {}
 
         /// <inheritdoc />
         protected override Point AddPositions(Point first, Point second) {

MLEM/Pathfinding/AStar3.cs

@@ -34,8 +34,8 @@ namespace MLEM.Pathfinding {
         }
 
         /// <inheritdoc />
-        public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) :
+        public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) :
-            base(AStar3.AllDirs, AStar3.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {}
+            base(AStar3.AllDirs, AStar3.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries, defaultSpecialDirections) {}
 
         /// <inheritdoc />
         protected override Vector3 AddPositions(Vector3 first, Vector3 second) {

Tests/PathfindingTests.cs

@@ -60,13 +60,33 @@ 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 => p.X == 2 && p.Y == 2 ? new[] {new Point(-1, 2)} : Enumerable.Empty<Point>()));
+        }
+
+        private static Stack<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals, AStar2.GetSpecialDirections getSpecialDirections = 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);
+            var pathFinder = new AStar2((_, p2) => costs[p2.Y][p2.X], allowDiagonals, 1, 64, getSpecialDirections);
             return pathFinder.FindPath(start, end);
         }