/// <summary>
/// <para>A* pathfinding algorithm. Calculates the shortest path from origin to target if exists.</para>
/// <para>If the number of checked positions is greater than maxCheckedPositions or the objective is not accessible, returns a path to the closest position found.</para>
/// </summary>
/// <param name="origin">Start position (will not be included in result path).</param>
/// <param name="target">Objective position.</param>
/// <param name="isPositionAccessible">Method to check if a position is accessible.</param>
/// <param name="maxCheckedPositions">Maximum number of checked positions to reach the objective.</param>
/// <returns>The best path found from origin to target.</returns>
public static List <Vector2Int> AstarPath(Vector2Int origin, Vector2Int target, IsPositionAccessible isPositionAccessible, int maxCheckedPositions)
{
Stopwatch chrono = new Stopwatch();
chrono.Start();
List <Vector2Int> path = new List <Vector2Int>();
Dictionary <Vector2Int, PosInPath> checkedPositions = new Dictionary <Vector2Int, PosInPath>();
PosInPath originPosInPath = new PosInPath(origin, 0, null);
Dictionary <Vector2Int, PosInPath> remainingPositions = new Dictionary <Vector2Int, PosInPath>
{
{ originPosInPath.Pos, originPosInPath }
};
bool targetNotAccessible = false;
bool targetReached = false;
PosInPath auxPosInPath;
PosInPath currentPosInPath = null;
PosInPath closestToObjective = originPosInPath;
float closestDistanceToObjective = closestToObjective.Pos.Distance(target);
PosInPath nextPosInPath;
float distance;
bool isAlreadyInRemaining;
bool hasBetterAccumulatedCost;
// Calcule path
while (!targetReached && !targetNotAccessible)
{
// Select next position
currentPosInPath = null;
foreach (KeyValuePair <Vector2Int, PosInPath> entry in remainingPositions)
{
auxPosInPath = entry.Value;
if (currentPosInPath == null ||
(auxPosInPath.AccumulatedCost + auxPosInPath.Pos.Distance(target)) <
(currentPosInPath.AccumulatedCost + currentPosInPath.Pos.Distance(target)))
{
currentPosInPath = auxPosInPath;
}
}
// Remove from remaining and add to checked
remainingPositions.Remove(currentPosInPath.Pos);
checkedPositions.Add(currentPosInPath.Pos, currentPosInPath);
// Check if already touching target
if (IsTouchingTarget(currentPosInPath.Pos, target, isPositionAccessible))
{
targetReached = true;
}
else
{
// Check around positions
foreach (Vector2Int movement in EightDirections2D)
{
nextPosInPath = new PosInPath(currentPosInPath.Pos + movement,
currentPosInPath.AccumulatedCost + movement.magnitude,
currentPosInPath);
// If the position hasn't been checked previously and is accessible
if (!checkedPositions.ContainsKey(nextPosInPath.Pos) && IsPossibleDirection(currentPosInPath.Pos, movement, isPositionAccessible))
{
isAlreadyInRemaining = remainingPositions.TryGetValue(nextPosInPath.Pos, out auxPosInPath);
hasBetterAccumulatedCost = isAlreadyInRemaining && nextPosInPath.AccumulatedCost < auxPosInPath.AccumulatedCost;
// If has a better accumulated cost than an existing one in remaining, substitute it
if (hasBetterAccumulatedCost)
{
remainingPositions.Remove(auxPosInPath.Pos);
}
// If is a new position or has a better accumulated cost, add to remaining
if (!isAlreadyInRemaining || hasBetterAccumulatedCost)
{
remainingPositions.Add(nextPosInPath.Pos, nextPosInPath);
// Check if is the closest to target
distance = nextPosInPath.Pos.Distance(target);
if (distance <= closestDistanceToObjective)
{
closestToObjective = nextPosInPath;
closestDistanceToObjective = distance;
}
}
}
}
// Check if target isn't accesible
targetNotAccessible = remainingPositions.Count == 0 || checkedPositions.Count >= maxCheckedPositions;
}
}
// If is impossible acces to the target use the closest position as desitiny
if (targetNotAccessible)
{
currentPosInPath = closestToObjective;
}
// If the currentPosition ( best last position found ) is closer to target than the origin is a good path
if (currentPosInPath.Pos.Distance(target) < origin.Distance(target))
{
// Do the reverse path, from the end to the origin position
while (currentPosInPath.Pos != origin)
{
path.Add(currentPosInPath.Pos);
currentPosInPath = currentPosInPath.OriginPos;
}
path.Reverse();
UpdateAstarStatistics(chrono.ElapsedMilliseconds, path.Count, checkedPositions.Count);
}
chrono.Stop();
return(path);
}
public PosInPath(Vector2Int position, float acummulatedCost, PosInPath originPosition)
{
Pos = position;
AccumulatedCost = acummulatedCost;
OriginPos = originPosition;
}
