From bfa4ab4ac2e3cc2e2f311cd355e1f42815b32cf2 Mon Sep 17 00:00:00 2001 From: Ellpeck Date: Sun, 9 Oct 2022 20:07:38 +0200 Subject: [PATCH] Added the ability to include special per-position directions in AStar pathfinding --- CHANGELOG.md | 1 + MLEM/Pathfinding/AStar.cs | 64 ++++++++++++++++++++++++++------------ MLEM/Pathfinding/AStar2.cs | 4 +-- MLEM/Pathfinding/AStar3.cs | 4 +-- Tests/PathfindingTests.cs | 24 ++++++++++++-- 5 files changed, 71 insertions(+), 26 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f50598c..4a2cec2 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/MLEM/Pathfinding/AStar.cs b/MLEM/Pathfinding/AStar.cs index 92c0a9f..11f8ade 100644 --- a/MLEM/Pathfinding/AStar.cs +++ b/MLEM/Pathfinding/AStar.cs @@ -16,6 +16,7 @@ namespace MLEM.Pathfinding { /// [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. @@ -26,6 +27,7 @@ namespace MLEM.Pathfinding { /// 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. /// @@ -42,6 +44,11 @@ namespace MLEM.Pathfinding { /// Whether or not diagonal directions are considered while finding a path. /// 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; + /// /// The amount of tries required for finding the last queried path /// @@ -60,13 +67,15 @@ namespace MLEM.Pathfinding { /// 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 - protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) { + /// 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; this.DefaultCostFunction = defaultCostFunction; this.DefaultCost = defaultCost; this.DefaultMaxTries = defaultMaxTries; this.DefaultAllowDiagonals = defaultAllowDiagonals; + this.DefaultSpecialDirections = defaultSpecialDirections; } /// @@ -83,14 +92,16 @@ namespace MLEM.Pathfinding { /// 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 path was found. - public Stack FindPath(T start, T goal, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) { + public Stack 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>(); var closed = new Dictionary>(); @@ -115,24 +126,11 @@ namespace MLEM.Pathfinding { break; } - var dirsUsed = diags ? this.AllDirections : this.AdjacentDirections; - foreach (var dir in dirsUsed) { - 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 - continue; - } - } - // add the new neighbor as a possible waypoint - open.Add(neighborPos, neighbor); - } + 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); } count++; @@ -144,6 +142,25 @@ namespace MLEM.Pathfinding { this.LastTriesNeeded = count; this.LastTimeNeeded = stopwatch.Elapsed; return ret; + + 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); + } + } } /// @@ -174,6 +191,13 @@ namespace MLEM.Pathfinding { /// The position we're trying to reach from the current position 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. + /// + /// 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); + } /// diff --git a/MLEM/Pathfinding/AStar2.cs b/MLEM/Pathfinding/AStar2.cs index 06008f6..89e45e8 100644 --- a/MLEM/Pathfinding/AStar2.cs +++ b/MLEM/Pathfinding/AStar2.cs @@ -13,8 +13,8 @@ namespace MLEM.Pathfinding { private static readonly Point[] AllDirs = Direction2Helper.All.Offsets().ToArray(); /// - public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) : - base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {} + 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) { diff --git a/MLEM/Pathfinding/AStar3.cs b/MLEM/Pathfinding/AStar3.cs index ef14939..71f6108 100644 --- a/MLEM/Pathfinding/AStar3.cs +++ b/MLEM/Pathfinding/AStar3.cs @@ -34,8 +34,8 @@ namespace MLEM.Pathfinding { } /// - public AStar3(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) : - base(AStar3.AllDirs, AStar3.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries) {} + 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) { diff --git a/Tests/PathfindingTests.cs b/Tests/PathfindingTests.cs index 6724eea..688d4fe 100644 --- a/Tests/PathfindingTests.cs +++ b/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 FindPathInArea(Point start, Point end, IEnumerable 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())); + } + + private static Stack FindPathInArea(Point start, Point end, IEnumerable 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); }