/// <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); }