/// <summary>
/// Searches path between start point and end point.
/// </summary>
/// <param name="start">Vector3 position of start point</param>
/// <param name="end">Vector3 position of end point</param>
/// <returns>Returns either null or retraced path between start and end points if path is found</returns>
private List <Node> FindPath(Vector3 start, Vector3 end)
{
Node startNode = m_Grid.NodeFromWorldPosition(start);
Node endNode = m_Grid.NodeFromWorldPosition(end);
Heap <Node> openSet = new Heap <Node>((m_Grid.m_iNodeAmountX * m_Grid.m_iNodeAmountY));
List <Node> closedSet = new List <Node>();
// Add start node directly to openSet.
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
currentNode.m_bProcessed = true;
// If current node is same as end node then path is retraced
// And processed nodes reset.
if (currentNode == endNode)
{
ResetProcessedNodes(closedSet);
return(RetracePath(startNode, endNode));
}
// Checks nodes neighbouring nodes.
foreach (Node neighbour in currentNode.m_aNeighbours)
{
// If node is blocked or is already already processed.
if (neighbour.m_bIsBlocked || neighbour.m_bProcessed)
{
continue;
}
// Calculates movement cost.
int NewMovementCost = currentNode.m_iGCost + GetDistance(currentNode, neighbour);
// Checks if current nodes movement cost is cheaper than next nodes or it isn't already contained in openSet.
if (NewMovementCost < neighbour.m_iGCost || !openSet.Contains(neighbour))
{
// If true, current node is set as a parent node, and G- & H-cost is calculated.
neighbour.m_iGCost = NewMovementCost;
neighbour.m_iHCost = GetDistance(neighbour, endNode);
neighbour.m_nParent = currentNode;
// And it is added to openSet if not already in it.
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
// if no path is found then return null and reset nodes.
ResetProcessedNodes(closedSet);
return(null);
}
