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 ;
2020-02-06 17:43:34 +01:00
using System.Threading.Tasks ;
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>
/// A value that represents an infinite path cost, or a cost for a location that cannot possibly be reached.
/// </summary>
2022-06-15 11:38:11 +02:00
[Obsolete("This field is deprecated. Use float.PositiveInfinity or float.MaxValue instead.")]
2020-09-16 01:21:37 +02:00
public const float InfiniteCost = float . PositiveInfinity ;
2020-05-21 17:21:34 +02:00
/// <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>
2019-08-18 15:14:35 +02:00
public readonly T [ ] AllDirections ;
2020-05-21 17:21:34 +02:00
/// <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>
2019-08-18 15:14:35 +02:00
public readonly T [ ] AdjacentDirections ;
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>
/// Whether or not diagonal directions are considered while finding a path.
/// </summary>
2019-08-18 15:14:35 +02:00
public bool DefaultAllowDiagonals ;
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="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="defaultAllowDiagonals">Whether or not diagonals should be allowed by default</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>
2019-08-18 15:14:35 +02:00
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 ;
}
2020-05-21 17:21:34 +02:00
/// <inheritdoc cref="FindPath"/>
2020-02-06 17:43:34 +01:00
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 ) ) ;
}
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>
/// <param name="allowDiagonals">If diagonals should be looked at for path finding</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>
2019-08-18 15:14:35 +02:00
public Stack < T > FindPath ( T start , T goal , GetCost costFunction = null , float? defaultCost = null , int? maxTries = null , bool? allowDiagonals = null ) {
2020-03-21 16:02:11 +01:00
var stopwatch = Stopwatch . StartNew ( ) ;
2019-09-01 10:54:25 +02:00
2019-08-18 15:14:35 +02:00
var getCost = costFunction ? ? this . DefaultCostFunction ;
var diags = allowDiagonals ? ? this . DefaultAllowDiagonals ;
var tries = maxTries ? ? this . DefaultMaxTries ;
var defCost = defaultCost ? ? this . DefaultCost ;
2020-09-10 02:12:53 +02:00
var open = 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 ) ) ;
2019-08-06 15:33:45 +02:00
var count = 0 ;
2019-09-01 10:54:25 +02:00
Stack < T > ret = null ;
2019-08-06 15:33:45 +02:00
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
2019-08-18 15:14:35 +02:00
if ( current . Pos . Equals ( goal ) ) {
2022-06-15 11:38:11 +02:00
ret = AStar < T > . CompilePath ( current ) ;
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
2019-08-18 15:14:35 +02:00
var dirsUsed = diags ? this . AllDirections : this . AdjacentDirections ;
2019-08-06 15:33:45 +02:00
foreach ( var dir in dirsUsed ) {
2019-08-18 15:14:35 +02:00
var neighborPos = this . AddPositions ( current . Pos , dir ) ;
var cost = getCost ( current . Pos , neighborPos ) ;
2021-11-23 21:42:18 +01:00
if ( ! float . IsPositiveInfinity ( cost ) & & cost < float . MaxValue & & ! closed . ContainsKey ( neighborPos ) ) {
2019-08-18 15:14:35 +02:00
var neighbor = new PathPoint < T > ( neighborPos , this . GetManhattanDistance ( neighborPos , goal ) , current , cost , defCost ) ;
2020-09-10 02:12:53 +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 ) ;
2019-08-06 15:33:45 +02:00
} else {
2020-09-10 02:12:53 +02:00
// if the old waypoint is better, we don't add ours
continue ;
2019-08-06 15:33:45 +02:00
}
}
2020-09-10 02:12:53 +02:00
// 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 ;
2019-09-01 10:54:25 +02:00
return ret ;
2019-08-06 15:33:45 +02:00
}
2020-05-21 17:21:34 +02:00
/// <summary>
/// A helper method to add two positions together.
/// </summary>
2019-08-18 15:14:35 +02:00
protected abstract T AddPositions ( T first , T second ) ;
2020-05-21 17:21:34 +02:00
/// <summary>
/// A helper method to get the Manhattan Distance between two points.
/// </summary>
2019-08-18 15:14:35 +02:00
protected abstract float GetManhattanDistance ( T first , T second ) ;
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>
/// A cost function for a given path finding position.
/// 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>
/// <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>
2019-08-18 15:14:35 +02:00
public delegate float GetCost ( T currPos , T nextPos ) ;
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>
/// The F cost of this path point
/// </summary>
2019-08-18 15:14:35 +02:00
public readonly float F ;
2020-05-21 17:21:34 +02:00
/// <summary>
/// The G cost of this path point
/// </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>
/// <param name="pos">The point's position</param>
/// <param name="distance">The point's manhattan distance from the start point</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="defaultCost">The default cost for a path point</param>
2019-08-18 15:14:35 +02:00
public PathPoint ( T pos , float distance , PathPoint < T > parent , float terrainCostForThisPos , float defaultCost ) {
2019-08-06 15:33:45 +02:00
this . Pos = pos ;
this . Parent = parent ;
this . G = ( parent = = null ? 0 : parent . G ) + terrainCostForThisPos ;
2019-08-18 15:14:35 +02:00
this . F = this . G + distance * 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 ( ) {
return this . Pos . GetHashCode ( ) ;
}
}
2022-06-17 18:23:47 +02:00
}