C# PriorityQueue.Contains方法代码示例

        public void Simple()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum) { "5" };

            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Contains("5"));
            Assert.IsTrue(priorityQueue.Remove("5"));
            Assert.AreEqual(priorityQueue.Count, 0);

            priorityQueue.Add("6");
            priorityQueue.Add("7");
            priorityQueue.Add("2", 4);

            Assert.AreEqual(priorityQueue.Count, 3);
            Assert.IsTrue(priorityQueue.Contains("6"));
            Assert.IsTrue(priorityQueue.Contains("7"));
            Assert.IsTrue(priorityQueue.Contains("2"));

            Assert.IsFalse(priorityQueue.Remove("8"));

            Assert.IsTrue(priorityQueue.Remove("7"));
            Assert.AreEqual(priorityQueue.Count, 2);
            Assert.IsTrue(priorityQueue.Remove("2"));
            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Remove("6"));
            Assert.AreEqual(priorityQueue.Count, 0);

            Assert.IsFalse(priorityQueue.Remove("7"));
        }

        public void WithPriority()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum);

            int priority;

            priorityQueue.Add("5", 3);
            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Contains("5"));
            Assert.IsTrue(priorityQueue.Remove("5", out priority));
            Assert.AreEqual(priority, 3);

            Assert.AreEqual(priorityQueue.Count, 0);

            priorityQueue.Add("6");
            priorityQueue.Add("7");
            priorityQueue.Add("2", 4);

            Assert.AreEqual(priorityQueue.Count, 3);
            Assert.IsTrue(priorityQueue.Contains("6"));
            Assert.IsTrue(priorityQueue.Contains("7"));
            Assert.IsTrue(priorityQueue.Contains("2"));

            Assert.IsFalse(priorityQueue.Remove("8", out priority));

            Assert.IsTrue(priorityQueue.Remove("7", out priority));
            Assert.AreEqual(priority, 0);
            Assert.AreEqual(priorityQueue.Count, 2);

            Assert.IsTrue(priorityQueue.Remove("2", out priority));
            Assert.AreEqual(priority, 4);

            Assert.AreEqual(priorityQueue.Count, 1);
            Assert.IsTrue(priorityQueue.Remove("6", out priority));
            Assert.AreEqual(priority, 0);
            Assert.AreEqual(priorityQueue.Count, 0);

            Assert.IsFalse(priorityQueue.Remove("7", out priority));
        }

 public static ArrayList FindPath(Node start, Node goal)
 {
     openList = new PriorityQueue();
     openList.Push(start);
     start.nodeTotalCost = 0.0f;
     start.estimatedCost = HeuristicEstimateCost(start, goal);
     closedList = new PriorityQueue();
     Node node = null;
     while (openList.Length != 0) {
         node = openList.First();
         //Check if the current node is the goal node
         if (node.position == goal.position) {
             return CalculatePath(node);
         }
         //Create an ArrayList to store the neighboring nodes
         ArrayList neighbours = new ArrayList();
         GridManager.instance.GetNeighbours(node, neighbours);
         for (int i = 0; i < neighbours.Count; i++) {
             Node neighbourNode = (Node)neighbours[i];
             if (!closedList.Contains(neighbourNode)) {
                 float cost = HeuristicEstimateCost(node,
                         neighbourNode);
                 float totalCost = node.nodeTotalCost + cost;
                 float neighbourNodeEstCost = HeuristicEstimateCost(
                         neighbourNode, goal);
                 neighbourNode.nodeTotalCost = totalCost;
                 neighbourNode.parent = node;
                 neighbourNode.estimatedCost = totalCost +
                         neighbourNodeEstCost;
                 if (!openList.Contains(neighbourNode)) {
                     openList.Push(neighbourNode);
                 }
             }
         }
         //Push the current node to the closed list
         closedList.Push(node);
         //and remove it from openList
         openList.Remove(node);
     }
     if (node.position != goal.position) {
         Debug.LogError("Goal Not Found");
         return null;
     }
     return CalculatePath(node);
 }

    public static List<Node> findPath(Node start, Node goal)
    {
        openList = new PriorityQueue();
        openList.Push(start);
        start.nodeTotalCost = 0.0f;
        start.estimatedCost = heuristicEstimateCost(start,goal);

        closedList = new PriorityQueue();
        Node node = null;

        while (openList.Length != 0) {
            node = openList.First();

            if(node.position == goal.position){
                return calculatePath(node);
            }

            List<Node> neighbours = new List<Node>();
            gridManager.getNeighbours(node,neighbours);
            for(int i=0;i<neighbours.Count;i++){
                Node neighbour = neighbours[i];

                if(!closedList.Contains(neighbour)){
                    float cost = heuristicEstimateCost(node,neighbour);
                    float totalCost = node.nodeTotalCost + cost;
                    float neighbourEstCost = heuristicEstimateCost(neighbour,goal);
                    neighbour.nodeTotalCost = totalCost;
                    neighbour.parent = node;
                    neighbour.estimatedCost = totalCost + neighbourEstCost;
                    if(!openList.Contains(neighbour)){
                        openList.Push(neighbour);
                    }
                }

            }
            closedList.Push(node);
            openList.Remove(node);
        }
        if (node.position != goal.position) {
            Debug.LogError("Goal Not Found");
            return null;
        }
        return calculatePath(node);
    }

    //A* implementation
    public static IEnumerator CalculatePath(PathNode start, PathNode end, List<PathNode> allNodes, System.Action<List<Vector2>> callback)
    {

        int n = 0;

        PriorityQueue openList = new PriorityQueue();
        PriorityQueue closedList = new PriorityQueue();

        openList.Push(start);
        start.cost = 0;
        start.estimatedCost = HeuristicEstimate(start, end, heuristicWeight);

        PathNode currentNode = null;

        while (openList.Count != 0)
        {

            currentNode = openList.Front();
            if (currentNode == end)
                break;

            List<int> links = currentNode.links;

            for (int i = 0; i != links.Count; i++)
            {

                PathNode endNode = allNodes[links[i]];

                float incrementalCost = GetCost(currentNode, endNode);
                float endNodeCost = currentNode.cost + incrementalCost;

                if (closedList.Contains(endNode))
                {
                    if (endNode.cost <= endNodeCost)
                        continue;

                    closedList.Remove(endNode);

                }
                else if (openList.Contains(endNode))
                {
                    if (endNode.cost <= endNodeCost)
                        continue;
                }

                float endNodeHeuristic = HeuristicEstimate(endNode, end, heuristicWeight);
                endNode.cost = endNodeCost;
                endNode.parent = currentNode;
                endNode.estimatedCost = endNodeCost + endNodeHeuristic;

                if (!openList.Contains(endNode))
                    openList.Push(endNode);
            }

            closedList.Push(currentNode);
            openList.Remove(currentNode);

            n++;
            if (n > 300)
                yield return null;
        }

        if (!currentNode.Equals(end))
        {

            // Debug.LogWarning("No path found :(");
            callback(new List<Vector2>());
            yield break;

        }
        else
        {

            var path = new List<Vector2>();

            while (currentNode != null)
            {

                path.Add(currentNode.pos);
                currentNode = currentNode.parent;
            }

            path.Reverse();
            callback(path);
            yield break;
        }
    }

        public static void Test()
        {
            Random r = new Random();
            PriorityQueue queue = new PriorityQueue();
            int count = 1000;
            int element;

            for(int i = 0; i < count; i++)
            {
                element = r.Next();
                queue.Enqueue(element);

                Debug.Assert(queue.Contains(element), "Contains Test");
            }

            Debug.Assert(queue.Count == count, "Count Test");

            int previousElement = (int)queue.Peek();
            int peekElement;

            for(int i = 0; i < count; i++)
            {
                peekElement = (int)queue.Peek();
                element = (int)queue.Dequeue();

                Debug.Assert(element == peekElement, "Peek Test");
                Debug.Assert(element <= previousElement, "Order Test");

                previousElement = element;
            }

            Debug.Assert(queue.Count == 0);
        }

    /// <summary>
    /// Find the path between start node and goal node using AStar Algorithm
    /// </summary>
    public static ArrayList FindPath(Shell start, Shell goal)
    {
        //Start Finding the path
        openList = new PriorityQueue();
        openList.Push(start);
        start.nodeTotalCost = 0.0f;
        start.estimatedCost = HeuristicEstimateCost(start, goal);

        closedList = new PriorityQueue();
        Shell node = null;

        while (openList.Length != 0)
        {
            node = openList.First();

            if (node.position == goal.position)
            {
                return CalculatePath(node);
            }

            ArrayList neighbours = new ArrayList();
            GridManager.instance.GetNeighbours(node, neighbours);

            #region CheckNeighbours

            //Get the Neighbours
            for (int i = 0; i < neighbours.Count; i++)
            {
                //Cost between neighbour nodes
                Shell neighbourNode = (Shell)neighbours[i];

                if (!closedList.Contains(neighbourNode))
                {
                    //Cost from current node to this neighbour node
                    float cost = HeuristicEstimateCost(node, neighbourNode);

                    //Total Cost So Far from start to this neighbour node
                    float totalCost = node.nodeTotalCost + cost;

                    //Estimated cost for neighbour node to the goal
                    float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal);

                    //Assign neighbour node properties
                    neighbourNode.nodeTotalCost = totalCost;
                    neighbourNode.parent = node;
                    neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost;

                    //Add the neighbour node to the list if not already existed in the list
                    if (!openList.Contains(neighbourNode))
                    {
                        openList.Push(neighbourNode);
                    }
                }
            }

            #endregion

            closedList.Push(node);
            openList.Remove(node);
        }

        //If finished looping and cannot find the goal then return null
        if (node.position != goal.position)
        {
            Debug.LogError("Goal Not Found");
            return null;
        }

        //Calculate the path based on the final node
        return CalculatePath(node);
    }

        public LinkedList<Vector3> FindPathTo(Node end)
        {
            HashSet<Node> closedSet = new HashSet<Node>();
            PriorityQueue openSet = new PriorityQueue();
            Dictionary<Node, int> g_score = new Dictionary<Node, int>();
            Dictionary<Node, Node> cameFrom = new Dictionary<Node, Node>();

            g_score.Add(this, 0);
            openSet.Insert(heuristicDistance(this, end), this);

            while (openSet.Length > 0)
            {
                Node currentNode = openSet.ExtractMin();

                if (currentNode == end)
                {
                    LinkedList<Vector3> path = ReconstructPath(cameFrom, end);
                    return path;
                }

                closedSet.Add(currentNode);

                if (currentNode.neighbours != null && currentNode.neighbours.Length > 0)
                {
                    foreach (var neighbour in currentNode.neighbours)
                    {
                        if (neighbour.cost >= 100)
                            continue;

                        int tentative_g_score = g_score[currentNode] + RealDistance(currentNode, neighbour);

                        if (closedSet.Contains(neighbour))
                        {
                            if(tentative_g_score >= g_score[currentNode])
                                continue;
                            else
                            {
                                Debug.Log("Remove");
                                closedSet.Remove(currentNode);
                            }
                        }

                        if (openSet.Contains(neighbour))
                        {
                            if (tentative_g_score >= g_score[neighbour])
                                continue;
                            else
                                openSet.DecreaseKey(tentative_g_score + heuristicDistance(neighbour, end), neighbour);
                        }
                        else
                            openSet.Insert(tentative_g_score + heuristicDistance(neighbour, end), neighbour);

                        cameFrom[neighbour] = currentNode;
                        g_score[neighbour] = tentative_g_score;
                    }
                }
            }

            return null;
        }

        public void ClearExample()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum);
            priorityQueue.Enqueue("cat");
            priorityQueue.Enqueue("dog");
            priorityQueue.Enqueue("canary");

            // There should be 3 items in the priorityQueue.
            Assert.AreEqual(3, priorityQueue.Count);

            // Clear the priorityQueue
            priorityQueue.Clear();

            // The priorityQueue should be empty.
            Assert.AreEqual(0, priorityQueue.Count);

            // No cat here..
            Assert.IsFalse(priorityQueue.Contains("cat"));
        }

        public void RemoveExample()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum);

            // Does not contain "canary"
            Assert.IsFalse(priorityQueue.Contains("canary"));

            priorityQueue.Enqueue("canary");

            // Does contain "canary"
            Assert.IsTrue(priorityQueue.Contains("canary"));
        }

        public void ContainsExample()
        {
            var priorityQueue = new PriorityQueue<string, int>(PriorityQueueType.Minimum);
            priorityQueue.Enqueue("cat");
            priorityQueue.Enqueue("dog");
            priorityQueue.Enqueue("canary");

            // priorityQueue does contain cat, dog and canary
            Assert.IsTrue(priorityQueue.Contains("cat"));
            Assert.IsTrue(priorityQueue.Contains("dog"));
            Assert.IsTrue(priorityQueue.Contains("canary"));
            // but not frog
            Assert.IsFalse(priorityQueue.Contains("frog"));
        }

    void FindPathPQ(Vector3 startPos, Vector3 targetPos, int x)
    {
        Node3d startNode = grid.NodeFromWorldPoint (startPos);
        Node3d targetNode = grid.NodeFromWorldPoint (targetPos);
        PriorityQueue<Node3d> openSet = new PriorityQueue<Node3d> ();
        HashSet<Node3d> closedSet = new HashSet<Node3d> ();
        openSet.Enqueue (startNode.fCost, startNode);
        while (openSet.Count > 0) {
            //get the lowest fCost in openSet, o(1)
            Node3d currentNode = openSet.Dequeue ();
            closedSet.Add (currentNode);

            if (currentNode == targetNode) {
                RetracePath (startNode, targetNode, x);
                return;
            }
            foreach (Node3d neighbor in grid.GetNeighbors(currentNode)) {
                if (!neighbor.walkable || closedSet.Contains (neighbor)) {
                    continue;
                }

                int newMovementCostToNeighbor = currentNode.gCost + GetDistance (currentNode, neighbor);
                //contains is constant because custom PQ uses another Dict<node,bool> to track values added
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains (neighbor)) {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = GetDistance (neighbor, targetNode);
                    neighbor.parent = currentNode;

                    if (!openSet.Contains (neighbor)) {
                        openSet.Enqueue (neighbor.fCost, neighbor);
                    }
                }
            }
        }
    }

    public bool FindPath()
    {
        //Check if indexes are valid
        if (!IsIndexValid (First) || !IsIndexValid (Last))
            return false;

        //Check if starting and ending position are same
        if (First == Last) {
            ConstructPath (new Dictionary<Point, Point> (), Last);
            return true;
        }

        var profiler = new Stopwatch ();
        profiler.Start ();

        //Create open/close sets and cameFrom
        var closed = new HashSet<Point> ();
        var open = new PriorityQueue<Point> (true);
        var cameFrom = new Dictionary<Point, Point> ();

        //Create f/g score dicts
        var gScore = new Dictionary<Point, float> ();
        var fScore = new Dictionary<Point, float> ();

        //Prepare the first node
        var current = First;
        gScore [current] = 0f;
        fScore [current] = gScore [current] + CostEstimate (current, Last);
        open.Insert (current, fScore [current]);

        //Main loop
        while (!open.IsEmpty) {
            //Take the first in the priority queue
            current = open.Pop ();
            closed.Add (current);

            //If goal reached
            if (current == Last) {
                ConstructPath (cameFrom, current);
                profiler.Stop ();
                ComputationTime = profiler.Elapsed;
                return true;
            }

            //Iterate neighbours
            var neighbours = GetNeighbours (current);
            foreach (var neighbourPair in neighbours) {
                var neighbour = neighbourPair.Key;
                var cost = neighbourPair.Value;
                //Don't search closed nodes
                if (closed.Contains (neighbour))
                    continue;

                //Check if it's worth it
                var tentativeGScore = gScore [current] + cost;
                if (open.Contains (neighbour) && tentativeGScore >= gScore [neighbour])
                    continue;

                //Set up neighbour
                cameFrom [neighbour] = current;
                gScore [neighbour] = tentativeGScore;
                fScore [neighbour] = gScore [neighbour] + CostEstimate (neighbour, Last);

                //Change priority or add it to the priority queue
                if (open.ChangePriority (neighbour, fScore [neighbour]))
                    continue;

                open.Insert (neighbour, fScore [neighbour]);
            }
        }

        profiler.Stop ();
        ComputationTime = profiler.Elapsed;
        return false;
    }

    void FindPathPQ(Vector3 startPos, Vector3 targetPos, int x)
    {
        Node startNode = grid.NodeFromWorldPoint(startPos);
        Node targetNode = grid.NodeFromWorldPoint(targetPos);
        PriorityQueue<Node> openSet = new PriorityQueue<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Enqueue(startNode.fCost, startNode);
        while (openSet.Count > 0)
        {
            //get the lowest fCost in openSet, o(logn)
            Node currentNode = openSet.Dequeue();
            closedSet.Add(currentNode);

            if (currentNode == targetNode)
            {
                RetracePath(startNode, targetNode, x);
                return;
            }
            foreach (Node neighbor in grid.GetNeighbors(currentNode))
            {
                if (!neighbor.walkable || closedSet.Contains(neighbor))
                {
                    continue;
                }
                //calculate new cost it takes to get to neighbor
                int newMovementCostToNeighbor = currentNode.gCost + GetNeighborCost(currentNode, neighbor, x);

                //update if new path to neighbor is less costly, or neighbor hasn't been considered
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = GetDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;
                    if (!openSet.Contains(neighbor))
                    {
                        openSet.Enqueue(neighbor.fCost, neighbor);
                    }
                }
            }
        }
    }

    /*!!! 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;
    }