void FindPath(Vector3 start, Vector3 end)
{
Node startNode = m_Grid.NodeFromWorldPos(start);
Node endNode = m_Grid.NodeFromWorldPos(end);
//List<Node> openSet = new List<Node>();
MinHeap <Node> openSet = new MinHeap <Node>(m_Grid.GridSize);
List <Node> closedSet = new List <Node>();
openSet.Add(startNode);
//int fCost = 0;
while (openSet.Count > 0)
{
Node _currentNode = openSet.RemoveFirstItem();
//The following was replaced by heap
//Node _currentNode = openSet[0];
//for (int i = 1; i < openSet.Count; i++)
//{
// if (openSet[i].m_ifCost < _currentNode.m_ifCost || openSet[i].m_ifCost == _currentNode.m_ifCost)
// {
// _currentNode = openSet[i];
// }
//}
//openSet.Remove(_currentNode);
closedSet.Add(_currentNode);
if (_currentNode == endNode)
{
RetracePath(startNode, endNode);
return;
}
foreach (Node neighbour in m_Grid.GetNeighbour(_currentNode))
{
if (neighbour.m_isBlocked || closedSet.Contains(neighbour))
{
continue;
}
int NewMovementCost = _currentNode.m_iGCost + GetDistance(_currentNode, neighbour);
if (NewMovementCost < neighbour.m_iGCost || !openSet.Contains(neighbour))
{
neighbour.m_iGCost = NewMovementCost;
neighbour.m_iHCost = GetDistance(neighbour, endNode);
neighbour.m_Parent = _currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
// This was added when heap was implemented, Sorts upwards if openset contains neighbour node
openSet.UpdateItem(neighbour);
}
}
}
}
}
