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); } } }