C# PriorityQueue.dequeue方法代码示例

        // <summary>
        // Given two Node objects, determines a path between the startNode and the endNode
        // </summary>
        // <param name="startNode"> the start Node </param>
        // <param name="endNode"> the end Node </param>
        public void determinePath(GenerateGraph.Node startNode, GenerateGraph.Node endNode)
        {
            if (startNode == null || endNode == null)
                return;
            PriorityQueue<GenerateGraph.Node> pq = new PriorityQueue<GenerateGraph.Node>(map.nodes.Count);
            pq.queue(startNode);
            Dictionary<GenerateGraph.Node, GenerateGraph.Node> came_from = new Dictionary<GenerateGraph.Node, GenerateGraph.Node>();
            Dictionary<GenerateGraph.Node, float> cost_so_far = new Dictionary<GenerateGraph.Node, float> ();
            came_from.Add(startNode, null);
            cost_so_far.Add (startNode, 0);

            Dictionary<GenerateGraph.Node, int> nodeToId = new Dictionary<GenerateGraph.Node, int>();
            for (int i = 0; i < map.nodes.Count; i++) {
                nodeToId.Add (map.nodes[i], i);
            }
            GenerateGraph.Node current;

            while (pq.getSize() > 0) {
                current = pq.dequeue();
                print(current.point.ToString());
                print("Neighbor Count: " + current.neighbors.Count);
                if (current.Equals(endNode)) {
                    print ("True");
                    break;
                }

                for (int i = 0; i < current.neighbors.Count; i++) {
                    print("Current Neighbor: " + current.neighbors[i].point.ToString());
                    float new_cost = cost_so_far[current] +
                        distanceBetweenNodes(current, current.neighbors[i]);
                    if (cost_so_far.ContainsKey(current.neighbors[i]) == false ||
                        new_cost < cost_so_far[current.neighbors[i]]) {
                        cost_so_far[current.neighbors[i]] = new_cost;
                        current.neighbors[i].priority = new_cost;
                        pq.queue(current.neighbors[i]);
                        came_from[current.neighbors[i]] = current;
                    }
                }
            }

            //Put nodes of the path into the list
            path = new List<GenerateGraph.Node> ();
            GenerateGraph.Node currentNode = endNode;
            path.Add (currentNode);
            while (currentNode.Equals(startNode) == false) {
                currentNode = came_from[currentNode];
                path.Add (currentNode);
            }

            path.Reverse();
            print ("Path Nodes: ");
            for (int i = 0; i < path.Count; i++) {
                print(nodeToId[path[i]] + "\n");
            }
        }

        public void TestEnqueueDequeue()
        {
            PriorityQueue<int> queue = new PriorityQueue<int>();
            queue.enqueueWithPriority (16,4);
            queue.enqueueWithPriority (14,5);
            queue.enqueueWithPriority (25,3);
            queue.enqueueWithPriority (17,4);

            Assert.AreEqual (queue.dequeue (), 14);
            Assert.AreEqual (queue.dequeue (), 16);
            Assert.AreEqual (queue.dequeue (), 17);
            Assert.AreEqual (queue.dequeue (), 25);

            queue.enqueueWithPriority(11,1);

            Assert.AreEqual (queue.dequeue (), 11);

            try {
                Console.Out.WriteLine("Attempting to dequeue an empty queue.");
                queue.dequeue();
                Console.Error.WriteLine("FAIL: System.InvalidOperationExcepti"+
                             "on was not thrown when dequeing an empty queue");
                Assert.IsTrue(false);
            } catch (System.InvalidOperationException e)
            {
                Console.Out.WriteLine("PASS: System.InvalidOperationException"+
                                            " thrown as expected: "+e.Message);
                Assert.IsTrue(true,"Exception thrown");
            }
        }

    // <summary>
    // Performs an A* traversal from the start node to the end node; however, the end node
    // only provides a direction for the path finding, as once the current node in the
    // traversal is pathLength away from the start node, the traversal is complete
    // </summary>
    protected override void ThreadFunction()
    {
        PriorityQueue<Node> open = new PriorityQueue<Node> (PathPlanningDataStructures.graph.Size ());
        Dictionary<Node, Node> came_from = new Dictionary<Node, Node> ();
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float> ();
        came_from.Add (startNode, null);
        cost_so_far.Add (startNode, 0);
        open.queue (PathPlanningDataStructures.heuristic.Estimate (startNode, useItemReduction),
            startNode);
        while (open.getSize() > 0) {
            Node current = open.dequeue ();

            if (current.Equals (PathPlanningDataStructures.graph.endNode) ||
                Node.distanceBetweenNodes (startNode, current) >= pathLength) {
                if (came_from[current] == null) {
                    came_from[current] = startNode;
                }
                destinationNode = current;
                break;
            }

            foreach (Node n in current.neighbors) {
                float graph_cost = cost_so_far [current] + Node.distanceBetweenNodes (current, n);
                if ((cost_so_far.ContainsKey (n) == false || graph_cost < cost_so_far [n]) &&
                closedNodes.Contains(n.position) == false) {
                    cost_so_far [n] = graph_cost;
                    float priority = graph_cost +
                        PathPlanningDataStructures.heuristic.Estimate (n, useItemReduction);
                    open.queue (priority, n);
                    came_from [n] = current;
                }
            }
        }

        //Put nodes of the path into the list
        pathWayPoints = new List<Vector3> ();
        Node currentNode = destinationNode;
        pathWayPoints.Add (currentNode.position);
        lock (PathPlanningDataStructures.globalLock) {
            PathPlanningDataStructures.nodeToCount [currentNode.position] += 1;
        }
        while (currentNode.Equals(startNode) == false) {
            currentNode = came_from [currentNode];
            pathWayPoints.Add (currentNode.position);
            lock (PathPlanningDataStructures.globalLock) {
                PathPlanningDataStructures.nodeToCount [currentNode.position] += 1;
            }
        }
        pathWayPoints.Reverse ();
    }

    public static List<Node> GetPath(Vector3 start)
    {
        Node startNode = graph.getClosestNode(start);
        PriorityQueue<Node> open = new PriorityQueue<Node>(graph.Size());
        HashSet<Node> closed = new HashSet<Node>();
        Dictionary<Node, Node> came_from = new Dictionary<Node, Node>();
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float>();
        came_from.Add(startNode, null);
        cost_so_far.Add(startNode, 0);
        open.queue(heuristic.Estimate(startNode), startNode);

        while (open.getSize() > 0) {
            Node current = open.dequeue();

            if (current.Equals(graph.endNode)) {
                break;
            }

            foreach (Node n in current.neighbors) {

                float graph_cost = cost_so_far[current] + Node.distanceBetweenNodes(current, n);

                if (cost_so_far.ContainsKey(n) == false ||  graph_cost < cost_so_far[n]) {
                    cost_so_far[n] = graph_cost;
                    float priority = graph_cost + heuristic.Estimate(n);
                    open.queue(priority, n);
                    came_from[n] = current;
                }
            }
        }

        //Put nodes of the path into the list
        List<Node> path = new List<Node> ();
        Node currentNode = graph.endNode;
        path.Add (currentNode);
        while (currentNode.Equals(startNode) == false) {
            currentNode = came_from[currentNode];
            path.Add (currentNode);
        }
        path.Reverse();
        return path;
    }

    public HeuristicD(GenerateGraph graph)
    {
        heuristicCost = new Dictionary<Node, float>();

        PriorityQueue<Node> pq = new PriorityQueue<Node>(graph.Size ());
        Dictionary<Node, float> cost_so_far = new Dictionary<Node, float> ();
        pq.queue(0.0f, graph.endNode);
        cost_so_far.Add (graph.endNode, 0.0f);
        while (pq.getSize() > 0) {
            Node current = pq.dequeue();
            heuristicCost[current] = cost_so_far[current];
            for (int i = 0; i < current.neighbors.Count; i++) {
                float new_cost = cost_so_far[current] + Node.distanceBetweenNodes(current, current.neighbors[i]);
                if (!cost_so_far.ContainsKey(current.neighbors[i]) || new_cost < cost_so_far[current.neighbors[i]]) {
                    cost_so_far[current.neighbors[i]] = new_cost;
                    pq.queue(new_cost, current.neighbors[i]);
                }
            }
        }
    }

 public void returnNodes(PriorityQueue q)
 {
     while(q.Count != 0){
         returnNode(q.dequeue());
     }
 }

    /// <summary>
    /// Performs A* search to find optimal path between two tiles.
    /// </summary>
    /// <returns>
    /// The star search.
    /// </returns>
    /// <param name='start'>
    /// Start.
    /// </param>
    /// <param name='finish'>
    /// Finish.
    /// </param>
    protected List<HexTile> AStarSearch(SearchNode start, SearchNode finish)
    {
        // Openlist
        PriorityQueue openList = new PriorityQueue();
        // Closed List
        List<SearchNode> closedList = new List<SearchNode>();
        // Add initial node
        openList.enqueue(start);

        List<HexTile> path = new List<HexTile>();
        // Go till there is nothing on openlist
        while(openList.Count>0){
            //Debug.Log(openList);
            //openList.printQueue();
            SearchNode head = openList.dequeue();
            //Debug.Log(head.Tile.Location);
            if(head.Tile == finish.Tile){
                path = generatePath(head);
                break;
            }
            else if(head.Tile.CanMove){
                List<HexTile> neighbors = getNeighbors(head.Tile);
                foreach(HexTile t in neighbors){
                    SearchNode s = generateSearchNode(t, finish.Tile, head.Cost + 1, head);
                    if(openList.contains(s)){
                        SearchNode oldNode = openList.get(s);
                        updateNode(s, oldNode);
                    }
                    else if(containsNode(closedList, s) == null){
                        openList.enqueue(s);
                    }
                }
            }
            closedList.Add(head);
        }
        //_fact.returnNodes(openList);
        //_fact.returnNodes(closedList);
        return path;
    }

    /// <summary>
    /// Find the vector telling next location to go to.
    /// if fails to find a solution, it returns the target by default
    /// </summary>
    /// <param name='position'>
    /// Position.
    /// </param>
    /// <param name='target'>
    /// Target.
    /// </param>
    /// <param name='mapInfo'>
    /// LevelInfo containing info on the level.
    /// </param>
    public static Vector2 Find(Vector2 position, Vector2 target, LevelInfo mapInfo)
    {
        if (canGoStraightLine(position, target, mapInfo)) return target;

        Vector2 defaultRtrnLoc = target; // by default tell to go straight to target

        if (!mapInfo.inBounds(position)) {
            // this SHOULDN'T happen, you should not be out of bounds of the level
            return defaultRtrnLoc;
        }

        int xPos = (int)position.x;
        int yPos = (int)position.y;
        int xTarget = (int)target.x;
        int yTarget = (int)target.y;

        // use 3th dimension to store c(p) in f(p) = h(p) + c(p)
        PriorityQueue<Vector3> frontier = new PriorityQueue<Vector3>();

        // add start position to frontier
        int cp = 0;
        int fp = getHP(position, target) + cp;
        frontier.enqueue(fp, new Vector3(xPos, yPos, cp));

        // backtracking (cpsc320 anyone?)
        // might not be the optimal method, but it works
        // Vector3, x, y map to coords, z is used for cost
        // z = -1 if visited, non-neg integer if not visited = cost to reach location
        Vector3[,] backtrack = new Vector3[(int)mapInfo.getSize().x, (int)mapInfo.getSize().y];
        // set the backtrack for position to -1,-1
        backtrack[xPos, yPos] = new Vector3(-1, -1, 1);

        for (int i = 0; i < STEP_LIMIT; i++) {
            if (frontier.isEmpty()) {
                return defaultRtrnLoc; // can't reach it...
            }

            // select lowest f(p) = h(p) + c(p)
            Vector3 nextDequeue = frontier.dequeue();
            Vector2 nextLocation = new Vector2(nextDequeue.x, nextDequeue.y);

            // double check that this location has not been visited
            if (backtrack[(int)nextLocation.x, (int)nextLocation.y].z == -1) {
                continue;
            }

            // check if goal
            if ((int)nextLocation.x == xTarget && (int)nextLocation.y == yTarget) {

                // backtrack from this position....
                // find first step to move
                return getBacktrack(backtrack, target, position, mapInfo);
            }
            // mark location as accessed
            backtrack[(int)nextLocation.x, (int)nextLocation.y].z = -1;

            bool verify_diag = false;
            // otherwise add neighbour of those to frontier that:
            for (int deltaX = -1; deltaX <= 1; deltaX++) {
                for (int deltaY = -1; deltaY <=1; deltaY++) {
                    verify_diag = false;
                    if (NO_DIAGONAL_MOVEMENT && (deltaX * deltaY != 0)) {
                        continue;
                    } else {
                        if (deltaX * deltaY != 0) {
                        // verify that we can actually move diagonally
                            verify_diag = true;
                        }
                    }

                    Vector2 neighbour = nextLocation + new Vector2(deltaX, deltaY);

                    int thisCP = (int)nextDequeue.z + Mathf.Abs(deltaX * deltaY) * DIAG_COST
                                    + LINEAR_COST;
                    int thisFP = getHP(neighbour, target) + thisCP;

                    // a) is within boundaries
                    if (!mapInfo.inBounds(neighbour)) { continue; }

                    // b) have not been visited or it's cheaper to go this way
                    Vector3 locInfo = backtrack[(int)neighbour.x, (int)neighbour.y];
                    if (locInfo != Vector3.zero && locInfo.z < thisCP) { continue; }

                    // c) are accessible from this location (walkable as specified by mapInfo)
                    if (mapInfo.isOccupiedBy(neighbour, MapScript.ENEMY_GROUP)) { continue; }
                    if (verify_diag) {
                        if (mapInfo.isOccupiedBy(nextLocation + new Vector2(0, deltaY),
//.........这里部分代码省略.........

    /***
     * Flee A* implementation
     * - what this does is similiar to A*. It searches and returns the location with the highest "distance"
     *   from the player. It has a limit to the distance it is willing to search (o), and it has a distance
     *   that it (P) refuses to come close to (x) the target (T)
     *
     *   Think of it as a donut shape centered around the target. The width of the donut depends on the distance
     *   between the position and the target
     *    --------------------------
     *    |  ooooooooo              |
     *    | oooooxooooo             |
     *    |ooPooxxxooooo            |
     *    |ooooxxTxxooooo           |
     *    |oooooxxxooooo            |
     *    | oooooxooooo             |
     *    |  ooooooooo              |
     *     -------------------------
     *
     * */
    public static Vector2 Flee(Vector2 position, Vector2 target, LevelInfo mapInfo)
    {
        float minDistancePercentage = 50.0f; //%
        float maxDistancePercentage = 350.0f; //%
        // if position and target are "r" units apart
        // will find cheapest path to farthest location that does not get within r*50% of
        // target, and no farther than r*350% of target
        // (of course, if this is repeatedly called, as you get farther from target
        // your search range increases

        // Alternatively, think of it as how "panic'd" the AI is.
        // if a AI running away from the target is really close
        // it will "panic" and simply tries to get slightly farther away
        // as it reaches farther away, it "relaxes" and makes a better analysis of the
        // possible paths.

        float distance = getDistance(position, target);
        int minDistance = (int)(distance * minDistancePercentage / 100.0f) - 1;
        int maxDistance = (int)(distance * maxDistancePercentage / 100.0f) + 1;

        Vector2 defaultRtrnLoc = position;//position + (position - target); // by default tell to go away

        if (!mapInfo.inBounds(position)) {
            // this SHOULDN'T happen, you should not be out of bounds of the level
            return defaultRtrnLoc;
        }

        int xPos = (int)position.x;
        int yPos = (int)position.y;

        // never go farther than twice the distance (computational resource reasons)

        // use 3rd dimension to store c(p) in f(p) = h(p) + c(p)
        PriorityQueue<Vector3> frontier = new PriorityQueue<Vector3>();

        // farthest location so far
        Vector2 farthestPoint = position;
        int farthestPointFP = int.MaxValue;

        // add start position to frontier
        int cp = 0;
        int fp = getFleeHP(position, target) + cp;
        frontier.enqueue(fp, new Vector3(xPos, yPos, cp));

        // backtracking (cpsc320 anyone?)
        // might not be the optimal method, but it works
        // Vector3, x, y map to coords, z is used for cost
        // z = -1 if visited, non-neg integer if not visited = cost to reach location
        Vector3[,] backtrack = new Vector3[(int)mapInfo.getSize().x, (int)mapInfo.getSize().y];
        // set the backtrack for position to -1,-1
        backtrack[xPos, yPos] = new Vector3(-1, -1, 1);

        for (int i = 0; i < STEP_LIMIT; i++) {
            if (frontier.isEmpty()) {
                //return defaultRtrnLoc; // can't reach it...
                return getBacktrack(backtrack, farthestPoint);
            }

            // select lowest f(p) = h(p) + c(p)
            Vector3 nextDequeue = frontier.dequeue();
            Vector2 nextLocation = new Vector2(nextDequeue.x, nextDequeue.y);

            // double check that this location has not been visited
            if (backtrack[(int)nextLocation.x, (int)nextLocation.y].z == -1) {
                continue;
            }

            // mark location as accessed
            backtrack[(int)nextLocation.x, (int)nextLocation.y].z = -1;

            bool verify_diag = false;
            // otherwise add neighbour of those to frontier that:
            for (int deltaX = -1; deltaX <= 1; deltaX++) {
                for (int deltaY = -1; deltaY <=1; deltaY++) {
                    verify_diag = false;
                    if (NO_DIAGONAL_MOVEMENT && (deltaX * deltaY != 0)) {
                        continue;
                    } else {
                        if (deltaX * deltaY != 0) {
                        // verify that we can actually move diagonally
                            verify_diag = true;
//.........这里部分代码省略.........

    /*!!! PATHFINDING ALGORITHM IS HIDDEN HERE !!!*/
    public LinkedList<GridSquare> findPath(Vector3 pathFrom,Vector3 pathTo)
    {
        AStarNode[] aStarNodes = new AStarNode[width*height];
        bool[] closedSet = new bool[width*height];
        PriorityQueue<GridSquare> openSet = new PriorityQueue<GridSquare>(false);

        GridSquare target = this.gridSquareFromVector3(pathTo);
        GridSquare currentSquare;

        AStarNode current;

        //Initialize with path beginning
        AStarNode temp = new AStarNode(this.gridSquareFromVector3(pathFrom),target);
        aStarNodes[temp.getSquare().getHash()] = temp;
        openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());

        //While there's still items in the open set
        while ((currentSquare = openSet.dequeue()) != null) {
            //openSet stores gridsquares for efficiency reasons, so get the relevant A* node
            current = aStarNodes[currentSquare.getHash()];

            //Add node to the closed set
            closedSet[current.getSquare().getHash()] = true;

            //If the current square is the target, we have found a path and
            //can return
            if (current.getSquare () == target) {
                break;
            }

            //For every neighbor
            foreach (GridSquare s in current.getNeighbors())
            {
                //If the square is already processed, skip it
                if (closedSet[s.getHash()] == true) {
                    continue;
                }
                //This is why the open set stores GridSquares instead of AStarNodes
                if (!openSet.Contains(s)) {
                    temp = new AStarNode(s,target);
                    aStarNodes[temp.getSquare().getHash()] = temp;

                    //setParent sets the g score automatically.
                    temp.setParent(current);

                    openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
                } else {
                    //if this is a worse path, skip it.
                    temp = aStarNodes[s.getHash ()];
                    if (current.gScore + 1 >= temp.gScore) {
                        continue;
                    }
                    //setParent sets the g score automatically.
                    temp.setParent(current);

                    //Re add to the open set.
                    openSet.Remove (temp.getSquare());
                    openSet.enqueueWithPriority(temp.getSquare(),temp.getFScore());
                }

            }
        }

        //No path was found
        if (currentSquare == null) {
            return default(LinkedList<GridSquare>);
        }

        //Reconstruct the path
        LinkedList<GridSquare> path = new LinkedList<GridSquare>();
        current = aStarNodes[target.getHash()];

        if (current == null) {
            return null;
        }

        do {
            //Add the current square to the beginning of the path
            path.AddFirst(current.getSquare());
            //Set the current node to the parent
            current = current.getParent ();
        } while (current != null);

        return path;
    }