/// <summary>
/// Update the path. Executes a single step of path finding until the final path has been found.
/// </summary>
/// <returns>The current path in progress, or null if there is no path.</returns>
public AStarNode[] Update()
{
// Loop while there are nodes in the open set, if there are none left and a path hasn't been found then there is no path.
if (_openSet.Count > 0)
{
// Get the node with the lowest score. (F)
AStarNode current = _openSet.OrderBy(x => x.F).First();
// Check if the current node is the end, in which case the path has been found.
if (current.Equals(_end))
{
Finished = true;
}
else
{
// Update sets.
_openSet.Remove(current);
_closedSet.Add(current);
// Get neighbors of current.
IEnumerable <AStarNode> neighbors = GetNeighbors(current.X, current.Y);
foreach (AStarNode neighbor in neighbors)
{
// Check if the neighbor is done with, in which case we skip.
if (_closedSet.Contains(neighbor))
{
continue;
}
// Get the tentative distance between the current and the neighbor. Using 1 as distance.
int tentativeG = current.G + 1;
// Check if the neighbor is being evaluated.
if (_openSet.Contains(neighbor))
{
// Check if we have found a more efficient way to the neighbor node.
if (tentativeG < neighbor.G)
{
neighbor.G = tentativeG;
}
}
else
{
// Assign the calculated distance and add the node to the open set.
neighbor.G = tentativeG;
_openSet.Add(neighbor);
}
neighbor.H = HeuristicFunction(neighbor, _end);
neighbor.CameFrom = current;
}
}
// Return the path as currently found.
List <AStarNode> path = new List <AStarNode> {
current
};
// Trace current's ancestry.
while (current.CameFrom != null)
{
path.Add(current.CameFrom);
current = current.CameFrom;
}
// Reverse so the goal isn't at the 0 index but is last node.
path.Reverse();
LastFoundPath = path.ToArray();
return(LastFoundPath);
}
Finished = true;
return(LastFoundPath);
}
/// <summary>
/// Find a path within the grid.
/// </summary>
/// <param name="pathMemory">The memory to fill with the path output.</param>
/// <param name="start">The location to start pathing from</param>
/// <param name="end">The location to path to.</param>
/// <param name="diagonalMovement">Whether diagonal movement is allowed.</param>
public void FindPath(List <Vector2> pathMemory, Vector2 start, Vector2 end, bool diagonalMovement = false)
{
pathMemory.Clear();
AStarNode startNode = CreateNodeFromIfValid(start);
AStarNode endNode = CreateNodeFromIfValid(end);
if (startNode == null || endNode == null)
{
return; // Invalid path
}
_openSet.Clear();
_closedSet.Clear();
_openSet.Add(startNode);
// Reset cache.
foreach ((int _, AStarNode cachedNode) in _cache)
{
cachedNode.CameFrom = null;
cachedNode.G = 0;
cachedNode.H = 0;
}
// Loop while there are nodes in the open set, if there are none left and a path hasn't been found then there is no path.
while (_openSet.Count > 0)
{
// Get the node with the lowest score. (F)
AStarNode current = null;
var closestF = 0;
foreach (AStarNode node in _openSet)
{
if (current != null && closestF <= node.F)
{
continue;
}
current = node;
closestF = node.F;
}
if (current == null)
{
break; // Should never occur.
}
// Check if the current node is the end, in which case the path has been found.
if (current.Equals(endNode))
{
pathMemory.Add(endNode.Location);
// Trace the path backwards.
AStarNode trace = endNode;
while (trace.CameFrom != null)
{
AStarNode nextNode = trace.CameFrom;
pathMemory.Add(nextNode.Location);
trace = nextNode;
}
// Reverse so the goal isn't at the 0 index but is last node.
pathMemory.Reverse();
return;
}
// Update sets.
_openSet.Remove(current);
_closedSet.Add(current);
// Get neighbors of current.
GetNeighbors(_neighbors, current, diagonalMovement);
// Apply heuristics to neighbors.
for (var i = 0; i < _neighbors.Count; i++)
{
AStarNode node = _neighbors[i];
node.H = Heuristic(node, endNode, current);
}
_neighbors.Sort();
for (var i = 0; i < _neighbors.Count; i++)
{
AStarNode neighbor = _neighbors[i];
if (neighbor.H < 0)
{
continue;
}
// Check if the neighbor is done with, in which case we skip.
if (_closedSet.Contains(neighbor))
{
continue;
}
// Get the tentative distance between the current and the neighbor. Using 1 as distance.
int tentativeG = DistanceBetweenNodes(current, neighbor, endNode);
// Check if the neighbor is being evaluated.
if (_openSet.Contains(neighbor))
{
// Check if we have found a more efficient way to the neighbor node.
if (tentativeG < neighbor.G)
{
neighbor.G = tentativeG;
}
else
{
continue;
}
}
else
{
// Assign the calculated distance and add the node to the open set.
neighbor.G = tentativeG;
_openSet.Add(neighbor);
}
neighbor.CameFrom = current;
}
}
}