Ellpeck/MLEM
1
0
Fork 0
mirror of https://github.com/Ellpeck/MLEM.git synced 2024-11-22 12:58:33 +01:00
Code Issues Projects Releases Wiki Activity

Cleaned up AStar code

Browse source
This commit is contained in:
Ell 2022-10-09 22:18:17 +02:00
parent 0b6e6743cf
commit d3b153fd45
6 changed files with 113 additions and 176 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

3
CHANGELOG.md
View file

@ -14,14 +14,13 @@ Jump to version:
### MLEM ### MLEM
Additions Additions
- Added TokenizedString.Realign - Added TokenizedString.Realign
- Added the ability to include special per-position directions in AStar pathfinding
Improvements Improvements
- Improved EnumHelper.GetValues signature to return an array - Improved EnumHelper.GetValues signature to return an array
- Allow using external gesture handling alongside InputHandler through ExternalGestureHandling - Allow using external gesture handling alongside InputHandler through ExternalGestureHandling
- Discard old data when updating a StaticSpriteBatch - Discard old data when updating a StaticSpriteBatch
- Multi-target net452, making MLEM compatible with MonoGame for consoles - 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** - **Drastically improved StaticSpriteBatch batching performance**
Fixes Fixes

3
Demos/PathfindingDemo.cs
View file

@ -1,6 +1,7 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Linq; using System.Linq;
using System.Threading.Tasks;
using Microsoft.Xna.Framework; using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics; using Microsoft.Xna.Framework.Graphics;
using MLEM.Extensions; 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 // 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) // 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; this.path = foundPath != null ? foundPath.ToList() : null;
// print out some info // print out some info

170
MLEM/Pathfinding/AStar.cs
View file

