IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition) { Vector3[] waypoints = new Vector3[0]; bool success = false; Node startNode = grid.NodeFromWorldPoint(startPosition); Node targetNode = grid.NodeFromWorldPoint(targetPosition); 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) { success = true; break; } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable && neighbor != targetNode || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor); if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } yield return(null); if (success) { waypoints = TracePath(startNode, targetNode); } requestManager.FinishProcessing(waypoints, success); }
public void FindPath(PathRequest request, Action <PathResult> callback) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; Node startNode = this.nodeGrid.NodeFromWorldPoint(request.pathStart); Node targetNode = this.nodeGrid.NodeFromWorldPoint(request.pathEnd); if (!startNode.walkable) { List <Node> neighbors = nodeGrid.GetNeighbors(startNode); if (neighbors.Count > 0) { startNode = neighbors[0]; } } if (!targetNode.walkable) { List <Node> neighbors = nodeGrid.GetNeighbors(targetNode); if (neighbors.Count > 0) { targetNode = neighbors[0]; } } if (startNode.walkable && targetNode.walkable) { Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize); HashSet <Node> closedSet = new HashSet <Node>(); openSet.Add(startNode); while (openSet.Count > 0) { Node currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (Node neighbor in nodeGrid.GetNeighbors(currentNode)) { if (closedSet.Contains(neighbor)) { continue; } int movementCost = currentNode.gCost + this.GetDistance(currentNode, neighbor) + neighbor.weight; if (movementCost < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = movementCost; neighbor.hCost = this.GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } else { openSet.UpdateItem(neighbor); } } } } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } callback(new PathResult(waypoints, pathSuccess, request.callback)); }
/// <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); }
public static Coord[] FindPath(Coord startPos, IList <Coord> goalPositions) { bool singleTarget = goalPositions.Count == 1; bool success = false; HashSet <Node> goals = new HashSet <Node>(goalPositions.Select(NodeGrid.GetNode)); Node startNode = NodeGrid.GetNode(startPos); Node goalNode = NodeGrid.GetNode(goalPositions[0]); if (startNode != null && goalNode != null && goalNode.IsWalkable && goalNode != startNode) { Heap <Node> open = new Heap <Node>(NodeGrid.GridCount); HashSet <Node> closed = new HashSet <Node>(); open.Add(startNode); while (open.Count > 0) { Node current = open.RemoveFirst(); closed.Add(current); if (singleTarget && current == goalNode || !singleTarget && goals.Contains(current) /*current == goalNode*/) { goalNode = current; success = true; break; } foreach (Node neighbor in NodeGrid.GetNeighbors(current)) { if (!neighbor.IsWalkable || closed.Contains(neighbor)) { continue; } int newMovementCost = current.gCost + GetDistance(current, neighbor); if (newMovementCost < neighbor.gCost || !open.Contains(neighbor)) { neighbor.gCost = newMovementCost; neighbor.hCost = GetDistance(neighbor, goalNode); neighbor.parent = current; if (!open.Contains(neighbor)) { open.Add(neighbor); } else { open.UpdateItem(neighbor); } } } } } Coord[] waypoints = null; if (success) { waypoints = RetracePath(startNode, goalNode); } return(waypoints == null || waypoints.Length == 0 ? null : waypoints); }