astarpathfinder.cs

两种AstarPathFinder 一种DijkstraPathfinder BFS

using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;

namespace PathFinder
{
    class AstarPathFinder:PathFinder
    {
        private int[,] areaMark;
        private static Point[] moves = new Point[] { new Point(0, 1), new Point(1, 0), new Point(0, -1), new Point(-1, 0) };
        //       下              右              上                  左
        private byte[,] parents;
        private int[,] whichList;
        int openMark, closeMark;

        //private int[,] openArrayID;
        private int[,] cost;
        private int[] openHeap;
        private Point[] openArray;
        private int[] gCost;
        private int[] fCost;
        private int[,] openPointToHeapID;
        private Map currentMap;

        int numberOfOpenListItems;
        int openID;
        double hValue;

        public double HValue
        {
            get { return hValue; }
            set { hValue = value; }
        }


        public AstarPathFinder(Map myMap)
        {
            currentMap = myMap;
            cost = new int[myMap.Width, myMap.Height];
            areaMark = new int[myMap.Width, myMap.Height];       //标记区域，用来预判能否到达目标
            parents = new byte[myMap.Width, myMap.Height];      //父结点方向
            whichList = new int[myMap.Width, myMap.Height];    //标记在哪个队列
            openMark = 1; closeMark = 2;                //队列标志，每次寻路之前更新
            //openArrayID = new int[Map.width, Map.height];    //标记在OPEN队列里的编号，位置映射到编号，方便更新OPEN队列中的结点

            openPointToHeapID = new int[myMap.Width, myMap.Height];         //位置映射到在堆中的ID，区别于openArrayID
            HValue = 1;
            openHeap = new int[myMap.Width * myMap.Height];    //二叉堆的open队列，只存在openArray中的下标
            openArray = new Point[myMap.Width * myMap.Height + 1];    //open队列中存的结点，编号映射到位置
            gCost = new int[myMap.Width * myMap.Height + 1];    //open队列的当前实际代价
            fCost = new int[myMap.Width * myMap.Height + 1];    //open队列的估计代价
            numberOfOpenListItems = 0;                      //二叉堆的大小，从1开始计数，每次寻路之前归零
            openID = 0;                                     //编号，每次寻路之前归零
            iniAreaMark();
        }

        public void iniAreaMark()
        {
            int nodeNumber = currentMap.Width * currentMap.Height;
            //Console.WriteLine("nodeNumber is {0}", nodeNumber);
            for (int i = 0; i < currentMap.Width; i++)               //标记，-1表示没走过，0表示在check队列里等待检查
            {
                for (int j = 0; j < currentMap.Height; j++)
                {
                    areaMark[i, j] = -1;
                }
            }

            int checkMark = 1;
            Point currentPoint = new Point(0, 0);
            List<Point> checkList = new List<Point>();


            Point nowCheckPoint = new Point();
            while (nodeNumber > 0)
            {
                if (areaMark[currentPoint.X, currentPoint.Y] > -1)
                {
                    do
                    {
                        if (currentPoint.X == currentMap.Width - 1)
                        {
                            currentPoint.X = 0;
                            currentPoint.Y++;
                        }
                        else
                        {
                            currentPoint.X++;
                        }
                    } while (areaMark[currentPoint.X, currentPoint.Y] > -1);
                }
                //Console.WriteLine(currentPoint);

                checkList.Add(new Point(currentPoint.X, currentPoint.Y));

                while (checkList.Count > 0)
                {
                    nowCheckPoint = checkList[checkList.Count - 1];
                    checkList.RemoveAt(checkList.Count - 1);
                    if (currentMap.map[nowCheckPoint.X, nowCheckPoint.Y] > 3)
                    {
                        areaMark[nowCheckPoint.X, nowCheckPoint.Y] = int.MaxValue;
                    }
                    else
                    {
                        areaMark[nowCheckPoint.X, nowCheckPoint.Y] = checkMark;
                        for (int i = 0; i < moves.Length; i++)
                        {
                            Point nextCheckPoint = new Point(nowCheckPoint.X + moves[i].X, nowCheckPoint.Y + moves[i].Y);
                            if (currentMap.isIn(nextCheckPoint) && (areaMark[nextCheckPoint.X, nextCheckPoint.Y] < 0))
                            {
                                checkList.Add(nextCheckPoint);
                                areaMark[nextCheckPoint.X, nextCheckPoint.Y] = 0;
                                //Console.WriteLine(new Point(nowCheckPoint.X + moves[i].X, nowCheckPoint.Y + moves[i].Y));
                            }
                        }
                    }
                    nodeNumber--;
                }

                //Console.WriteLine("nodeNumber is {0}", nodeNumber);

                checkMark++;
            }

            //for (int i = 0; i < Map.width; i++)
            //{
            //    for (int j = 0; j < Map.height; j++)
            //    {
            //        if (areaMark[j, i] == int.MaxValue)
            //        {
            //            Console.Write(" .");
            //        }
            //        else
            //        {
            //            Console.Write(" " + areaMark[j, i]);
            //        }
            //    }
            //    Console.WriteLine();
            //}
        }

        private void heapSwap(int p, int q)
        {
            openPointToHeapID[openArray[openHeap[p]].X, openArray[openHeap[p]].Y] = q;
            openPointToHeapID[openArray[openHeap[q]].X, openArray[openHeap[q]].Y] = p;
            int temp = openHeap[p];
            openHeap[p] = openHeap[q];
            openHeap[q] = temp;

            //totalSwapNumber++;
        }

        private int getHCost(Point currentPoint, Point to)
        {
            // return Convert.ToInt32(Math.Floor(Math.Abs(currentPoint.X - to.X) + Math.Abs(currentPoint.Y - to.Y) * 2.1)) ;
            return Convert.ToInt32(Math.Abs(currentPoint.X - to.X) + Math.Abs(currentPoint.Y - to.Y) * HValue);
        }

        /// <summary>
        /// 插入OPEN队列的二叉树，为其分配OPEN队列的编号唯一标记，二叉树中只存编号，用数组存下各项代价
        /// </summary>
        /// <param name="currentPoint"></param>
        /// <param name="to"></param>
        /// <param name="currentGCost"></param>
        private void insertOpenHeap(Point currentPoint, Point to, int currentGCost)
        {
            whichList[currentPoint.X, currentPoint.Y] = openMark;   //标记进入OPEN队列
            openID++;                            //分配编号
            //openArrayID[currentPoint.X, currentPoint.Y] = openID;       //位置映射到编号

            numberOfOpenListItems++;                //加入堆
            openArray[openID] = currentPoint;        //加入OPEN队列
            openPointToHeapID[currentPoint.X, currentPoint.Y] = numberOfOpenListItems;
            gCost[openID] = currentGCost;        //求实际值
            fCost[openID] = currentGCost + getHCost(currentPoint, to);    //求估价后的值

            openHeap[numberOfOpenListItems] = openID;        //插入二叉堆
            int m = numberOfOpenListItems;                                  //排序
            while (m != 1)
            {
                if (fCost[openHeap[m]] <= fCost[openHeap[m >> 1]])
                {
                    heapSwap(m, m >> 1);
                    m = m >> 1;
                }
                else break;
            }

            //heapDepth = Math.Max(heapDepth, numberOfOpenListItems);
        }

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

            int m = 1;
            int smallest = 1;
            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);


            return minPointID;

        }

        /// <summary>
        /// 更新OPEN队列中ID为changePointID的结点的G, F, Parent，调整二叉堆
        /// </summary>
        /// <param name="changePointID"></param>
        /// <param name="changePointGCost"></param>