public List <Node> FindRange(Vector3 startPosition, int range) { Node startingNode = grid.GetNodeFromWorldPosition(startPosition); List <Node> inRange = new List <Node>(); Queue <Node> BFSQueue = new Queue <Node>(); bool[,,] searched = new bool[grid.maxX, grid.maxY, grid.maxZ]; BFSQueue.Enqueue(startingNode); while (BFSQueue.Count > 0) { Node current = BFSQueue.Dequeue(); inRange.Add(current); List <Node> neighbors = grid.GetNodeNeighbors(current); foreach (Node neighbor in neighbors) { if (range - GetDistance(startingNode, neighbor) > 0 && !searched[neighbor.x, neighbor.y, neighbor.z]) { BFSQueue.Enqueue(neighbor); searched[neighbor.x, neighbor.y, neighbor.z] = true; } } } return(inRange); }
/// <summary> /// Finds a path between two points on the grid. /// </summary> /// <param name="from">initial position</param> /// <param name="to">target position</param> /// <returns>path array</returns> private IEnumerator FindPath(Vector3 from, Vector3 to, Action <Vector3[], bool> callback) { /* F cost = G cost + H cost * G cost = distance from origin to node * H cost = distance from node to target */ // Open set => Nodes that are yet to be evaluated. BinaryHeap <Node> openSet = new BinaryHeap <Node>(Grid.NodeCount); // Closes set => Nodes that have been claimed by another. HashSet <Node> closedSet = new HashSet <Node>(); Node initialNode = Grid.GetNodeFromWorldPosition(from); Node targetNode = Grid.GetNodeFromWorldPosition(to); // The initial node also must pass through the first iteration. //initialNode.HCost = GetDistance(initialNode, targetNode); openSet.Add(initialNode); while (openSet.Count > 0) { // Removing and getting the node with the lowest F cost (highest G cost in ties). Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); // Returning the path if the node is the target. if (currentNode.Equals(targetNode)) { callback.Invoke(RetracePath(initialNode, targetNode), true); yield break; } // Getting the surrounding nodes. Node[] neighbors = Grid.GetNeighbors(currentNode); // Deciding what to do with each neighbor. foreach (var neighbor in neighbors) { if (!neighbor.Walkable) { continue; } if (closedSet.Contains(neighbor)) { continue; } /* newGCost refers to the new hypothetical G cost of the neighbor * (it could have been assigned by another node and now be lower by this path).*/ int newGCost = GetDistance(currentNode, neighbor) + currentNode.GCost + neighbor.Weight; bool openSetHasNotNeighbor = !openSet.Contains(neighbor); if (openSetHasNotNeighbor || newGCost < neighbor.GCost) { // Parent node is used to retrace the path once it has been found. neighbor.ParentNode = currentNode; neighbor.GCost = newGCost; neighbor.HCost = GetDistance(neighbor, targetNode); if (openSetHasNotNeighbor) { openSet.Add(neighbor); } else { /* The reason for this line is that if the neighbor is already in the * open set, it certainly got its G cost lowered and must me sorted * up in the binary heap.*/ openSet.SortUp(neighbor); } } } } yield return(null); callback.Invoke(null, false); }