IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) //If we can actually get the the target node { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); //Add the first node while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) //Are we here? { pathSuccess = true; //yes break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) //For every neighbour to our node { if (!neighbour.walkable || closedSet.Contains(neighbour)) //If the neighbour is walkable or we've already looked at it { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistanceCost(currentNode, neighbour); //Get the cost if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) //If this new cost is shorter or the neighbour isnt in the openset { neighbour.gCost = newMovementCostToNeighbour; //Set the gCost neighbour.hCost = GetDistanceCost(neighbour, targetNode); //Heauristic neighbour.parent = currentNode; //Set the tree/path if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition) { Stopwatch stopWatch = new Stopwatch(); stopWatch.Start(); Vector3[] wayPoints = new Vector3[0]; bool pathSucces = false; Node startNode = grid.NodeFromWorldPoint(startPosition); Node targetNode = grid.NodeFromWorldPoint(targetPosition); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { stopWatch.Stop(); UnityEngine.Debug.Log("Path found in " + stopWatch.ElapsedMilliseconds + "ms."); pathSucces = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (/*!neighbour.walkable ||*/ closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { // Calculate corner if (neighbour.gridX != currentNode.gridX && neighbour.gridY != currentNode.gridY) { UnityEngine.Debug.Log("Checking node: " + neighbour.worldPosition.ToString() + " | " + currentNode.worldPosition.ToString()); int gridX = neighbour.gridX + (neighbour.gridX > currentNode.gridX ? -1 : 1); int gridY = neighbour.gridY + (neighbour.gridY > currentNode.gridY ? -1 : 1); Node nodeA = grid.GetNode(gridX, currentNode.gridY); Node nodeB = grid.GetNode(currentNode.gridX, gridY); if (!nodeA.walkable || !nodeB.walkable) { continue; } } neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSucces) { wayPoints = RetracePath(startNode, targetNode); } pathfindingManager.FinishedProcessingPath(wayPoints, pathSucces); }