public void FindPath(Vector3 startPos, Vector3 endPos)
{
Node startNode = grid.NodeFromWorld(startPos);
Node endNode = grid.NodeFromWorld(endPos);
Heap <Node> OpenNodes = new Heap <Node>(grid.MaxSize);
HashSet <Node> ClosedNodes = new HashSet <Node>();
OpenNodes.Add(startNode);
while (OpenNodes.Count > 0)
{
Node currentNode = OpenNodes.RemoveFirst();
//Debug.Log("ClosedNodes: " + ClosedNodes.Count + " OpenNodes: " + OpenNodes.Count + "currentNode: " + currentNode.gridX +"," + currentNode.gridY);
ClosedNodes.Add(currentNode);
if (currentNode == endNode)
{
GetFinalPath(startNode, endNode);
}
foreach (Node NeighbourNode in grid.GetNeighbourNodes(currentNode))
{
//Debug.Log("NeighbourNode");
if (NeighbourNode.isWall == true || ClosedNodes.Contains(NeighbourNode))
{
//Debug.Log("is wall");
continue;
}
int MoveCost = currentNode.gCost + getManhattanDistance(currentNode, NeighbourNode);
//Debug.Log("MoveCost: " + MoveCost + " gCost: " + currentNode.gCost + "MH Distance to neighbour: " + getManhattanDistance(currentNode, NeighbourNode));
//Debug.Log("Neighbour gCost: " + NeighbourNode.gCost);
if (MoveCost < NeighbourNode.gCost || !OpenNodes.Contains(NeighbourNode))
{
NeighbourNode.gCost = MoveCost;
NeighbourNode.hCost = getManhattanDistance(NeighbourNode, endNode);
NeighbourNode.parent = currentNode;
if (OpenNodes.Contains(NeighbourNode) == false)
{
OpenNodes.Add(NeighbourNode);
}
}
}
}
}