public void FindPath(GameObject startPos, GameObject endPos)
{
Node startNode = grid.WorldToGridCoordinates(startPos.transform);
Node endNode = grid.WorldToGridCoordinates(endPos.transform);
//List<Node> openSet = new List<Node>();
//List<Node> closedSet = new List<Node>();
//Change the lists to HEAPS
Heap <Node> openSet = new Heap <Node>(grid.grid.Length);
Heap <Node> closedSet = new Heap <Node>(grid.grid.Length);
openSet.Add(startNode);
while (openSet.Count > 0)
{
//Node currentNode = openSet[0];
//for (int i = 1; i < openSet.Count; i++)
//{
// if ((openSet[i].fCost < currentNode.fCost)
// ||
// (openSet[i].fCost == currentNode.fCost)
// &&
// (openSet[i].hCost < currentNode.hCost))
// {
// currentNode = openSet[i];
// }
//}
//openSet.Remove(currentNode);
//HEAP
currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == endNode)
{
RetracePath(startNode, endNode);
return;
}
List <Node> neighbours = grid.GetNeighbours(currentNode);
foreach (Node neighbour in neighbours)
{
if (neighbour.isBlocked || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCost = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCost < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCost;
neighbour.hCost = GetDistance(neighbour, endNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
}
