using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading.Tasks;
namespace MLEM.Pathfinding {
///
/// This is an abstract implementation of the A* path finding algorithm.
/// This implementation is used by , a 2-dimensional A* path finding algorithm, and , a 3-dimensional A* path finding algorithm.
///
/// 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.
///
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.
///
public GetCost DefaultCostFunction;
///
/// The default cost for a path point.
///
public float DefaultCost;
///
/// The default amount of maximum tries that will be used before path finding is aborted.
///
public int DefaultMaxTries;
///
/// Whether or not diagonal directions are considered while finding a path.
///
public bool DefaultAllowDiagonals;
///
/// The amount of tries required for finding the last queried path
///
public int LastTriesNeeded { get; private set; }
///
/// The amount of time required for finding the last queried path
///
public TimeSpan LastTimeNeeded { get; private set; }
///
/// 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
protected AStar(T[] allDirections, T[] adjacentDirections, GetCost defaultCostFunction, bool defaultAllowDiagonals, float defaultCost = 1, int defaultMaxTries = 10000) {
this.AllDirections = allDirections;
this.AdjacentDirections = adjacentDirections;
this.DefaultCostFunction = defaultCostFunction;
this.DefaultCost = defaultCost;
this.DefaultMaxTries = defaultMaxTries;
this.DefaultAllowDiagonals = defaultAllowDiagonals;
}
///
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));
}
///
/// Finds a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings.
///
/// The point to start path finding at
/// The point 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
/// 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) {
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 open = new Dictionary>();
var closed = new Dictionary>();
open.Add(start, new PathPoint(start, this.GetManhattanDistance(start, goal), null, 0, defCost));
var count = 0;
Stack ret = null;
while (open.Count > 0) {
PathPoint current = null;
foreach (var point in open.Values) {
if (current == null || point.F < current.F)
current = point;
}
if (current == null)
break;
open.Remove(current.Pos);
closed.Add(current.Pos, current);
if (current.Pos.Equals(goal)) {
ret = CompilePath(current);
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);
}
}
count++;
if (count >= tries)
break;
}
stopwatch.Stop();
this.LastTriesNeeded = count;
this.LastTimeNeeded = stopwatch.Elapsed;
return ret;
}
///
/// A helper method to add two positions together.
///
protected abstract T AddPositions(T first, T second);
///
/// A helper method to get the Manhattan Distance between two points.
///
protected abstract float GetManhattanDistance(T first, T second);
private static Stack CompilePath(PathPoint current) {
var path = new Stack();
while (current != null) {
path.Push(current.Pos);
current = current.Parent;
}
return path;
}
///
/// A cost function for a given path finding position.
/// If a path point should have the default cost, should be returned.
/// If a path point should be unreachable, should be returned.
///
/// The current position in the path
/// The position we're trying to reach from the current position
public delegate float GetCost(T currPos, T nextPos);
}
///
/// A point in a path
///
/// The type of point used for this path
public class PathPoint {
///
/// The path point that this point originated from
///
public readonly PathPoint Parent;
///
/// The position of this path point
///
public readonly T Pos;
///
/// The F cost of this path point
///
public readonly float F;
///
/// The G cost of this path point
///
public readonly float G;
///
/// 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 parent
/// The point's terrain cost, based on
/// The default cost for a path point
public PathPoint(T pos, float distance, PathPoint parent, float terrainCostForThisPos, float defaultCost) {
this.Pos = pos;
this.Parent = parent;
this.G = (parent == null ? 0 : parent.G) + terrainCostForThisPos;
this.F = this.G + distance * defaultCost;
}
/// Indicates whether this instance and a specified object are equal.
/// The object to compare with the current instance.
/// if and this instance are the same type and represent the same value; otherwise, .
public override bool Equals(object obj) {
if (obj == this)
return true;
return obj is PathPoint point && point.Pos.Equals(this.Pos);
}
/// 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();
}
}
}