C# PriorityQueue.Push方法代码示例

        public void PriorityQueueTest1()
        {
            PriorityQueue<string> a = new PriorityQueue<string>();

            a.Push("World", 7);
            a.Push("Hello", 2);
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("World", a.Pop());

            a.Push("Hello", 2);
            a.Push("World", 7);
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("World", a.Pop());
        }

        public void PriorityQueueTest2()
        {
            PriorityQueue<string> a = new PriorityQueue<string>(new[] { Tuple.Create("Hello", 4), Tuple.Create("World", 9), Tuple.Create("Test", 1) });
            a.Push("Test2", 2);

            Assert.AreEqual("Test", a.Pop());
            int p2;
            Assert.AreEqual("Test2", a.Pop(out p2));
            Assert.AreEqual(2, p2);

            a.Push("First", 0);
            a.Push("Second", 5);
            Assert.AreEqual("First", a.Pop());
            Assert.AreEqual("Hello", a.Pop());
            Assert.AreEqual("Second", a.Pop());
            Assert.AreEqual("World", a.Pop());
            a.Pop();
        }

        static int FindPath(char[,] map, int row, int col, Point start, Point end)
        {
            int[,] gscore = new int[row, col];
            int[,] fscore = new int[row, col];
            Color[,] status = new Color[row, col];

            PriorityQueue<Point> queue = new PriorityQueue<Point>();
            queue.Comparer = Compare;

            gscore[start.Y, start.X] = 0;
            start.F = fscore[start.Y, start.X] = Heuristic(start, end);
            queue.Push(start);
            status[start.Y, start.X] = Color.Grey;

            while (!queue.IsEmpty())
            {
                Point cur = queue.Pop();

                if (cur.X == end.X && cur.Y == end.Y)
                    return fscore[cur.Y, cur.X];

                foreach (var n in GetNeighbors(cur, map, row, col))
                {
                    if (status[n.Y, n.X] == Color.Black)
                        continue;

                    int tentative = gscore[cur.Y, cur.X] + 1;
                    bool useTentative = true;

                    if (status[n.Y, n.X] == Color.Grey && gscore[n.Y, n.X] <= tentative)
                        useTentative = false;

                    if (useTentative)
                    {
                        gscore[n.Y, n.X] = tentative;
                        n.F = fscore[n.Y, n.X] = tentative + Heuristic(n, end);
                        queue.Push(n);
                        n.P = cur;
                    }
                }
            }

            return int.MinValue;
        }

        public static void Main()
        {
            PriorityQueue<int> queue = new PriorityQueue<int>();

            queue.Push(1);
            queue.Push(2);
            queue.Push(12);
            queue.Push(34);
            queue.Push(6);
            queue.Push(9);

            Console.WriteLine(queue.Pop());
            Console.WriteLine(queue.Pop());
        }

        public void PriorityQueueSize()
        {
            PriorityQueue<int> p = new PriorityQueue<int>();
            Assert.AreEqual(0, p.Size);

            p.Push(5, 4);
            Assert.AreEqual(1, p.Size);

            p.Pop();
            Assert.AreEqual(0, p.Size);

            p = new PriorityQueue<int>(new[] { Tuple.Create(1, 1), Tuple.Create(1, 7) });
            Assert.AreEqual(2, p.Size);

            p.Push(9, 12);
            Assert.AreEqual(3, p.Size);

            p.Pop();
            Assert.AreEqual(2, p.Size);
        }

        public bool searchPath(Dictionary<string, string> pathMap)
        {
            PriorityQueue<Node> priorityQueue;
            priorityQueue = new PriorityQueue<Node>();

            priorityQueue.Push(this.begainNode);

            while (!priorityQueue.Empty())
            {
                Node topNode = priorityQueue.Pop();

                #region 判断是否找到目状态
                if (matched(topNode, this.targetNode))
                {
                    MessageBox.Show("Finished!");
                    return true;
                }
                #endregion

                int row = topNode.row_0;
                int col = topNode.col_0;

                if (row > 0 && topNode.cannotAct != Direction.up)
                {
                    Node curNode = new Node(topNode);

                    exchange(curNode, row, col, row - 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.down;

                    if (!pathMap.ContainsKey(curNode.state))
                    {
                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);
                        curNode.row_0 = row - 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (row < 2 && topNode.cannotAct != Direction.down)
                {
                    Node curNode = new Node(topNode);

                    exchange(curNode, row, col, row + 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.up;

                    if (!pathMap.ContainsKey(curNode.state))
                    {
                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);
                        curNode.row_0 = row + 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (col > 0 && topNode.cannotAct != Direction.left)
                {
                    Node curNode = new Node(topNode);

                    exchange(curNode, row, col, row, col - 1);
                    curNode.ToString();
                    curNode.cannotAct = Direction.left;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);
                        curNode.row_0 = row;
                        curNode.col_0 = col - 1;
                        priorityQueue.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (col < 2 && topNode.cannotAct != Direction.right)
                {
                    Node curNode = new Node(topNode);
                    exchange(curNode, row, col, row, col + 1);
                    curNode.ToString();
                    curNode.cannotAct = Direction.right;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);
                        curNode.row_0 = row;
                        curNode.col_0 = col + 1;
                        priorityQueue.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }
            }

            return false;
        }

        public static Cell DijkstraNextCell(int startI, int startJ, int endI, int endJ, Cell[] forbidden)
        {
            if (_distMap == null)
            {
                _distMap = new double[world.Height, world.Width];
                _distPrev = new Cell[world.Height, world.Width];
            }

            var q = new PriorityQueue<Pair<double, Cell>>();
            q.Push(new Pair<double, Cell>(0.0, new Cell(endI, endJ)));
            for (var i = 0; i < world.Height; i++)
                for (var j = 0; j < world.Width; j++)
                    _distMap[i, j] = Infinity;
            _distMap[endI, endJ] = 0;
            while (q.Count > 0)
            {
                var cur = q.Top().Second;
                var minDist = -q.Top().First;
                q.Pop();

                if (minDist > _distMap[cur.I, cur.J])
                    continue;

                EnumerateNeigbours(cur, to =>
                {
                    if (!CanPass(cur.I, cur.J, to.I, to.J) || forbidden.Any(x => x.Equals(to.I, to.J)))
                        return;

                    var distTo = _distMap[cur.I, cur.J] + GetCost(to);
                    if (distTo < _distMap[to.I, to.J])
                    {
                        _distMap[to.I, to.J] = distTo;
                        _distPrev[to.I, to.J] = cur;
                        q.Push(new Pair<double, Cell>(-distTo, to));
                    }
                });
            }

            if (_distPrev[startI, startJ] == null)
            {
                if (forbidden.Length == 0)
                    throw new Exception("path not found");
                return DijkstraNextCell(startI, startJ, endI, endJ, new Cell[] {});
            }

            return _distPrev[startI, startJ];
        }

        public static String Solve( Puzzle map )
        {
            Puzzle initialNode = map;

            Puzzle end = null;
            var moves = Enum.GetValues(typeof(Puzzle.Position));

            PriorityQueue<Puzzle> nodes = new PriorityQueue<Puzzle>();

            nodes.Push(initialNode);

            while (nodes.Count != 0)
            {
                Puzzle currentNode = nodes.Top;
                nodes.Pop();

                if (currentNode.MD == 0)
                {
                    end = currentNode;
                    break;
                }

                if (currentNode.Blank != -1)
                {
                    foreach (var move in moves)
                    {
                        if (currentNode.NowStatus.answer.Count != 0)
                        {
                            Puzzle.Position now = currentNode.NowStatus.answer[currentNode.NowStatus.answer.Count - 1];
                            if (currentNode.ReversePosition(now) == (Puzzle.Position)move)
                                continue;
                        }
                        Puzzle newPuzzle = (Puzzle)currentNode.Clone();
                        if (newPuzzle.DoMove((Puzzle.Position)move))
                        {
                            newPuzzle.CalculateMD();
                            nodes.Push(newPuzzle);
                        }
                    }
                }

                if (currentNode.maxChoice != 0 && ( currentNode.NowStatus.answer.Count != 0 || currentNode.Blank == -1 ))
                {
                    for (int i = 0; i < currentNode.Data.Count(); i++)
                    {
                        if (i == currentNode.Blank)
                            continue;

                        Puzzle newPuzzle = (Puzzle)currentNode.Clone();
                        newPuzzle.Choice(i);
                        newPuzzle.CalculateMD();
                        nodes.Push(newPuzzle);
                    }
                }
            }

            if (end != null){
                return end.GetSolution();
            }
            else
                return "";
        }

        static void Main(string[] args)
        {
            search begainSearch = new search();

            //测试优先队列功能
            PriorityQueue<int> que;
            que = new PriorityQueue<int>();
            que.Push(12);
            que.Push(132);
            que.Push(123);
            que.Push(212);
            que.Push(322);
            que.Push(126);
            que.Push(13);
            que.Push(189);
            while (!que.Empty())
            {
                Console.Write(que.Pop().ToString()+" ");
            }
        }

        bool searchPath(Dictionary<string, string> pathMap, Node result)
        {
            PriorityQueue<Node> priorityQueue;
            Stack<Node> pathStack;
            priorityQueue = new PriorityQueue<Node>();
            pathStack = new Stack<Node>();

            priorityQueue.Push(this.begainNode);
            pathStack.Push(this.begainNode);

            int cycle = 0;

            while (!priorityQueue.Empty())
            {
                cycle++;
               //     Console.WriteLine("第 "+cycle.ToString()+" 步");
            //    Console.WriteLine("队列中的元素  " + priorityQueue.Count);
                Node topNode = priorityQueue.Top();

                priorityQueue.Pop();

                #region 判断是否找到目状态
                if (matched(topNode, this.targetNode))
                {
                    printState(targetNode);
                    Console.WriteLine("搜索完成");
                    printState(topNode);
                    result = topNode;
                    return true;
                }
                #endregion

                int row = topNode.row_0;
                int col = topNode.col_0;

                if (row > 0 && topNode.cannotAct != Direction.up)
                {
                    Node curNode = new Node(topNode);

               //     Console.WriteLine("当前状态");
              //      printState(topNode);
               //     Console.WriteLine(row.ToString()+" "+col.ToString()+"   空格上移后状态");

                    exchange(curNode, row, col, row - 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.down;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                //        printState(curNode);

                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);

                //        Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());

                        curNode.father = topNode;
                        curNode.row_0 = row - 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathStack.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (row < 2 && topNode.cannotAct != Direction.down)
                {
                    Node curNode = new Node(topNode);

                //    Console.WriteLine("当前状态");
                //    printState(topNode);
                //    Console.WriteLine(row.ToString()+" "+col.ToString()+"    下移后状态");

                    exchange(curNode, row, col, row + 1, col);
                    curNode.ToString();
                    curNode.cannotAct = Direction.up;
                    if (!pathMap.ContainsKey(curNode.state))
                    {
                //        printState(curNode);

                        curNode.deepth = topNode.deepth + 1;
                        curNode.value = getValue(curNode, this.targetNode);

                 //       Console.WriteLine("当前代价值："+(curNode.value + curNode.deepth).ToString());

                        curNode.father = topNode;
                        curNode.row_0 = row + 1;
                        curNode.col_0 = col;
                        priorityQueue.Push(curNode);
                        pathStack.Push(curNode);
                        pathMap.Add(curNode.state, topNode.state);
                    }
                }

                if (col > 0 && topNode.cannotAct != Direction.left)
                {
                    Node curNode = new Node(topNode);

                 //   Console.WriteLine("当前状态");
                 //   printState(topNode);
                //    Console.WriteLine(row.ToString()+" "+col.ToString()+"    左移之后的状态");
//.........这里部分代码省略.........

        public List<Waypoint> FindPath(Waypoint start, Waypoint goal, OccupancyGrid2D og, out bool success)
        {
            List<Waypoint> path = new List<Waypoint>();

            //added by aaron (sort of a hack)
            if (og == null || goal.Coordinate.DistanceTo(start.Coordinate) == 0)
            {
                path.Add(new Waypoint(start.Coordinate, true, 0));
                success = true;
                return path;
            }

            int xIdx, yIdx;
            success = true;
            Vector2[] NESWVector = new Vector2[4];
            Vector2[] diagVector = new Vector2[4];
            bool[] NESW = new bool[4];
            Vector2 startV = start.Coordinate; // Start Vector2
            Vector2 goalV = goal.Coordinate; // Goal Vector2

            PriorityQueue open = new PriorityQueue();
            closed = new OccupancyGrid2D(resX, resY, extentX, extentY);
            opened = new OccupancyGrid2D(resX, resY, extentX, extentY);

            GetIndicies(startV.X, startV.Y, out xIdx, out yIdx);
            startV = new Vector2(xIdx, yIdx);
            GetIndicies(goalV.X, goalV.Y, out xIdx, out yIdx);
            goalV = new Vector2(xIdx, yIdx);

            Node root = new Node(goalV, goalV.DistanceTo(startV), 0, null);

            Node current = root;
            open.Push(current);

            // Do the spreading/discovering stuff until we discover a path.
            while (current.xy != startV)
            {
                if (open.q.Count == 0 || open.q.Count > MAX_OPEN)
                {
                    Console.WriteLine("Failure in DSstar. Open count is: " + open.q.Count);
                    success = false;
                    break;
                }
                current = open.Pop();

                NESWVector[0] = new Vector2(current.xy.X, current.xy.Y - 1);
                NESWVector[1] = new Vector2(current.xy.X + 1, current.xy.Y);
                NESWVector[2] = new Vector2(current.xy.X, current.xy.Y + 1);
                NESWVector[3] = new Vector2(current.xy.X - 1, current.xy.Y);

                diagVector[0] = new Vector2(current.xy.X + 1, current.xy.Y - 1);
                diagVector[1] = new Vector2(current.xy.X + 1, current.xy.Y + 1);
                diagVector[2] = new Vector2(current.xy.X - 1, current.xy.Y + 1);
                diagVector[3] = new Vector2(current.xy.X - 1, current.xy.Y - 1);

                for (int i = 0; i < 4; i++)
                {
                    if ((int)og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) < 255)
                    {
                        if (closed.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                        {
                            NESW[i] = true;
                            if (opened.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) == 0)
                            {
                                open.Push(new Node(NESWVector[i], NESWVector[i].DistanceTo(startV), current.h + 1
                                    + og.GetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y) / blurWeight, current));
                                opened.SetCellByIdx((int)NESWVector[i].X, (int)NESWVector[i].Y, 1);
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++)
                {
                    if (NESW[i % 4] && NESW[(i + 1) % 4])
                    {
                        if (og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) < 255)
                        {
                            if (closed.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                            {
                                if (opened.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) == 0)
                                {
                                    open.Push(new Node(diagVector[i], diagVector[i].DistanceTo(startV), current.h + 1.4
                                        + og.GetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y) / blurWeight, current));
                                    opened.SetCellByIdx((int)diagVector[i].X, (int)diagVector[i].Y, 1);
                                }
                            }
                        }
                    }
                }

                for (int i = 0; i < 4; i++) NESW[i] = false;

                closed.SetCellByIdx((int) current.xy.X, (int) current.xy.Y, 1);
            }

            // Build a path using the discovered path.
            double x, y;
            Waypoint waypoint;

//.........这里部分代码省略.........

 private void requeue()
 {
     _queue = new PriorityQueue<Node>();
     for (int i = 0; i < 9; i++)
         for (int j = 0; j < 9; j++)
         {
             _queue.Push(_board[i, j]);
         }
 }

        //Given seed probability P, find the best k nodes that can maximize influence spread
        public Tuple<List<int>, double> Greedy(int k, List<double> P)
        {
            HashSet<int> seeds = new HashSet<int> ();
            List<int> seedSet = new List<int> ();
            List<double> edgeW = new List<double> ();
            for (int h = 0; h < numS; ++h)
                edgeW.Add (1.0);

            //CELF Algorithm
            PriorityQueue<VNode> pq = new PriorityQueue<VNode> (numV+1, new VNodeComparer ());
            List<bool> update = new List<bool> ();
            for (int u = 0; u < numV; ++u)
            {
                VNode node = new VNode (u, numS);
                pq.Push (node);
                update.Add (false);
            }

            double total_gain = 0.0;
            for (int i = 0; i < k; ++i)
            {
                for (int u = 0; u < numV; ++u)
                    update [u] = false;
                int next = 0;
                double gain = 0;
                while (true)
                {
                    VNode node = pq.Pop ();
                    int max_u = node.id;

                    if (update [max_u])
                    {
                        next = max_u;
                        gain = node.val;
                        break;
                    }
                    else
                    {
                        double sum = 0;
                        if (i == 0)
                            sum = V2S[max_u].Count * P[max_u];
                        else
                        {
                            foreach (int sid in V2S[max_u])
                            sum += edgeW[sid] * P[max_u];
                        }
                        VNode n1 = new VNode (max_u, sum);
                        pq.Push (n1);
                        update [max_u] = true;
                    }
                }
                total_gain += gain;
                foreach (int sid in V2S[next])
                    edgeW [sid] = edgeW [sid] * (1 - P [next]);
                seeds.Add (next);
                seedSet.Add (next);
            }

            return new Tuple<List<int>, double> (seedSet, total_gain*numV/numS);
        }

        public static Map.Box AStar(Map m, Map.Box start, Vector3 target, out int pathLength)
        {
            Dictionary<Map.Box, int> closed = new Dictionary<Map.Box, int>();

            PriorityQueue<AStarEntry> queue = new PriorityQueue<AStarEntry>(new LambdaComparer<AStarEntry>((x, y) => x.F.CompareTo(y.F)));

            Dictionary<Map.Box, AStarEntry> queueLookup = new Dictionary<Map.Box, AStarEntry>();

            AStarEntry startEntry = new AStarEntry
            {
                Parent = null,
                Box = start,
                G = 0,
                F = (target - start.GetCenter()).Length(),
                BoxSize = Math.Max(start.Width, Math.Max(start.Height, start.Depth)),
                PathIndex = 0,
            };
            queue.Push(startEntry);
            queueLookup[start] = startEntry;

            const float thresholdFCoefficient = 0.6f;
            const int iterationLimit = 10;

            int iteration = 0;
            while (queue.Count > 0)
            {
                AStarEntry entry = queue.Pop();

                if (iteration >= iterationLimit || entry.F < entry.BoxSize * thresholdFCoefficient)
                    return VoxelChaseAI.reconstructPath(entry, out pathLength);

                iteration++;

                queueLookup.Remove(entry.Box);

                closed[entry.Box] = entry.G;
                foreach (Map.Box adjacent in entry.Box.Adjacent.ToList())
                {
                    if (adjacent == null)
                        continue;

                    int boxSize = Math.Max(adjacent.Width, Math.Max(adjacent.Height, adjacent.Depth));

                    int tentativeGScore = entry.G + boxSize;

                    int previousGScore;
                    bool hasPreviousGScore = closed.TryGetValue(adjacent, out previousGScore);

                    if (hasPreviousGScore && tentativeGScore > previousGScore)
                        continue;

                    AStarEntry alreadyInQueue;
                    bool throwaway = queueLookup.TryGetValue(adjacent, out alreadyInQueue);

                    if (alreadyInQueue == null || tentativeGScore < previousGScore)
                    {
                        AStarEntry newEntry = alreadyInQueue != null ? alreadyInQueue : new AStarEntry();

                        newEntry.Parent = entry;
                        newEntry.G = tentativeGScore;
                        newEntry.F = tentativeGScore + (target - adjacent.GetCenter()).Length();
                        newEntry.PathIndex = entry.PathIndex + 1;

                        if (alreadyInQueue == null)
                        {
                            newEntry.Box = adjacent;
                            newEntry.BoxSize = boxSize;
                            queue.Push(newEntry);
                            queueLookup[adjacent] = newEntry;
                        }
                    }
                }
            }
            pathLength = 0;
            return null;
        }