using System;
using System.Collections.Generic;
using System.Diagnostics;
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 {
///
/// 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;
///
/// The default function.
///
public CollectAdditionalNeighbors DefaultAdditionalNeighbors;
///
/// 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.
///
/// The default function for cost determination of a path point
/// The default cost for a path point
/// The default amount of tries before path finding is aborted
/// The default function.
protected AStar(GetCost defaultCostFunction, float defaultCost, int defaultMaxTries, CollectAdditionalNeighbors defaultAdditionalNeighbors) {
this.DefaultCostFunction = defaultCostFunction;
this.DefaultCost = defaultCost;
this.DefaultMaxTries = defaultMaxTries;
this.DefaultAdditionalNeighbors = defaultAdditionalNeighbors;
}
///
/// 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
/// A function that determines a set of additional neighbors to be considered for a given point.
/// 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, CollectAdditionalNeighbors additionalNeighbors = null) {
this.TryFindPath(start, new[] {goal}, out var path, out _, costFunction, defaultCost, maxTries, additionalNeighbors);
return path;
}
///
/// Tries to find 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 points to find a path to, one of which will be chosen as the closest or best destination
/// The path that was found, or if no path was found.
/// The total cost that was calculated for the path, or if no path was found.
/// 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
/// A function that determines a set of additional neighbors to be considered for a given point.
/// Whether a path was found.
public bool TryFindPath(T start, ICollection goals, out Stack 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 tries = maxTries ?? this.DefaultMaxTries;
var defCost = defaultCost ?? this.DefaultCost;
var additional = additionalNeighbors ?? this.DefaultAdditionalNeighbors;
var neighbors = new HashSet();
var open = new Dictionary>();
var closed = new Dictionary>();
open.Add(start, new PathPoint(start, this.GetMinHeuristicDistance(start, goals), null, 0, defCost));
var count = 0;
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 (goals.Contains(current.Pos)) {
path = AStar.CompilePath(current);
totalCost = current.F;
break;
}
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(neighborPos, this.GetMinHeuristicDistance(neighborPos, goals), 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 path != null;
}
///
/// This method should implement a heuristic that determines the total distance between the given position and the given second position .
/// Note that this is multiplied with the automatically, so no costs need to be considered in this method's return value.
///
/// The start position.
/// The position to get the distance to.
/// The total distance between the two positions.
protected abstract float GetHeuristicDistance(T start, T position);
///
/// This method should populate a set of positions that are considered to the given . For example, this method might return directly adjacent positions, diagonal positions, or faraway positions that can be teleported to.
///
/// The position whose neighbors to return.
/// The set to populate with neighbors.
protected abstract void CollectNeighbors(T position, ISet neighbors);
private float GetMinHeuristicDistance(T start, IEnumerable positions) {
var min = float.MaxValue;
foreach (var position in positions)
min = Math.Min(min, this.GetHeuristicDistance(start, position));
return min;
}
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 pair of neighboring path finding positions.
/// If a path point should have the default cost, should be returned.
/// If a path point should be unreachable, or should be returned.
///
/// The current position in the path.
/// The neighboring position whose cost to check.
public delegate float GetCost(T currPos, T nextPos);
///
/// A delegate that determines a set of additional to be considered for a given .
///
/// The position whose neighbors to return.
/// The set to populate with neighbors.
public delegate void CollectAdditionalNeighbors(T position, ISet neighbors);
}
///
/// A point in a path
///
/// The type of point used for this path
public class PathPoint : IEquatable> {
///
/// 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, which is the estimated total distance from the start to the goal.
///
public readonly float F;
///
/// The G cost of this path point, which is the actual distance from the start to the current .
///
public readonly float G;
///
/// Creates a new path point with the supplied settings.
///
/// The point's position.
/// The point's estimated distance from the to the goal, based on the .
/// The point's parent.
/// The terrain cost to move from the to this point, based on .
/// The default cost for a path point.
public PathPoint(T pos, float heuristicDistance, PathPoint parent, float terrainCost, float defaultCost) {
this.Pos = pos;
this.Parent = parent;
this.G = (parent == null ? 0 : parent.G) + terrainCost;
this.F = this.G + heuristicDistance * defaultCost;
}
/// Indicates whether the current object is equal to another object of the same type.
/// An object to compare with this object.
/// true if the current object is equal to the other parameter; otherwise, false.
public bool Equals(PathPoint other) {
return object.ReferenceEquals(this, other) || EqualityComparer.Default.Equals(this.Pos, other.Pos);
}
/// 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) {
return obj is PathPoint other && this.Equals(other);
}
/// 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 EqualityComparer.Default.GetHashCode(this.Pos);
}
}
}