public Node FindPath(Node[,] map, Node start, Node target)
{
List <Node> openNodes = new List <Node>();
List <Node> closedNodes = new List <Node>();
start.previous = null;
openNodes.Add(start);
while (openNodes.Count > 0)
{
Node currentNode = openNodes[0];
for (int i = 0; i < openNodes.Count; i++)
{
if (openNodes[i].f < currentNode.f)
{
currentNode = openNodes[i];
}
if (openNodes[i].f == currentNode.f)
{
if (openNodes[i].g > currentNode.g)
{
currentNode = openNodes[i];
}
}
}
if (currentNode == target)
{
return(currentNode);
}
openNodes.Remove(currentNode);
closedNodes.Add(currentNode);
foreach (Node neighbour in currentNode.GetNeighbours(map))
{
if (!closedNodes.Contains(neighbour))
{
int tempG = currentNode.g + Heuristic(neighbour, currentNode);
if (!openNodes.Contains(neighbour))
{
openNodes.Add(neighbour);
}
else if (tempG >= neighbour.g)
{
continue;
}
neighbour.g = tempG;
neighbour.h = Heuristic(neighbour, target);
neighbour.f = neighbour.g + neighbour.h;
neighbour.previous = currentNode;
}
}
}
return(null);
}
