private Vector2[] CalculatePath(AStarNode startNode, AStarNode endNode)
{
var openList = new PolyNav.Heap <AStarNode>(1000);
var closedList = new HashSet <AStarNode>();
var success = false;
openList.Add(startNode);
while (openList.Count > 0)
{
var currentNode = openList.RemoveFirst();
if (currentNode == endNode)
{
success = true;
break;
}
closedList.Add(currentNode);
var links = currentNode.links;
for (var i = 0; i < links.Count; i++)
{
var neighbour = links[i];
if (closedList.Contains(neighbour))
{
continue;
}
var costToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (costToNeighbour < neighbour.gCost || !openList.Contains(neighbour))
{
neighbour.gCost = costToNeighbour;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
openList.UpdateItem(neighbour);
}
}
}
}
if (success)
{ //Retrace Path if one exists
var path = new List <Vector2>();
var currentNode = endNode;
while (currentNode != startNode)
{
path.Add(NodeToPosition(currentNode));
currentNode = currentNode.parent;
}
path.Add(NodeToPosition(startNode));
path.Reverse();
return(path.ToArray());
}
return(null);
}