public Stack<Node> FindPath(int xs, int ys, int xt, int yt, ICollection<Zombie> zombies, IBot bot) { int maxSearch = 2560; int currentSearch = 0; bool isStartingSquare = true; isOpen = new bool[bot.Room.Width, bot.Room.Height]; isClosed = new bool[bot.Room.Width, bot.Room.Height]; gValues = new int[bot.Room.Width, bot.Room.Height]; debug = new Queue<Node>(); open = new PathHeap(); search = new LowHPathHeap(); //open = new SortedSet<Node>(); //closed = new HashSet<Node>(); Node start = new Node(xs, ys); start.H = Heuristic(xs, ys, xt, yt); Node target = new Node(xt, yt); isOpen[start.x, start.y] = true; open.Add(start); search.Add(start); while (!open.IsEmpty() && currentSearch < maxSearch) //while (open.Count > 0) { Node current = open.GetRemoveFirst(); //Node current = open.First(); //open.Remove(current); isOpen[current.x, current.y] = false; isClosed[current.x, current.y] = true; if (current.Equals(target)) return GetPath(current, bot); for (int i = 4; i < 8; i++) { int x = adjacentNodes[i].x + current.x; int y = adjacentNodes[i].y + current.y; //Console.WriteLine("X:" + x + " Y:" + y + " " + room.GetMapBlock(x, y)); int id = bot.Room.BlockMap.getForegroundBlockIdFast(x, y); if (x >= 0 && x <= bot.Room.Width - 1 && y >= 0 && y <= bot.Room.Height - 1 && (isStartingSquare ? !isZombie(x, y, zombies): true)) { int totalAddCost = adjacentNodes[i].cost;// + (room.GetMapBlock(x, y) != 0 ? wallCost : 0); Node baby = new Node(x, y); if (isClosed[baby.x, baby.y]) continue; if (!isOpen[baby.x, baby.y]) { int addg = totalAddCost; baby.G = current.G + addg; gValues[baby.x, baby.y] = baby.G; baby.H = Heuristic(baby.x, baby.y, target.x, target.y); baby.Mother = current; open.Add(baby); //bot.Connection.Send(bot.Room.WorldKey, 0, baby.x, baby.y, 9); //System.Threading.Thread.Sleep(10); isOpen[baby.x, baby.y] = true; //debug.Enqueue(baby); search.Add(baby); ++currentSearch; } else { int addg = totalAddCost; if (current.G + addg < gValues[baby.x, baby.y]) { //Path to that Node is better, switch parents! baby.Mother = current; } } } } isStartingSquare = false; } //Target was not found, use closest search node if (!search.IsEmpty()) return GetPath(search.GetRemoveFirst(), bot); return null; }
public IReadOnlyList <Node> FindShortestPath(Network network, Node startNode, Node endNode) { // the heap var distancesSoFar = new PathHeap(); // dictionary to track nodes and distances to them var finalDistances = new Dictionary <Node, int>(); // paths from start node to every other var paths = new Dictionary <Node, IReadOnlyList <Node> >(); // add the start node to the heap distancesSoFar.AddToHeap(startNode); while (distancesSoFar.Any()) { // find the closes node var minNode = distancesSoFar.RemoveMin(); // lock it down, we have been here finalDistances.Add(minNode, minNode.Value); // add it to the list of paths if (!paths.ContainsKey(minNode)) { paths.Add(minNode, new List <Node>() { minNode }); } // we have found the shortest path to the end node if (minNode == endNode) { return(paths[endNode]); } foreach (var node in network.GetNodeConnections(minNode)) { // only need the nodes that are not over and done if (finalDistances.ContainsKey(node)) { continue; } var value = Math.Max(finalDistances[minNode], network.GetDistance(minNode, node)); if (node.Index < 0) { // if the node is not in the heap, add it to both heap and dictionary node.Value = value; distancesSoFar.AddToHeap(node); paths.Add(node, new List <Node>(paths[minNode]) { node }); } else if (value < node.Value) { // if it is in the heap and the new value is smaller // change it's value and balance the heap node.Value = value; distancesSoFar.UpHeapify(node.Index); paths[node] = new List <Node>(paths[minNode]) { node }; } } } if (!paths.ContainsKey(endNode)) { return(new Node[0]); } return(paths[endNode]); }
private IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Stopwatch sw = new Stopwatch(); sw.Start(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; PathNode startNode = pathGrid.NodeFromWorldPoint(startPos); PathNode targetNode = pathGrid.NodeFromWorldPoint(targetPos); if (startNode.Walkable && targetNode.Walkable) { PathHeap <PathNode> openSet = new PathHeap <PathNode>(pathGrid.MaxSize); HashSet <PathNode> closedSet = new HashSet <PathNode>(); openSet.Add(startNode); while (openSet.Count > 0) { PathNode currentNode = openSet.RemoveFirstItem(); closedSet.Add(currentNode); if (currentNode == targetNode) { sw.Stop(); print("Path found: " + sw.ElapsedMilliseconds + " ms"); pathSuccess = true; //RetracePath(startNode, targetNode); //return; break; } foreach (PathNode neighbour in pathGrid.GetNeighbour(currentNode)) { if (neighbour == null) { continue; } if (!neighbour.Walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour) + neighbour.Hindrance; if (newMovementCostToNeighbour < neighbour.GCost || openSet.Contains(neighbour) == false) { neighbour.GCost = newMovementCostToNeighbour; neighbour.HCost = GetDistance(neighbour, targetNode); neighbour.Parent = currentNode; if (openSet.Contains(neighbour) == false) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } // List<PathNode> tempList = openSet; // HashSet<PathNode> tempList2 = closedSet; } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishProcessingPath(waypoints, pathSuccess); }