astarpathfinder.cs

两种AstarPathFinder 一种DijkstraPathfinder BFS

        /// <param name="changePointFCost"></param>
        /// <param name="destination"></param>
        private void changeOpenHeap(Point changePoint, int changePointGCost, int changePointFCost)
        {
            int m = openPointToHeapID[changePoint.X, changePoint.Y];                                  //排序，m代表当前结点在openHeap中的位置，查找其位置
            //for (int i = 1; i <= numberOfOpenListItems; i++)
            //{

            //    if (openArray[openHeap[i]] == changePoint)
            //    {
            //        totalCommpareNumber += i;
            //        m = i;
            //        break;
            //    }
            //}

            gCost[openHeap[m]] = changePointGCost;
            fCost[openHeap[m]] = changePointFCost;
            while (m != 1)
            {
                if (fCost[openHeap[m]] <= fCost[openHeap[m >> 1]])
                {
                    heapSwap(m, m >> 1);
                    m = m >> 1;
                }
                else break;
            }

            int smallest = m;
            do
            {
                m = smallest;
                if (((m << 1) <= numberOfOpenListItems) && (fCost[openHeap[smallest]] > fCost[openHeap[m << 1]]))
                {
                    smallest = (m << 1);
                }
                if (((m << 1) + 1 <= numberOfOpenListItems) && (fCost[openHeap[smallest]] > fCost[openHeap[(m << 1) + 1]]))
                {
                    smallest = (m << 1) + 1;
                }
                if (m != smallest)
                {
                    heapSwap(m, smallest);
                }
            } while (m != smallest);
        }

        public override Stack<Point> FindWay(Point now, Point to)
        {
            // int checkedIfBetterInOpennumber = 0;                  //test use 
            // int changednumber = 0;
            //totalCommpareNumber = 0;
            //heapDepth = 0;
            Stack<Point> path = new Stack<Point>();
            if (areaMark[now.X, now.Y] == areaMark[to.X, to.Y])             //可达才计算
            {
                numberOfOpenListItems = 0;
                openID = 0;
                openMark += 2;
                closeMark += 2;
                for (int i = 0; i < currentMap.Width; i++)
                {
                    for (int j = 0; j < currentMap.Height; j++)
                    {
                        parents[i, j] = 5;
                    }
                }


                //把当前点加入OPEN队列
                insertOpenHeap(now, to, 0);
                while ((numberOfOpenListItems > 0) && (openArray[openHeap[1]] != to))
                {
                    int currentPointID = getMin();
                    whichList[openArray[currentPointID].X, openArray[currentPointID].Y] = closeMark;
                    for (byte i = 0; i < moves.Length; i++)
                    {
                        Point nextPoint = new Point(openArray[currentPointID].X + moves[i].X, openArray[currentPointID].Y + moves[i].Y);
                        if (currentMap.canGo(nextPoint) && (whichList[nextPoint.X, nextPoint.Y] != closeMark))
                        {
                            if (whichList[nextPoint.X, nextPoint.Y] == openMark)
                            {
                                // Console.WriteLine("Point in OpenList is checked twice!");
                                //checkedIfBetterInOpennumber++;
                                if (gCost[openHeap[openPointToHeapID[nextPoint.X, nextPoint.Y]]] > gCost[currentPointID] + currentMap.map[nextPoint.X, nextPoint.Y])
                                {
                                    //changednumber++;
                                    changeOpenHeap(nextPoint, gCost[currentPointID] + currentMap.map[nextPoint.X, nextPoint.Y],
                                        gCost[currentPointID] + currentMap.map[nextPoint.X, nextPoint.Y] + getHCost(nextPoint, to));
                                    parents[nextPoint.X, nextPoint.Y] = i;
                                }
                            }
                            else
                            {
                                insertOpenHeap(nextPoint, to, gCost[currentPointID] + currentMap.map[nextPoint.X, nextPoint.Y]);
                                parents[nextPoint.X, nextPoint.Y] = i;
                            }
                        }
                    }
                }

                if (numberOfOpenListItems > 0)
                {
                    Point insertedPoint = to;
                    byte num;
                    while (insertedPoint != now)
                    {
                        path.Push(insertedPoint);
                        num = parents[insertedPoint.X, insertedPoint.Y];
                        insertedPoint.X -= moves[num].X;
                        insertedPoint.Y -= moves[num].Y;
                    }
                }
                else
                {
                    path.Clear();
                }
            }

            //  Console.WriteLine("totalSwapNumber is {5}\nhasBeenInOpenNumber is {4}\ncheckedIfBetterInOpennumber is {0}\nchanged number is {1}\ntotalCommpareNumber is {2}\nheapDepth is {3}", checkedIfBetterInOpennumber, changednumber, totalCommpareNumber, heapDepth, openID, totalSwapNumber);
            return path;
        }

        private void insertOpenHeapTEST(int num)
        {
            numberOfOpenListItems++;                //加入堆
            openHeap[numberOfOpenListItems] = num;        //插入二叉堆
            int m = numberOfOpenListItems;                                  //排序
            while (m != 1)
            {
                if (openHeap[m] <= openHeap[m / 2])
                {
                    heapSwap(m, m / 2);
                    m = m / 2;
                }
                else break;
            }
        }


        private int getMinTEST()                    //返回当前fCost最小的结点的ID，调整二叉堆
        {
            int minPointID = openHeap[1];

            openHeap[1] = openHeap[numberOfOpenListItems];
            numberOfOpenListItems--;

            int m;
            int smallest = 1;
            do
            {
                m = smallest;
                if (((m * 2) <= numberOfOpenListItems) && (openHeap[smallest] > openHeap[m * 2]))
                {
                    smallest = m * 2;
                }
                if (((m * 2) + 1 <= numberOfOpenListItems) && (openHeap[smallest] > openHeap[(m * 2) + 1]))
                {
                    smallest = (m * 2) + 1;
                }
                if (m != smallest)
                {
                    heapSwap(m, smallest);
                }
            } while (m != smallest);

            return minPointID;
        }

        private void changeOpenHeapTEST(int changePointID, int num)
        {
            openHeap[changePointID] = num;
            int m = changePointID;                                  //排序，m代表当前结点的ID
            while (m != 1)
            {
                if (openHeap[m] <= openHeap[m >> 1])
                {
                    heapSwap(m, m >> 1);
                    m = m >> 1;
                }
                else break;
            }

            int smallest = m;
            do
            {
                m = smallest;
                if (((m << 1) <= numberOfOpenListItems) && (openHeap[smallest] > openHeap[m << 1]))
                {
                    smallest = m << 1;
                }
                if (((m << 1) + 1 <= numberOfOpenListItems) && (openHeap[smallest] > openHeap[(m << 1) + 1]))
                {
                    smallest = (m << 1) + 1;
                }
                if (m != smallest)
                {
                    heapSwap(m, smallest);
                }
            } while (m != smallest);
        }


        public void test()
        {
            //numberOfOpenListItems = 0;
            //int Length = 50;
            //for (int i = Length; i > 0; i--)
            //{
            //    insertOpenHeapTEST(i);
            //}

            //for (int i = 0; i < Length; i++)
            //{
            //    Console.WriteLine(getMinTEST());
            //}

            int m = 1;
            for (int i = 0; i < 10; i++)
            {
                m = i;
            }
            Console.WriteLine(m);
        }
    }
}