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>
2020-04-08 17:45:12 +02:00
public static readonly 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>
/// <returns>A stack of path points, where the top item is the first point to go to.</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 ;
var open = new List < PathPoint < T > > ( ) ;
var closed = new List < PathPoint < T > > ( ) ;
open . Add ( 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 ;
var lowestF = float . MaxValue ;
2019-08-06 15:33:45 +02:00
foreach ( var point in open )
if ( point . F < lowestF ) {
current = point ;
lowestF = point . F ;
}
if ( current = = null )
break ;
open . Remove ( current ) ;
closed . Add ( current ) ;
2019-08-18 15:14:35 +02:00
if ( current . Pos . Equals ( goal ) ) {
2019-09-01 10:54:25 +02:00
ret = CompilePath ( current ) ;
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 ) ;
2020-04-08 17:45:12 +02:00
if ( ! float . IsInfinity ( cost ) & & cost < float . MaxValue ) {
2019-08-18 15:14:35 +02:00
var neighbor = new PathPoint < T > ( neighborPos , this . GetManhattanDistance ( neighborPos , goal ) , current , cost , defCost ) ;
2019-08-06 15:33:45 +02:00
if ( ! closed . Contains ( neighbor ) ) {
var alreadyIndex = open . IndexOf ( neighbor ) ;
if ( alreadyIndex < 0 ) {
open . Add ( neighbor ) ;
} else {
var alreadyNeighbor = open [ alreadyIndex ] ;
if ( neighbor . G < alreadyNeighbor . G ) {
open . Remove ( alreadyNeighbor ) ;
open . Add ( neighbor ) ;
}
}
}
}
}
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.
/// If a path point should be unreachable, <see cref="AStar{T}.InfiniteCost"/> should be returned.
/// </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>
2019-08-18 15:14:35 +02:00
public class 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
}
2020-05-21 17:21:34 +02:00
/// <inheritdoc />
2019-08-06 15:33:45 +02:00
public override bool Equals ( object obj ) {
if ( obj = = this )
return true ;
2019-08-18 15:14:35 +02:00
return obj is PathPoint < T > point & & point . Pos . Equals ( this . Pos ) ;
2019-08-06 15:33:45 +02:00
}
2020-05-21 17:21:34 +02:00
/// <inheritdoc />
2019-08-06 15:33:45 +02:00
public override int GetHashCode ( ) {
return this . Pos . GetHashCode ( ) ;
}
}
}