@ -1,7 +1,6 @@
using System; using System;
using System.Collections.Generic; using System.Collections.Generic;
using System.Diagnostics; using System.Diagnostics;
using System.Threading.Tasks;
namespace MLEM.Pathfinding { namespace MLEM.Pathfinding {
/// <summary> /// <summary>
@ -11,23 +10,6 @@ namespace MLEM.Pathfinding {
/// <typeparam name="T">The type of points used for this path</typeparam> /// <typeparam name="T">The type of points used for this path</typeparam>
public abstract class AStar<T> { 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> /// <summary>
/// The default cost function that determines the cost for each path finding position. /// The default cost function that determines the cost for each path finding position.
/// </summary> /// </summary>
@ -41,13 +23,9 @@ namespace MLEM.Pathfinding {
/// </summary> /// </summary>
public int DefaultMaxTries; public int DefaultMaxTries;
/// <summary> /// <summary>
/// Whether or not diagonal directions are considered while finding a path. /// The default <see cref="CollectAdditionalNeighbors"/> function.
/// </summary> /// </summary>
public bool DefaultAllowDiagonals; public CollectAdditionalNeighbors DefaultAdditionalNeighbors;
/// <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> /// <summary>
/// The amount of tries required for finding the last queried path /// The amount of tries required for finding the last queried path
@ -61,26 +39,15 @@ namespace MLEM.Pathfinding {
/// <summary> /// <summary>
/// Creates a new A* pathfinder with the supplied default settings. /// Creates a new A* pathfinder with the supplied default settings.
/// </summary> /// </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="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="defaultCost">The default cost for a path point</param>
/// <param name="defaultMaxTries">The default amount of tries before path finding is aborted</param> /// <param name="defaultMaxTries">The default amount of tries before path finding is aborted</param>
/// <param name="defaultSpecialDirections">The default function that determines a set of additional directions (or offsets) that should be tested for walkability.</param> /// <param name="defaultAdditionalNeighbors">The default <see cref="CollectAdditionalNeighbors"/> function.</param>
protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) { protected AStar(GetCost defaultCostFunction, float defaultCost, int defaultMaxTries, CollectAdditionalNeighbors defaultAdditionalNeighbors) {
this.AllDirections = allDirections;
this.AdjacentDirections = adjacentDirections;
this.DefaultCostFunction = defaultCostFunction; this.DefaultCostFunction = defaultCostFunction;
this.DefaultCost = defaultCost; this.DefaultCost = defaultCost;
this.DefaultMaxTries = defaultMaxTries; this.DefaultMaxTries = defaultMaxTries;
this.DefaultAllowDiagonals = defaultAllowDiagonals; this.DefaultAdditionalNeighbors = defaultAdditionalNeighbors;
this.DefaultSpecialDirections = defaultSpecialDirections;
}
/// <inheritdoc cref="FindPath(T,T,MLEM.Pathfinding.AStar{T}.GetCost,System.Nullable{float},System.Nullable{int},System.Nullable{bool},MLEM.Pathfinding.AStar{T}.GetSpecialDirections)"/>
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));
} }
/// <summary> /// <summary>
@ -91,45 +58,13 @@ namespace MLEM.Pathfinding {
/// <param name="costFunction">The function that determines the cost for each path point</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="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="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>
/// <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> /// <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, GetSpecialDirections specialDirections = null) { 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, allowDiagonals, specialDirections); this.TryFindPath(start, goal, out var path, out _, costFunction, defaultCost, maxTries, additionalNeighbors);
return path; return path;
} }
/// <inheritdoc cref="FindPath(T,IEnumerable{T},MLEM.Pathfinding.AStar{T}.GetCost,System.Nullable{float},System.Nullable{int},System.Nullable{bool},MLEM.Pathfinding.AStar{T}.GetSpecialDirections)"/>
public Task<Stack<T>> FindPathAsync(T start, IEnumerable<T> goals, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null) {
return Task.Run(() => this.FindPath(start, goals, costFunction, defaultCost, maxTries, allowDiagonals));
}
/// <summary>
/// Tries to find paths between a <paramref name="start"/> position and a set of <paramref name="goals"/> 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 <paramref name="goals"/> is much lower than the total amount of possible positions, or the <paramref name="goals"/> are relatively close to each other.
/// </summary>
/// <param name="start">The point to start path finding at</param>
/// <param name="goals">The set of points to try to find a path to</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="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 paths were found.</returns>
public Stack<T> FindPath(T start, IEnumerable<T> goals, GetCost costFunction = null, float? defaultCost = null, int? maxTries = null, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) {
var lowestCost = float.PositiveInfinity;
Stack<T> 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;
}
/// <summary> /// <summary>
/// Tries to find a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings. /// Tries to find a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings.
/// </summary> /// </summary>
@ -140,23 +75,22 @@ namespace MLEM.Pathfinding {
/// <param name="costFunction">The function that determines the cost for each path point</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="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="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>
/// <param name="specialDirections">An optional function that determines a set of additional directions (or offsets) that should be tested for walkability.</param>
/// <returns>Whether a path was found.</returns> /// <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, bool? allowDiagonals = null, GetSpecialDirections specialDirections = null) { 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; path = null;
totalCost = float.PositiveInfinity; totalCost = float.PositiveInfinity;
var stopwatch = Stopwatch.StartNew(); var stopwatch = Stopwatch.StartNew();
var getCost = costFunction ?? this.DefaultCostFunction; var getCost = costFunction ?? this.DefaultCostFunction;
var diags = allowDiagonals ?? this.DefaultAllowDiagonals;
var tries = maxTries ?? this.DefaultMaxTries; var tries = maxTries ?? this.DefaultMaxTries;
var defCost = defaultCost ?? this.DefaultCost; var defCost = defaultCost ?? this.DefaultCost;
var special = specialDirections ?? this.DefaultSpecialDirections; var additional = additionalNeighbors ?? this.DefaultAdditionalNeighbors;
var neighbors = new HashSet<T>();
var open = new Dictionary<T, PathPoint<T>>(); var open = new Dictionary<T, PathPoint<T>>();
var closed = 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; var count = 0;
while (open.Count > 0) { while (open.Count > 0) {
@ -171,17 +105,32 @@ namespace MLEM.Pathfinding {
open.Remove(current.Pos); open.Remove(current.Pos);
closed.Add(current.Pos, current); closed.Add(current.Pos, current);
if (current.Pos.Equals(goal)) { if (EqualityComparer<T>.Default.Equals(current.Pos, goal)) {
path = AStar<T>.CompilePath(current); path = AStar<T>.CompilePath(current);
totalCost = current.F; totalCost = current.F;
break; break;
} }
foreach (var dir in diags ? this.AllDirections : this.AdjacentDirections) neighbors.Clear();
ExamineDirection(current, dir); this.CollectNeighbors(current.Pos, neighbors);
if (special != null) { additional?.Invoke(current.Pos, neighbors);
foreach (var dir in special(current.Pos))
ExamineDirection(current, dir); 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.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++; count++;
@ -193,36 +142,23 @@ namespace MLEM.Pathfinding {
this.LastTriesNeeded = count; this.LastTriesNeeded = count;
this.LastTimeNeeded = stopwatch.Elapsed; this.LastTimeNeeded = stopwatch.Elapsed;
return path != null; return path != null;
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> /// <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> /// </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> /// <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> /// </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) { private static Stack<T> CompilePath(PathPoint<T> current) {
var path = new Stack<T>(); var path = new Stack<T>();
@ -234,20 +170,20 @@ namespace MLEM.Pathfinding {
} }
/// <summary> /// <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 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. /// If a path point should be unreachable, <see cref="float.PositiveInfinity"/> or <see cref="float.MaxValue"/> should be returned.
/// </summary> /// </summary>
/// <param name="currPos">The current position in the path</param> /// <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="nextPos">The neighboring position whose cost to check.</param>
public delegate float GetCost(T currPos, T nextPos); public delegate float GetCost(T currPos, T nextPos);
/// <summary> /// <summary>
/// A delegate used by <see cref="AStar{T}.DefaultSpecialDirections"/> and <see cref="AStar{T}.TryFindPath"/> that determines a set of additional directions (or offsets) that should be tested for walkability. /// A delegate that determines a set of additional <paramref name="neighbors"/> to be considered for a given <paramref name="position"/>.
/// </summary> /// </summary>
/// <param name="currPos">The current position in the path.</param> /// <param name="position">The position whose neighbors to return.</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> /// <param name="neighbors">The set to populate with neighbors.</param>
public delegate IEnumerable<T> GetSpecialDirections(T currPos); public delegate void CollectAdditionalNeighbors(T position, ISet<T> neighbors);
} }
@ -278,7 +214,7 @@ namespace MLEM.Pathfinding {
/// Creates a new path point with the supplied settings. /// Creates a new path point with the supplied settings.
/// </summary> /// </summary>
/// <param name="pos">The point's position</param> /// <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="parent">The point's parent</param>
/// <param name="terrainCostForThisPos">The point's terrain cost, based on <see cref="AStar{T}.GetCost"/></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> /// <param name="defaultCost">The default cost for a path point</param>
@ -307,7 +243,7 @@ namespace MLEM.Pathfinding {
/// <summary>Returns the hash code for this instance.</summary> /// <summary>Returns the hash code for this instance.</summary>
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns> /// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
public override int GetHashCode() { public override int GetHashCode() {
return this.Pos.GetHashCode(); return EqualityComparer<T>.Default.GetHashCode(this.Pos);
} }
} }

31
MLEM/Pathfinding/AStar2.cs
View file

@ -1,29 +1,36 @@
using System; using System;
using System.Linq; using System.Collections.Generic;
using Microsoft.Xna.Framework; using Microsoft.Xna.Framework;
using MLEM.Misc; using MLEM.Misc;
namespace MLEM.Pathfinding { namespace MLEM.Pathfinding {
/// <summary> /// <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> /// </summary>
public class AStar2 : AStar<Point> { public class AStar2 : AStar<Point> {
private static readonly Point[] AdjacentDirs = Direction2Helper.Adjacent.Offsets().ToArray(); private readonly bool includeDiagonals;
private static readonly Point[] AllDirs = Direction2Helper.All.Offsets().ToArray();
/// <inheritdoc /> /// <inheritdoc />
public AStar2(GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, GetSpecialDirections defaultSpecialDirections = null) : public AStar2(GetCost defaultCostFunction, bool includeDiagonals, float defaultCost = 1, int defaultMaxTries = 10000, CollectAdditionalNeighbors defaultAdditionalNeighbors = null) :
base(AStar2.AllDirs, AStar2.AdjacentDirs, defaultCostFunction, defaultAllowDiagonals, defaultCost, defaultMaxTries, defaultSpecialDirections) {} base(defaultCostFunction, defaultCost, defaultMaxTries, defaultAdditionalNeighbors) {
this.includeDiagonals = includeDiagonals;
/// <inheritdoc />
protected override Point AddPositions(Point first, Point second) {
return first + second;
} }
/// <inheritdoc /> /// <inheritdoc />
protected override float GetManhattanDistance(Point first, Point second) { protected override float GetHeuristicDistance(Point start, Point position) {
return Math.Abs(second.X - first.X) + Math.Abs(second.Y - first.Y); 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());
}
} }
} }

50
MLEM/Pathfinding/AStar3.cs
View file

@ -4,47 +4,43 @@ using Microsoft.Xna.Framework;
namespace MLEM.Pathfinding { namespace MLEM.Pathfinding {
/// <summary> /// <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> /// </summary>
public class AStar3 : AStar<Vector3> { public class AStar3 : AStar<Vector3> {
private static readonly Vector3[] AdjacentDirs = { private readonly bool includeDiagonals;
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 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() { /// <inheritdoc />
var dirs = new List<Vector3>(); 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 x = -1; x <= 1; x++) {
for (var y = -1; y <= 1; y++) { for (var y = -1; y <= 1; y++) {
for (var z = -1; z <= 1; z++) { for (var z = -1; z <= 1; z++) {
if (x == 0 && y == 0 && z == 0) if (x == 0 && y == 0 && z == 0)
continue; continue;
dirs.Add(new Vector3(x, y, z)); neighbors.Add(position + new Vector3(x, y, z));
} }
} }
} }
AStar3.AllDirs = dirs.ToArray(); } 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));
} }
/// <inheritdoc />
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) {}
/// <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);
} }
} }

