IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; QuadTreeNode startNode = grid.locateNode(startPos); QuadTreeNode targetNode = grid.locateNode(targetPos); //if ((targetNode.danger < 1 && targetNode.walkable)) if (startNode.walkable && targetNode.walkable) { ListHeap<QuadTreeNode> openSet = new ListHeap<QuadTreeNode>(); HashSet<QuadTreeNode> closedSet = new HashSet<QuadTreeNode>(); openSet.Add(startNode); while (openSet.Count > 0) { QuadTreeNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); // print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (KeyValuePair<QuadTreeNode, float> entry in currentNode.neighbours) { //if (neighbour.danger > 0 || !neighbour.walkable || closedSet.Contains(neighbour)) QuadTreeNode neighbour = entry.Key; if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } float newMovementCostToNeighbour = currentNode.gCost + entry.Value; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = Vector3.Distance(neighbour.worldPosition,targetNode.worldPosition); neighbour.successor = currentNode; if (!openSet.Contains(neighbour)) openSet.Add(neighbour); else openSet.UpdateItem(neighbour); } } } } yield return null; if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; QuadTreeNode startNode = grid.locateNode(startPos); QuadTreeNode targetNode = grid.locateNode(targetPos); //if ((targetNode.danger < 1 && targetNode.walkable)) if (startNode.walkable && targetNode.walkable) { ListHeap <QuadTreeNode> openSet = new ListHeap <QuadTreeNode>(); HashSet <QuadTreeNode> closedSet = new HashSet <QuadTreeNode>(); openSet.Add(startNode); while (openSet.Count > 0) { QuadTreeNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); // print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; break; } foreach (KeyValuePair <QuadTreeNode, float> entry in currentNode.neighbours) { //if (neighbour.danger > 0 || !neighbour.walkable || closedSet.Contains(neighbour)) QuadTreeNode neighbour = entry.Key; if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } float newMovementCostToNeighbour = currentNode.gCost + entry.Value; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = Vector3.Distance(neighbour.worldPosition, targetNode.worldPosition); neighbour.successor = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }