public List <GridNode> FindPathActual(GridNode start, GridNode target)
{
List <GridNode> foundPath = new List <GridNode>();
//Two lists, one for nodes that need to be checked and one for nodes that have already been checked
List <GridNode> openSet = new List <GridNode>();
HashSet <GridNode> closedSet = new HashSet <GridNode>();
//Start adding to the open set
openSet.Add(start);
while (openSet.Count > 0)
{
GridNode currentNode = openSet[0];
for (int i = 0; i < openSet.Count; i++)
{
if (openSet[i].fCost < currentNode.fCost || (openSet[i].fCost
== currentNode.fCost && openSet[i].hCost < currentNode.hCost))
{
if (!currentNode.Equals(openSet[i]))
{
currentNode = openSet[i];
}
}
}
//remove current node from the open set and add to the closed set
openSet.Remove(currentNode);
closedSet.Add(currentNode);
//if the current node is the target node
if (currentNode.Equals(target))
{
foundPath = RetracePath(start, currentNode);
break;
}
foreach (GridNode neighbour in GetNeighbours(currentNode, true))
{
if (!closedSet.Contains(neighbour))
{
float newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
//calculate the new costs
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, target);
//Assign parent node
neighbour.parentNode = currentNode;
//Add neighbour node to open set
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
return(foundPath);
}