20
Tests/PathfindingTests.cs
View file

@ -76,12 +76,14 @@ namespace Tests {
Assert.IsNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, true)); 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 // 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, Assert.IsNotNull(PathfindingTests.FindPathInArea(new Point(1, 1), new Point(2, 4), area, false, (p, n) => {
p => p.X == 2 && p.Y == 2 ? new[] {new Point(-1, 2)} : Enumerable.Empty<Point>())); if (p.X == 2 && p.Y == 2)
n.Add(new Point(1, 4));
}));
} }
[Test] [Test]
public void TestMultiplePaths() { public void TestCosts() {
var area = new[] { var area = new[] {
"XXXXXXXX", "XXXXXXXX",
"X 2 X", "X 2 X",
@ -99,23 +101,19 @@ namespace Tests {
pathfinder.TryFindPath(new Point(1, 1), goals[i], out _, out var cost); pathfinder.TryFindPath(new Point(1, 1), goals[i], out _, out var cost);
Assert.AreEqual(goalCosts[i], 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<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals, AStar2.GetSpecialDirections getSpecialDirections = null) { private static Stack<Point> FindPathInArea(Point start, Point end, IEnumerable<string> area, bool allowDiagonals, AStar2.CollectAdditionalNeighbors collectAdditionalNeighbors = null) {
return PathfindingTests.CreatePathfinder(area, allowDiagonals, getSpecialDirections).FindPath(start, end); return PathfindingTests.CreatePathfinder(area, allowDiagonals, collectAdditionalNeighbors).FindPath(start, end);
} }
private static AStar2 CreatePathfinder(IEnumerable<string> area, bool allowDiagonals, AStar2.GetSpecialDirections getSpecialDirections = null) { private static AStar2 CreatePathfinder(IEnumerable<string> area, bool allowDiagonals, AStar2.CollectAdditionalNeighbors collectAdditionalNeighbors = null) {
var costs = area.Select(s => s.Select(c => c switch { var costs = area.Select(s => s.Select(c => c switch {
' ' => 1, ' ' => 1,
'X' => float.PositiveInfinity, 'X' => float.PositiveInfinity,
_ => (float) char.GetNumericValue(c) _ => (float) char.GetNumericValue(c)
}).ToArray()).ToArray(); }).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);
} }
} }