IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { List <Node> path = new List <Node>(); bool pathSuccess = false; Node startNode = grid.GetNodeFromWorldPoint(startPos); Node targetNode = grid.GetNodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(grid.MaxSize); HashSet <Node> closeSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closeSet.Add(currentNode); if (currentNode == targetNode) { path = RetracePath(startNode, targetNode); pathSuccess = true; break; } List <Node> neighbourNodes = grid.GetNeighbourNodes(currentNode); foreach (var neighbour in neighbourNodes) { if (neighbour.walkable == false || closeSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } } yield return(null); if (pathSuccess) { Vector3[] waypoints = GenerateWayPoints(path); requestManager.FinishedProcessPath(waypoints, pathSuccess); } requestManager.FinishedProcessPath(new Vector3[0], pathSuccess); }
// finds a path from the start to the end IEnumerator FindPath(Vector3 start, Vector3 end) { Stopwatch sw = new Stopwatch(); // stopwatch for efficiency diagnostics sw.Start(); Vector3[] waypoints = new Vector3[0]; // the waypoints on the path - will be properly set up later bool pathSuccess = false; // we have not successfully found a path yet // get the nodes on the grid based on the world positions TileNode startNode = grid.NodeFromWorldPosition(start); TileNode targetNode = grid.NodeFromWorldPosition(end); // only pathfind if both the start and end are walkable if (startNode.walkable && targetNode.walkable) { //List<TileNode> openSet = new List<TileNode>(); // what nodes are available to check Heap <TileNode> openSet = new Heap <TileNode>(grid.MaxSize); // use a Binary Heap instead of List so that we only compare to parent rather than iterate everything HashSet <TileNode> closedSet = new HashSet <TileNode>(); // what nodes have been checked openSet.Add(startNode); // add the start node while (openSet.Count > 0) { TileNode currentNode = openSet.PopFirst(); // openSet[0] // very slow because it has to iterate through everything //for(int i = 1; i < openSet.Count; i++) //{ // if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost) // { // currentNode = openSet[i]; // } //} //openSet.Remove(currentNode); closedSet.Add(currentNode); // add the current node to the closed set so we can't check it again // if we find the node we want, then retrace the path and exit the loop because that's our path if (currentNode == targetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + "ms"); pathSuccess = true; break; } // check each neighbor foreach (TileNode neighbor in grid.GetNeighbors(currentNode)) { // if the neighbor is not walkable (value of 1) or it's in the closed set, don't check it if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } // get path from the current node to the neighbor. Also add weight value here int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor) + neighbor.movementPenalty; // add the current node's gCost to find out how far it is from the start in total // if the new path to the neighbor is shorter or the the neighbor isn't in the open set yet if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { // set up the neighbor neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); // heuristic distance to the target neighbor.parent = currentNode; // set the neighbor's parent to the currentNode (this allows us to traverse the path later) // add the neighbor if it's not in the open set if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else // if it is in the open set, update it's values so we get the correct path { openSet.UpdateItem(neighbor); } } } } } yield return(null); // wait a frame then try again // if we found a path, set the waypoints to the retrace if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; // in case we move the target } requestManager.FinishedProcessPath(waypoints, pathSuccess); // give the requestmanager the data }
IEnumerator PathFinding(Vector3 StartPos, Vector3 TargetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool PathSuccess = false; grid = GetComponent <Grid>(); Node StartNode = grid.GetNodePositionFromWorld(StartPos); Node TargetNode = grid.GetNodePositionFromWorld(TargetPos); if (StartNode.walkable && TargetNode.walkable) { //List<Node> openList = new List<Node> (); Heap <Node> openList = new Heap <Node> (grid.MaxSize); HashSet <Node> closeList = new HashSet <Node> (); openList.Add(StartNode); while (openList.Count > 0) { //use loop to find the minium fCost instead of heap /*Node currentNode = openList [0]; * * for (int i = 1; i < openList.Count; i++) { * if (openList [i].fCost < currentNode.fCost || (openList [i].fCost == currentNode.fCost && openList [i].hCost < currentNode.hCost)) { * currentNode = openList [i]; * } * * }*/ //Node currentNode = openList [0]; //openList.Remove (currentNode); Node currentNode = openList.RemoveFirst(); closeList.Add(currentNode); if (currentNode == TargetNode) { if (ShowPathFindingTime) { sw.Stop(); print("Path Found: " + sw.ElapsedMilliseconds + " ms"); } PathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closeList.Contains(neighbour)) { continue; } int MoveCostToNeighbour = currentNode.gCost + GetMoveCost(currentNode, neighbour) + neighbour.movePenality; if (MoveCostToNeighbour < neighbour.gCost || !openList.Contains(neighbour)) { neighbour.gCost = MoveCostToNeighbour; neighbour.hCost = GetManHattanDiatance(neighbour, TargetNode); //neighbour.hCost = GetMoveCost (neighbour, TargetNode); neighbour.parent = currentNode; if (!openList.Contains(neighbour)) { openList.Add(neighbour); } else { openList.Update(neighbour); } } } } yield return(null); if (PathSuccess) { waypoints = getPath(StartNode, TargetNode); closeList.Clear(); } requestManger.FinishedProcessPath(waypoints, PathSuccess); } else { print("Start Node or End Node is unwalkable."); } }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoint = new Vector3[0]; bool pathSuccess = false; Node startNode = grid.NodeFromWorldPoint(startPos); Node targetNode = grid.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { List <Node> openSet = new List <Node>(); HashSet <Node> closeSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet[0]; for (int i = 0; i < openSet.Count; i++) { if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost) { currentNode = openSet[i]; } } openSet.Remove(currentNode); closeSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closeSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour); if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoint = RetracePath(startNode, targetNode); requestManager.FinishedProcessPath(waypoint, pathSuccess); } }