public List <Node> FindPath(Vector2 startPosition, Vector2 targetPosition, bool allowDiagonalMovement = false)
{
// Perform all pathing operations on a clone of currrent A* grid
nodes = navigationMesh.CloneGrid();
gridWidth = navigationMesh.GridWidth;
gridHeight = navigationMesh.GridHeight;
// Find the nodes closest to our destination positions
// if these are off the grid, will return the edge of the grid as close as it can get
var startNodePosition = navigationMesh.FindNearestNodeIndex(startPosition);
var targetNodePosition = navigationMesh.FindNearestNodeIndex(targetPosition);
var startNode = nodes[startNodePosition.ix][startNodePosition.iy];
// For the target node, if the destination is a solid node, find the nearest non-solid neighbor node instead
var targetNode = FindNearestOpenNode(nodes[targetNodePosition.ix][targetNodePosition.iy]);
// The "open list" is the list of nodes that we need to visit
var openList = new List <Node>()
{
startNode
};
// The "closed set" is the list of nodes that we have visited already
var closedList = new List <Node>();
// Loop until we have no more nodes to visit
while (openList.Count > 0)
{
// Grab whatever the next node on the list is
var currentNode = openList.First();
// Iterate through the open list starting from the second element to figure out what node we want to visit next
// If there is no other element, this whole thing is skipped
foreach (var node in openList.Skip(1))
{
// if this node appears to get us to our destination quicker, visit that boy first
if (node.FCost <= currentNode.FCost && node.HCost < currentNode.HCost)
{
currentNode = node;
}
}
// mark this node as visited, remove it from the open list and add it to the closed list
openList.Remove(currentNode);
closedList.Add(currentNode);
// if we find our target node
if (currentNode == targetNode)
{
// Crawl backwards to retrieve the total path
return(TraceFinalPath(startNode, targetNode));
}
foreach (Node neighbor in FindNeighborNodes(currentNode, allowDiagonalMovement))
{
// Ignore any solid or previously visited nodes
if (neighbor.IsSolid || closedList.Contains(neighbor))
{
continue;
}
// For this implementation of A*, the GCost is the linear distance between node indices
// Add the distance between currentNode and this particular neighbor
var moveCost = currentNode.GCost + GetDistance(currentNode, neighbor);
// Check if this neighbor node should be where we visit next
if (moveCost < neighbor.GCost || !openList.Contains(neighbor))
{
neighbor.GCost = moveCost;
neighbor.HCost = GetDistance(neighbor, targetNode);
neighbor.Parent = currentNode;
if (!openList.Contains(neighbor))
{
openList.Add(neighbor);
}
}
}
}
// If we managed to get here, it means we have visited every node and could not find a path.
// Return an empty list, and leave it up to the implementor to decide what to do
return(new List <Node>());
}
