C# PriorityQueue.pop方法代码示例

 void testPriorityQueue()
 {
     Vector2[] vs = { new Vector2(0, 1), new Vector2(0, 3), new Vector2(0, 4), new Vector2(0, 2), new Vector2(0, 5),Vector2.zero };
     Comparison<Vector2> cmp = (v1, v2) => {
         float dist = (Vector2.zero - v1).sqrMagnitude - (Vector2.zero - v2).sqrMagnitude;
         //Debug.LogFormat("v1:{0} v2:{1} D:{2}",v1,v2,dist);
         return Mathf.RoundToInt(dist);
     };
     PriorityQueue<Vector2> pq = new PriorityQueue<Vector2>(cmp);
     pq.push(vs);
     Debug.Log(pq.showBuffer());
     int i = 0;
     while (pq.Count != 0) {
         Debug.LogFormat("[{1}]:{0}",pq.pop(),i++);
     }
 }

	/**
	 * A coroutine that uses A* pathfinding to find an optimal path between the unit's
	 * 'targetTile' to the given tileToMoveTo
	 * Note: it does not use 'currentTile' since that can often be considered 'previousTile' as well
	 */
	public virtual IEnumerator moveTo(Tile tileToMoveTo, bool activelySetNewTarget = false) {
		// Avoid multiple calls to move the unit while the path is still being calculated
		if ((isCalculatingPath && !activelySetNewTarget) || isInBattle || tileToMoveTo == null) yield break;
		isCalculatingPath = true;
		Debug.Log("Calculating path to new tile");
	
		// Clear the previous path in case the user gave overriding commands
		targetPath.setNewTileQueue(new Queue<Tile>());
		
		// Set up the priority queue for our pathfinding algo: A*
		PriorityQueue priorityQueue = new PriorityQueue();
		priorityQueue.add(
			getNewPath(
				targetTile, 
				Vector2.Distance(targetTile.gameObject.transform.position, tileToMoveTo.gameObject.transform.position), 
				getTileCost(targetTile)
			)
		);
		// Keep track of which tiles (by instance id) we have already 'visited' to cut down on running time
		Dictionary<int, bool> visitedTiles = new Dictionary<int, bool>();
		visitedTiles[targetTile.GetInstanceID()] = true;
		
		// Continue to check and expand the first Path in the queue until we reach our target
		Path path;
		float startingTime = Time.time;
		while (true) {
			// If we have no paths left in the queue, then a solution is impossible
			if (priorityQueue.getCount() == 0) {
				Debug.Log("Could not find a path to target tile");
				isCalculatingPath = false;
				return false;
			}
		
			// Pop our next path and check if we have reached our target yet
			path = priorityQueue.pop();
			if (tileToMoveTo == path.getLastTileInPath()) {
				//Debug.Log("Found optimal path: ");
				//path.printPath();
				break;
			}
			
			// If we havent reached the target, expand on the currently popped path with all adjacent tile options
			foreach (Tile adjacentTile in mapManager.getAdjacentTiles(path.getLastTileInPath())) {
				// Only add it to the path if it is unoccuppied and we haven't already visited it yet
				if (visitedTiles.ContainsKey(adjacentTile.GetInstanceID()) || !canWalkTo(adjacentTile.transform.position)) continue;
				else visitedTiles[adjacentTile.GetInstanceID()] = true;
				
				// Add the tile to a deep copy of our original path
				//Debug.Log("Adding adjacentTile to path: " + adjacentTile.GetInstanceID() + " - " + adjacentTile.gameObject.transform.position.ToString());
				Path copiedPath = getNewPath(path);
				// Our A* heuristic is the straight line distance between our next tile and our target
				float heuristic = Vector2.Distance(adjacentTile.gameObject.transform.position, tileToMoveTo.gameObject.transform.position);
				
				// Add the full path back to our priority queue
				copiedPath.add(adjacentTile, heuristic, getTileCost(adjacentTile));
				priorityQueue.add(copiedPath);
			}
			
			// If 100 milliseconds have passed Yield the coroutine to let the rest of unity work for a bit (until the next frame)
			if (Time.time - startingTime > 0.1f) {
				yield return null;
				startingTime = Time.time;
			}
		}
		//Debug.Log("Found optimal path");
		setPath(path);
		
		// Finish the coroutine
		isCalculatingPath = false;
		return true;
	}

    //This is the method that will do A*. It returns a vector of locations to follow
    public LinkedList<Vector3> findPath(Vector3 start, Vector3 end)
    {
        LinkedList<Vector3> result = new LinkedList<Vector3> ();
        Node startNode = currentGraph.getNodeByLocation ((int)start.y, (int)start.x);
        startNode.rawCost = 0.0f;
        queue = new PriorityQueue (startNode);
        while (!queue.isEmpty()) {
            //The A* magic happens here
            Node minNode = queue.pop ();
            //if this is our ending node, stop pathfinding and form our full path on the graph
            if (minNode == currentGraph.getNodeByLocation ((int) end.y, (int) end.x)) {
                //Here we form the path depending
                Node currentNode = minNode;

                while (currentNode != null) {
                    result.AddFirst(new Vector3(currentNode.widthPos, currentNode.heightPos));
                    currentNode = currentNode.parent;
                }
                resetGraph();
                return result;
            }
            //else, we need to update our priority queue, etc.
            else {
                float currentRaw = minNode.rawCost;
                foreach (Node neighbor in minNode.edges.Values) {
                    if (queue.isVisited(neighbor)) {
                        float oldRaw = neighbor.rawCost;
                        float newRaw = currentRaw + Vector2.Distance(new Vector2(minNode.widthPos, minNode.heightPos), new Vector2(neighbor.widthPos, neighbor.heightPos));
                        if (newRaw < oldRaw) {
                            neighbor.rawCost = newRaw;
                            neighbor.parent = minNode;
                        }
                    } else {
                        neighbor.rawCost = currentRaw + Vector2.Distance(new Vector2(minNode.widthPos, minNode.heightPos), new Vector2(neighbor.widthPos, neighbor.heightPos));
                        neighbor.parent = minNode;
                        queue.insert(neighbor);
                    }
                }
            }
        }
        resetGraph ();
        return result;
    }