2019-09-01 10:54:25 +02:00
using System ;
2019-08-06 15:33:45 +02:00
using System.Collections.Generic ;
2020-03-21 16:02:11 +01:00
using System.Diagnostics ;
2019-08-06 15:33:45 +02:00
namespace MLEM.Pathfinding {
2020-05-21 17:21:34 +02:00
/// <summary>
/// This is an abstract implementation of the A* path finding algorithm.
/// This implementation is used by <see cref="AStar2"/>, a 2-dimensional A* path finding algorithm, and <see cref="AStar3"/>, a 3-dimensional A* path finding algorithm.
/// </summary>
/// <typeparam name="T">The type of points used for this path</typeparam>
2019-08-18 15:14:35 +02:00
public abstract class AStar < T > {
2020-05-21 17:21:34 +02:00
/// <summary>
/// The default cost function that determines the cost for each path finding position.
/// </summary>
2019-08-18 15:14:35 +02:00
public GetCost DefaultCostFunction ;
2020-05-21 17:21:34 +02:00
/// <summary>
/// The default cost for a path point.
/// </summary>
2019-08-18 15:14:35 +02:00
public float DefaultCost ;
2020-05-21 17:21:34 +02:00
/// <summary>
/// The default amount of maximum tries that will be used before path finding is aborted.
/// </summary>
2019-08-18 15:14:35 +02:00
public int DefaultMaxTries ;
2020-05-21 17:21:34 +02:00
/// <summary>
2022-10-09 22:18:17 +02:00
/// The default <see cref="CollectAdditionalNeighbors"/> function.
2020-05-21 17:21:34 +02:00
/// </summary>
2022-10-09 22:18:17 +02:00
public CollectAdditionalNeighbors DefaultAdditionalNeighbors ;
2022-10-09 20:07:38 +02:00
2020-05-21 17:21:34 +02:00
/// <summary>
/// The amount of tries required for finding the last queried path
/// </summary>
2019-08-18 15:14:35 +02:00
public int LastTriesNeeded { get ; private set ; }
2020-05-21 17:21:34 +02:00
/// <summary>
/// The amount of time required for finding the last queried path
/// </summary>
2019-09-01 10:54:25 +02:00
public TimeSpan LastTimeNeeded { get ; private set ; }
2019-08-18 15:14:35 +02:00
2020-05-21 17:21:34 +02:00
/// <summary>
/// Creates a new A* pathfinder with the supplied default settings.
/// </summary>
/// <param name="defaultCostFunction">The default function for cost determination of 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>
2022-10-09 22:18:17 +02:00
/// <param name="defaultAdditionalNeighbors">The default <see cref="CollectAdditionalNeighbors"/> function.</param>
protected AStar ( GetCost defaultCostFunction , float defaultCost , int defaultMaxTries , CollectAdditionalNeighbors defaultAdditionalNeighbors ) {
2019-08-18 15:14:35 +02:00
this . DefaultCostFunction = defaultCostFunction ;
this . DefaultCost = defaultCost ;
this . DefaultMaxTries = defaultMaxTries ;
2022-10-09 22:18:17 +02:00
this . DefaultAdditionalNeighbors = defaultAdditionalNeighbors ;
2020-02-06 17:43:34 +01:00
}
2020-05-21 17:21:34 +02:00
/// <summary>
/// Finds a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings.
/// </summary>
/// <param name="start">The point to start path finding at</param>
/// <param name="goal">The point 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>
2022-10-09 22:18:17 +02:00
/// <param name="additionalNeighbors">A function that determines a set of additional neighbors to be considered for a given point.</param>
2021-11-23 21:42:18 +01:00
/// <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>
2022-10-09 22:18:17 +02:00
public Stack < T > FindPath ( T start , T goal , GetCost costFunction = null , float? defaultCost = null , int? maxTries = null , CollectAdditionalNeighbors additionalNeighbors = null ) {
2022-10-10 11:31:23 +02:00
this . TryFindPath ( start , new [ ] { goal } , out var path , out _ , costFunction , defaultCost , maxTries , additionalNeighbors ) ;
2022-10-09 21:04:39 +02:00
return path ;
}
/// <summary>
/// Tries to find a path between two points using this pathfinder's default settings or, alternatively, the supplied override settings.
/// </summary>
/// <param name="start">The point to start path finding at</param>
2022-10-10 11:31:23 +02:00
/// <param name="goals">The points to find a path to, one of which will be chosen as the closest or best destination</param>
2022-10-09 21:04:39 +02:00
/// <param name="path">The path that was found, or <see langword="null"/> if no path was found.</param>
/// <param name="totalCost">The total cost that was calculated for the path, or <see cref="float.PositiveInfinity"/> if no path was found.</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>
2022-10-09 22:18:17 +02:00
/// <param name="additionalNeighbors">A function that determines a set of additional neighbors to be considered for a given point.</param>
2022-10-09 21:04:39 +02:00
/// <returns>Whether a path was found.</returns>
2022-10-10 11:31:23 +02:00
public bool TryFindPath ( T start , ICollection < T > goals , out Stack < T > path , out float totalCost , GetCost costFunction = null , float? defaultCost = null , int? maxTries = null , CollectAdditionalNeighbors additionalNeighbors = null ) {
2022-10-09 21:04:39 +02:00
path = null ;
totalCost = float . PositiveInfinity ;
2019-09-01 10:54:25 +02:00
2022-10-09 21:04:39 +02:00
var stopwatch = Stopwatch . StartNew ( ) ;
2019-08-18 15:14:35 +02:00
var getCost = costFunction ? ? this . DefaultCostFunction ;
var tries = maxTries ? ? this . DefaultMaxTries ;
var defCost = defaultCost ? ? this . DefaultCost ;
2022-10-09 22:18:17 +02:00
var additional = additionalNeighbors ? ? this . DefaultAdditionalNeighbors ;
2019-08-18 15:14:35 +02:00
2022-10-09 22:18:17 +02:00
var neighbors = new HashSet < T > ( ) ;
2020-09-10 02:12:53 +02:00
var open = new Dictionary < T , PathPoint < T > > ( ) ;
var closed = new Dictionary < T , PathPoint < T > > ( ) ;
2022-10-10 11:31:23 +02:00
open . Add ( start , new PathPoint < T > ( start , this . GetMinHeuristicDistance ( start , goals ) , null , 0 , defCost ) ) ;
2019-08-06 15:33:45 +02:00
var count = 0 ;
while ( open . Count > 0 ) {
2019-08-18 15:14:35 +02:00
PathPoint < T > current = null ;
2020-09-10 02:12:53 +02:00
foreach ( var point in open . Values ) {
if ( current = = null | | point . F < current . F )
2019-08-06 15:33:45 +02:00
current = point ;
2020-09-10 02:12:53 +02:00
}
2019-08-06 15:33:45 +02:00
if ( current = = null )
break ;
2020-09-10 02:12:53 +02:00
open . Remove ( current . Pos ) ;
closed . Add ( current . Pos , current ) ;
2019-08-06 15:33:45 +02:00
2022-10-10 11:31:23 +02:00
if ( goals . Contains ( current . Pos ) ) {
2022-10-09 21:04:39 +02:00
path = AStar < T > . CompilePath ( current ) ;
totalCost = current . F ;
2019-09-01 10:54:25 +02:00
break ;
2019-08-18 15:14:35 +02:00
}
2019-08-06 15:33:45 +02:00
2022-10-09 22:18:17 +02:00
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 ) ) {
2022-10-10 11:31:23 +02:00
var neighbor = new PathPoint < T > ( neighborPos , this . GetMinHeuristicDistance ( neighborPos , goals ) , current , cost , defCost ) ;
2022-10-09 22:18:17 +02:00
// 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 ) ;
}
2019-08-06 15:33:45 +02:00
}
count + + ;
2019-08-18 15:14:35 +02:00
if ( count > = tries )
2019-08-06 15:33:45 +02:00
break ;
}
2020-02-06 17:43:34 +01:00
2020-03-21 16:02:11 +01:00
stopwatch . Stop ( ) ;
2019-08-18 15:14:35 +02:00
this . LastTriesNeeded = count ;
2020-03-21 16:02:11 +01:00
this . LastTimeNeeded = stopwatch . Elapsed ;
2022-10-09 21:04:39 +02:00
return path ! = null ;
2019-08-06 15:33:45 +02:00
}
2020-05-21 17:21:34 +02:00
/// <summary>
2022-10-09 22:18:17 +02:00
/// 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.
2020-05-21 17:21:34 +02:00
/// </summary>
2022-10-09 22:18:17 +02:00
/// <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 ) ;
2019-08-18 15:14:35 +02:00
2020-05-21 17:21:34 +02:00
/// <summary>
2022-10-09 22:18:17 +02:00
/// 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.
2020-05-21 17:21:34 +02:00
/// </summary>
2022-10-09 22:18:17 +02:00
/// <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 ) ;
2019-08-18 15:14:35 +02:00
2022-10-10 11:31:23 +02:00
private float GetMinHeuristicDistance ( T start , IEnumerable < T > positions ) {
var min = float . MaxValue ;
foreach ( var position in positions )
min = Math . Min ( min , this . GetHeuristicDistance ( start , position ) ) ;
return min ;
}
2019-08-18 15:14:35 +02:00
private static Stack < T > CompilePath ( PathPoint < T > current ) {
var path = new Stack < T > ( ) ;
2019-08-06 15:33:45 +02:00
while ( current ! = null ) {
path . Push ( current . Pos ) ;
current = current . Parent ;
}
return path ;
}
2020-05-21 17:21:34 +02:00
/// <summary>
2022-10-09 22:18:17 +02:00
/// A cost function for a given pair of neighboring path finding positions.
2020-05-21 17:21:34 +02:00
/// If a path point should have the default cost, <see cref="AStar{T}.DefaultCost"/> should be returned.
2022-06-15 11:38:11 +02:00
/// If a path point should be unreachable, <see cref="float.PositiveInfinity"/> or <see cref="float.MaxValue"/> should be returned.
2020-05-21 17:21:34 +02:00
/// </summary>
2022-10-09 22:18:17 +02:00
/// <param name="currPos">The current position in the path.</param>
/// <param name="nextPos">The neighboring position whose cost to check.</param>
2019-08-18 15:14:35 +02:00
public delegate float GetCost ( T currPos , T nextPos ) ;
2019-08-06 15:33:45 +02:00
2022-10-09 20:07:38 +02:00
/// <summary>
2022-10-09 22:18:17 +02:00
/// A delegate that determines a set of additional <paramref name="neighbors"/> to be considered for a given <paramref name="position"/>.
2022-10-09 20:07:38 +02:00
/// </summary>
2022-10-09 22:18:17 +02:00
/// <param name="position">The position whose neighbors to return.</param>
/// <param name="neighbors">The set to populate with neighbors.</param>
public delegate void CollectAdditionalNeighbors ( T position , ISet < T > neighbors ) ;
2022-10-09 20:07:38 +02:00
2019-08-06 15:33:45 +02:00
}
2020-05-21 17:21:34 +02:00
/// <summary>
/// A point in a <see cref="AStar{T}"/> path
/// </summary>
/// <typeparam name="T">The type of point used for this path</typeparam>
2021-12-21 11:39:29 +01:00
public class PathPoint < T > : IEquatable < PathPoint < T > > {
2019-08-06 15:33:45 +02:00
2020-05-21 17:21:34 +02:00
/// <summary>
/// The path point that this point originated from
/// </summary>
2019-08-18 15:14:35 +02:00
public readonly PathPoint < T > Parent ;
2020-05-21 17:21:34 +02:00
/// <summary>
/// The position of this path point
/// </summary>
2019-08-18 15:14:35 +02:00
public readonly T Pos ;
2020-05-21 17:21:34 +02:00
/// <summary>
2023-02-18 12:32:32 +01:00
/// The F cost of this path point, which is the estimated total distance from the start to the goal.
2020-05-21 17:21:34 +02:00
/// </summary>
2019-08-18 15:14:35 +02:00
public readonly float F ;
2020-05-21 17:21:34 +02:00
/// <summary>
2023-02-18 12:32:32 +01:00
/// The G cost of this path point, which is the actual distance from the start to the current <see cref="Pos"/>.
2020-05-21 17:21:34 +02:00
/// </summary>
2019-08-18 15:14:35 +02:00
public readonly float G ;
2019-08-06 15:33:45 +02:00
2020-05-21 17:21:34 +02:00
/// <summary>
/// Creates a new path point with the supplied settings.
/// </summary>
2023-02-18 12:32:32 +01:00
/// <param name="pos">The point's position.</param>
/// <param name="heuristicDistance">The point's estimated distance from the <paramref name="pos"/> to the goal, based on the <paramref name="defaultCost"/>.</param>
/// <param name="parent">The point's parent.</param>
/// <param name="terrainCost">The terrain cost to move from the <paramref name="parent"/> to this point, based on <see cref="AStar{T}.GetCost"/>.</param>
/// <param name="defaultCost">The default cost for a path point.</param>
public PathPoint ( T pos , float heuristicDistance , PathPoint < T > parent , float terrainCost , float defaultCost ) {
2019-08-06 15:33:45 +02:00
this . Pos = pos ;
this . Parent = parent ;
2023-02-18 12:32:32 +01:00
this . G = ( parent = = null ? 0 : parent . G ) + terrainCost ;
this . F = this . G + heuristicDistance * defaultCost ;
2019-08-06 15:33:45 +02:00
}
2021-12-21 11:39:29 +01:00
/// <summary>Indicates whether the current object is equal to another object of the same type.</summary>
/// <param name="other">An object to compare with this object.</param>
/// <returns>true if the current object is equal to the <paramref name="other">other</paramref> parameter; otherwise, false.</returns>
public bool Equals ( PathPoint < T > other ) {
2022-06-15 11:38:11 +02:00
return object . ReferenceEquals ( this , other ) | | EqualityComparer < T > . Default . Equals ( this . Pos , other . Pos ) ;
2021-12-21 11:39:29 +01:00
}
2021-11-22 19:25:18 +01:00
/// <summary>Indicates whether this instance and a specified object are equal.</summary>
/// <param name="obj">The object to compare with the current instance.</param>
/// <returns><see langword="true" /> if <paramref name="obj" /> and this instance are the same type and represent the same value; otherwise, <see langword="false" />.</returns>
2019-08-06 15:33:45 +02:00
public override bool Equals ( object obj ) {
2021-12-21 11:39:29 +01:00
return obj is PathPoint < T > other & & this . Equals ( other ) ;
2019-08-06 15:33:45 +02:00
}
2021-11-22 19:25:18 +01:00
/// <summary>Returns the hash code for this instance.</summary>
/// <returns>A 32-bit signed integer that is the hash code for this instance.</returns>
2019-08-06 15:33:45 +02:00
public override int GetHashCode ( ) {
2022-10-09 22:18:17 +02:00
return EqualityComparer < T > . Default . GetHashCode ( this . Pos ) ;
2019-08-06 15:33:45 +02:00
}
}
2022-06-17 18:23:47 +02:00
}