/* Connects a node to another (by creating an edge between them)
*
* param: node - node to connect
* param: cost - the cost of the path between two nodes
*/
public void connect(AStarNode node, float cost)
{
// Create a weighted edge between this node and the specified node
AStarEdge edge = new AStarEdge(this, node, cost);
// Add the edge to this node's list of edges
this.neighbours.Add(edge);
// Add the edge to the other node's list of edges
node.neighbours.Add(edge);
}
/* Find the shortest path from a start to target position
*
* param: start - start position
* param: target - end position
* returns: List<Vector2> - list of positions corresponding to grid nodes
* of the shortest path between start and target, null if start or target
* not on the walkable part of the grid
*
* This method implements the A* algorithm. It maintains a list of open nodes (to
* which path from starting position has been found). At the beginning, only the
* starting node is added to the list of open nodes. In each iteration, connections from
* all open nodes are considered, and one that gives the smallest cost + guess is the next
* node chosen for travelling through. If the next node is not on open list, it gets added
* to the open list. This continues until the next node is the target node.
*
* The paths are maintained by saving recording in each node the prevoius node in the path.
* Tracing path of previous nodes from target to start node allows reconstruction of the path taken.
*
*/
public List <Vector2> ShortestPath(Vector2 start, Vector2 target)
{
// List of nodes that have a path to from the start node
List <AStarNode> open = new List <AStarNode> ();
// List of nodes that are dead ends - cannot take a new path from
// those nodes to the target
List <AStarNode> closed = new List <AStarNode> ();
//Find the closest node to the start position
AStarNode startNode = closestNode(start);
//Find the closest node to the target position
AStarNode targetNode = closestNode(target);
if (startNode == null || !startNode.walkable)
{
return(null);
}
if (targetNode == null || !targetNode.walkable)
{
return(null);
}
if (startNode == targetNode)
{
return(null);
}
//Reset the start node cost - clears the path links from the prevoius run of A*
startNode.reset();
//Compute the guess value - Euclidean distance from start to target
startNode.guess = Vector2.Distance(startNode.position, targetNode.position);
//Add the start node to the list of nodes to consider for taking on the next
//iteration
open.Add(startNode);
#if UNITY_EDITOR
// Create a list of examined nodes - debugging only */
gizmos = new List <AStarNode>();
gizmos.Add(startNode);
#endif
//Keep traversing the grid until get to the target node
while (true)
{
//Sort the list of open nodes according to path cost+guess - this assures
// that open nodes with smallest total cost will be considered first
open = open.OrderBy(x => x.cost + x.guess).ToList();
List <AStarNode> removeFromOpen = new List <AStarNode>();
AStarNode nextNode = null;
//Examine connections of each open node.
foreach (AStarNode node in open)
{
AStarEdge nextEdge = null;
//For each connection of the open node...
foreach (AStarEdge edge in node.neighbours)
{
AStarNode neighbour = edge.getNeighbour(node);
//Check if there is a new path to consider.
if (!closed.Contains(neighbour))
{
if (!open.Contains(neighbour))
{
nextEdge = edge;
break;
}
else
{
float newGuess = Vector2.Distance(neighbour.position, targetNode.position);
// If there is a new path to consider, compute its cost - save the edge
// that gives new path with the lowest cost
float newCost = node.cost + edge.cost;
if ((newCost + newGuess < neighbour.cost + neighbour.guess))
{
nextEdge = edge;
break;
}
}
}
}
//If no new paths were found, move the node from the opne nodes list
//to the closed list (it won't be considered for new paths)
if (nextEdge == null)
{
removeFromOpen.Add(node);
closed.Add(node);
}
else
{
//If a new path has been found, take it, and update the cost in the
//node
nextNode = nextEdge.getNeighbour(node);
#if UNITY_EDITOR
/* Save new noded to the list of examined nodes - debugging only */
if (!gizmos.Contains(nextNode))
{
gizmos.Add(nextNode);
}
#endif
float newGuess = Vector2.Distance(nextNode.position, targetNode.position);
float newCost = node.cost + nextEdge.cost;
nextNode.prev = node;
nextNode.cost = newCost;
nextNode.guess = newGuess;
//If the new path takes us to the target node, we are done - return
//the path taken to this node
if (nextNode == targetNode)
{
return(targetNode.getPath());
}
else
{
break;
}
}
}
if (nextNode == null)
{
break;
}
//If the next node is not on open nodes list, add it to open
//nodes list
if (!open.Contains(nextNode))
{
open.Add(nextNode);
}
//Clean up - remove any nodes that have been added to the closed
//node list from te open nodes list
foreach (AStarNode node in removeFromOpen)
{
open.Remove(node);
}
}
return(null);
}
