/// <summary>
/// Find the quickest path from point 1 to point 2
/// </summary>
/// <param name="startPos">The start-position of the path</param>
/// <param name="targetPos">The target-position for the path</param>
/// <param name="grid">The grid that it should perform the search on.</param>
/// <returns></returns>
private IEnumerator FindThePath(Vector3 startPos, Vector3 targetPos, GridGraph grid)
{
if (grid.nodes == null || grid.nodes.Length == 0)
{
ScanGrid(grid);
}
if (grid.Scanning)
{
yield break;
}
float max = 300; //Maximum number of nodes, before canceling the path.
Path p = new Path(grid);
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPos(startPos);
Node targetNode = grid.NodeFromWorldPos(targetPos);
if (startNode == null || targetNode == null)
{
OnProccesingDone(p, false);
yield break;
}
if (startNode.Walkable && targetNode.Walkable)
{
List<Node> open = new List<Node>(grid.maxSize);
HashSet<Node> closed = new HashSet<Node>();
open.Add(startNode);
int cur = 0;
while (open.Count > 0)
{
Node currentNode = open[0];
open.RemoveAt(0);
closed.Add(currentNode);
cur++;
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (neighbour == null || !neighbour.Walkable || closed.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + grid.GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !open.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = grid.GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!open.Contains(neighbour))
open.Add(neighbour);
}
}
if (cur > max)
{
break;
}
}
}
if (pathSuccess)
{
p = grid.RetracePath(startNode, targetNode, grid);
}
OnProccesingDone(p, pathSuccess);
}
