/// <summary>
/// A* algorithm
/// </summary>
/// <param name="startPos">Node at which to start the pathat</param>
/// <param name="targetPos">Target node</param>
void FindPath(Node startPos, Node targetPos)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startPos);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
// Path found
if (currentNode == targetPos)
{
RetracePath(startPos, targetPos);
return;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetPos);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
}
