A星寻路简单实现

思路参照地址：http://www.cnblogs.com/technology/archive/2011/05/26/2058842.html

using System;using System.Collections.Generic;using System.Linq;using System.Text;using System.Threading.Tasks;namespace Astar{    class Program    {        static void Main(string[] args)        {            Point begin = new Point();//起点            begin.x = 1;            begin.y = 1;//坐标            //定义目的地            Point end = new Astar.Point();//终点            end.x = 9;            end.y = 9;            AllOperation ao= new AllOperation();            ao.WayFinding(begin,end);           ao.PrintMap();//打印            Console.ReadKey();        }    }    class AllOperation    {        #region 地图        int[,] Map = new int[10, 10]//定义一个地图              {                    { 1,1,1,1,1,1,1,1,1,1},                     { 1,1,1,1,1,1,1,1,1,1},                     { 1,1,0,1,1,1,1,1,1,1},                     { 1,1,0,1,0,1,0,0,0,0},                     { 1,1,0,1,1,1,0,1,1,1},                     { 0,0,0,1,1,1,0,1,1,1},                     { 0,1,1,1,1,0,0,1,1,1},                     { 0,1,0,1,1,0,1,1,1,1},                     { 1,1,0,0,0,0,1,1,1,1},                     { 1,1,1,1,1,1,1,1,1,1}              };//1表示可通过无障碍                //两个集合保存可用和不可用的点        #endregion        List<Point> can = new List<Point>();        List<Point> cannot = new List<Point>();        public bool ThePointIsInCannot(int x,int y)        {            foreach (Point p in cannot)            {                if(p.x==x&&p.y==y)                    return true;            }            return false;        }        public bool ThePointIsInCan(int x, int y)        {            foreach (Point p in can)            {                if (p.x == x && p.y == y)                    return true;            }            return false;        }        public int GetG(Point p)        {            if (p.father == null) return 0;//指的是没有第一个点            if (p.x == p.father.x || p.y == p.father.y)                return p.father.G + 10;//表示上一个点到这个点的距离            else                return p.father.G + 14;//表示斜着的        }//得到G，得到H        public int GetH(Point p,Point end)        {            return Math.Abs(p.x - end.x)*10 + Math.Abs(p.y - end.y)*10;        }        public Point getMinGH()   //得到最小路径值，返回最小的点        {            Point pmin = null;            foreach (Point p in can)//遍历所有p值            {                if (pmin == null)                {                    pmin = p;                }                else {                    //说明已经有pmin值                    if (pmin.G + pmin.H > p.G + p.H) pmin = p;                                     }            }            return pmin;//返回路径最小的点        }        public Point GetPointFromCan(int x,int y)//按照坐标找到can集合中的点        {           foreach(Point p in can )                if (p.x==x&&p.y==y)            return p;            return null;          }        public void CheckAround(Point p0,int [,] map,Point a,Point b)//检查p0点周围的点，if没有初始化值那就先赋值，else         {            for (int i = p0.x - 1; i <= p0.x + 1; i++)            {                for (int j = p0.y - 1; j <= p0.y + 1; j++)                {                    //我们要遍历除了自身之外的8个点                    //而且这8个点要在地图内部                    if (i >= 0 && i < 10 && j >= 0 && j < 10 &&  !(i == p0.x && j == p0.y)     )                    {                        //第一轮可能会刷下去一波点                        //这轮我们要刷下去障碍的点和已经处在关闭集合里的点                        if ((map[i, j] != 0) && !ThePointIsInCannot(i, j))                        {                            //第二轮结束                            //如果这几个周围点不在开始列表里我们要先放进开始列表                            //如果在里面说明已经初始化点的信息了                            if (!ThePointIsInCan(i, j))                            {                                Point p = new Point();                                p.x = i;                                p.y = j;                                p.father = p0;//表示从0走过来的                                p.G = GetG(p);                                p.H = GetH(p,b);                                //初始化后我们要把这个点添加到can集合                                can.Add(p);                                Console.WriteLine("我们给can集合添加新的点，把周围的点给赋值 :"+i+"   "+j +"   "+p.G+"   "+p.H+"  父节点："+p.father.x+" "+p.father.y);                            }                            else                            {                                //说明此点已经处于can集合中，可以走一波                                Point p = GetPointFromCan(i,j);                                //找到了p点，                                int GGGG = 0;                                //判断参照点到这里的距离                                if (p0.x == p.x || p0.y == p.y)                                    GGGG = p0.G + 10;                                else                                    GGGG = p0.G + 14;                                if (GGGG < p.G)                                //如果新的g值比原来的小，那就说明原来的路线可以用现在的路线代替                                //否则说明这么走不省路费                                {                                    //todo//这个地方我们应该怎样控制呢                                 //   can.Remove(p);//我们要把这个点移除，因为信息更新了，路线更新了，源点发生改变，G点改变                                    p.father = p0;//p的父点换成p0                                    p.G = GGGG;//替换掉G值                                  }                            }                        }                                                }                                         }                }          }        public  void  WayFinding(Point a,Point b)        {            can.Add(a);//我们要把开始点添加到can集合            //while()            while (true)//我们要一直循环，直到所有的点都不在can集合里，说明没找到路                        //，或者  我们找到了终点            {                Console.Write("还在while里");                Point p0 = getMinGH();//得到距离最近的点                Console.WriteLine("我们本次找到的最合适的点是：" + p0.x + "   " + p0.y +"   G  H分别是多少呢 ？："+p0.G+"  "+p0.H+"   ");//+p0.father.x+ " "+p0.father.y                if (p0 == null)                {                    Console.WriteLine("我们没找到最小的");                    break;                }//没有点就是找不到路了                if (p0.x==b.x&&p0.y==b.y) {                    Console.WriteLine("终点现在包含在can集合里");                    break;                }                 can.Remove(p0);//删除那个最近距离的点                cannot.Add(p0);//在关闭列表中添加此值                CheckAround(p0, Map, a, b);//传递参数依次为参照点，地图，起始点，终点            }            //循环结束后我们要记录路径           Point p = GetPointFromCan(b.x, b.y);//找到终点的point            Map[b.x, b.y] = 5;            Map[a.x, a.y] = 4;                  while (p.father != null)            {                Console.WriteLine(p.x+"   "+p.y+"地图路线赋值");                p = p.father;//往起始点方向跳转                Map[p.x, p.y] = 2;//2就表示我们的路线            }            Map[b.x, b.y] = 5;//收尾赋值            Map[a.x, a.y] = 4;        }        public void PrintMap()        {            for (int i= 0; i < 10; i++)            {                for (int j = 0; j < 10; j++)                {                    if (Map[i, j] == 1) Console.Write("■");//█  ■▲▼♤※☹☺                    else if (Map[i, j] == 2) Console.Write("★");                    else if (Map[i, j] == 0) Console.Write("※");                    else if (Map[i, j] == 4) Console.Write("▲");                    else if (Map[i, j] == 5) Console.Write("yes");                  //  else Console.Write("  ");                }                Console.Write("\n");            }        }    }    public class Point    {        public int x, y;//坐标        public int G, H;//相关值        public Point father=null;//指向的对象    };}

运行结果：

还在while里我们本次找到的最合适的点是：1   1   G  H分别是多少呢 ？：0  0我们给can集合添加新的点，把周围的点给赋值 :0   0   14   180  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :0   1   10   170  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :0   2   14   160  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :1   0   10   170  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :1   2   10   150  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :2   0   14   160  父节点：1 1我们给can集合添加新的点，把周围的点给赋值 :2   1   10   150  父节点：1 1还在while里我们本次找到的最合适的点是：1   2   G  H分别是多少呢 ？：10  150我们给can集合添加新的点，把周围的点给赋值 :0   3   24   150  父节点：1 2我们给can集合添加新的点，把周围的点给赋值 :1   3   20   140  父节点：1 2我们给can集合添加新的点，把周围的点给赋值 :2   3   24   130  父节点：1 2还在while里我们本次找到的最合适的点是：2   3   G  H分别是多少呢 ？：24  130我们给can集合添加新的点，把周围的点给赋值 :1   4   38   130  父节点：2 3我们给can集合添加新的点，把周围的点给赋值 :2   4   34   120  父节点：2 3我们给can集合添加新的点，把周围的点给赋值 :3   3   34   120  父节点：2 3还在while里我们本次找到的最合适的点是：2   4   G  H分别是多少呢 ？：34  120我们给can集合添加新的点，把周围的点给赋值 :1   5   48   120  父节点：2 4我们给can集合添加新的点，把周围的点给赋值 :2   5   44   110  父节点：2 4我们给can集合添加新的点，把周围的点给赋值 :3   5   48   100  父节点：2 4还在while里我们本次找到的最合适的点是：3   5   G  H分别是多少呢 ？：48  100我们给can集合添加新的点，把周围的点给赋值 :2   6   62   100  父节点：3 5我们给can集合添加新的点，把周围的点给赋值 :4   4   62   100  父节点：3 5我们给can集合添加新的点，把周围的点给赋值 :4   5   58   90  父节点：3 5还在while里我们本次找到的最合适的点是：4   5   G  H分别是多少呢 ？：58  90我们给can集合添加新的点，把周围的点给赋值 :5   4   72   90  父节点：4 5我们给can集合添加新的点，把周围的点给赋值 :5   5   68   80  父节点：4 5还在while里我们本次找到的最合适的点是：5   5   G  H分别是多少呢 ？：68  80我们给can集合添加新的点，把周围的点给赋值 :6   4   82   80  父节点：5 5还在while里我们本次找到的最合适的点是：3   3   G  H分别是多少呢 ？：34  120我们给can集合添加新的点，把周围的点给赋值 :4   3   44   110  父节点：3 3还在while里我们本次找到的最合适的点是：4   4   G  H分别是多少呢 ？：48  100我们给can集合添加新的点，把周围的点给赋值 :5   3   62   100  父节点：4 4还在while里我们本次找到的最合适的点是：5   4   G  H分别是多少呢 ？：58  90我们给can集合添加新的点，把周围的点给赋值 :6   3   72   90  父节点：5 4还在while里我们本次找到的最合适的点是：6   4   G  H分别是多少呢 ？：68  80我们给can集合添加新的点，把周围的点给赋值 :7   3   82   80  父节点：6 4我们给can集合添加新的点，把周围的点给赋值 :7   4   78   70  父节点：6 4还在while里我们本次找到的最合适的点是：7   4   G  H分别是多少呢 ？：78  70还在while里我们本次找到的最合适的点是：2   5   G  H分别是多少呢 ？：44  110我们给can集合添加新的点，把周围的点给赋值 :1   6   58   110  父节点：2 5还在while里我们本次找到的最合适的点是：2   6   G  H分别是多少呢 ？：54  100我们给can集合添加新的点，把周围的点给赋值 :1   7   68   100  父节点：2 6我们给can集合添加新的点，把周围的点给赋值 :2   7   64   90  父节点：2 6还在while里我们本次找到的最合适的点是：4   3   G  H分别是多少呢 ？：44  110还在while里我们本次找到的最合适的点是：5   3   G  H分别是多少呢 ？：54  100我们给can集合添加新的点，把周围的点给赋值 :6   2   68   100  父节点：5 3还在while里我们本次找到的最合适的点是：6   3   G  H分别是多少呢 ？：64  90还在while里我们本次找到的最合适的点是：7   3   G  H分别是多少呢 ？：74  80还在while里我们本次找到的最合适的点是：2   7   G  H分别是多少呢 ？：64  90我们给can集合添加新的点，把周围的点给赋值 :1   8   78   90  父节点：2 7我们给can集合添加新的点，把周围的点给赋值 :2   8   74   80  父节点：2 7还在while里我们本次找到的最合适的点是：2   8   G  H分别是多少呢 ？：74  80我们给can集合添加新的点，把周围的点给赋值 :1   9   88   80  父节点：2 8我们给can集合添加新的点，把周围的点给赋值 :2   9   84   70  父节点：2 8还在while里我们本次找到的最合适的点是：2   9   G  H分别是多少呢 ？：84  70还在while里我们本次找到的最合适的点是：2   1   G  H分别是多少呢 ？：10  150我们给can集合添加新的点，把周围的点给赋值 :3   0   24   150  父节点：2 1我们给can集合添加新的点，把周围的点给赋值 :3   1   20   140  父节点：2 1还在while里我们本次找到的最合适的点是：1   3   G  H分别是多少呢 ？：20  140我们给can集合添加新的点，把周围的点给赋值 :0   4   34   140  父节点：1 3还在while里我们本次找到的最合适的点是：1   4   G  H分别是多少呢 ？：30  130我们给can集合添加新的点，把周围的点给赋值 :0   5   44   130  父节点：1 4还在while里我们本次找到的最合适的点是：1   5   G  H分别是多少呢 ？：40  120我们给can集合添加新的点，把周围的点给赋值 :0   6   54   120  父节点：1 5还在while里我们本次找到的最合适的点是：1   6   G  H分别是多少呢 ？：50  110我们给can集合添加新的点，把周围的点给赋值 :0   7   64   110  父节点：1 6还在while里我们本次找到的最合适的点是：1   7   G  H分别是多少呢 ？：60  100我们给can集合添加新的点，把周围的点给赋值 :0   8   74   100  父节点：1 7还在while里我们本次找到的最合适的点是：1   8   G  H分别是多少呢 ？：70  90我们给can集合添加新的点，把周围的点给赋值 :0   9   84   90  父节点：1 8还在while里我们本次找到的最合适的点是：1   9   G  H分别是多少呢 ？：80  80还在while里我们本次找到的最合适的点是：3   1   G  H分别是多少呢 ？：20  140我们给can集合添加新的点，把周围的点给赋值 :4   0   34   140  父节点：3 1我们给can集合添加新的点，把周围的点给赋值 :4   1   30   130  父节点：3 1还在while里我们本次找到的最合适的点是：4   1   G  H分别是多少呢 ？：30  130还在while里我们本次找到的最合适的点是：6   2   G  H分别是多少呢 ？：68  100我们给can集合添加新的点，把周围的点给赋值 :6   1   78   110  父节点：6 2我们给can集合添加新的点，把周围的点给赋值 :7   1   82   100  父节点：6 2还在while里我们本次找到的最合适的点是：0   2   G  H分别是多少呢 ？：14  160还在while里我们本次找到的最合适的点是：2   0   G  H分别是多少呢 ？：14  160还在while里我们本次找到的最合适的点是：0   3   G  H分别是多少呢 ？：24  150还在while里我们本次找到的最合适的点是：3   0   G  H分别是多少呢 ？：24  150还在while里我们本次找到的最合适的点是：0   4   G  H分别是多少呢 ？：34  140还在while里我们本次找到的最合适的点是：0   5   G  H分别是多少呢 ？：44  130还在while里我们本次找到的最合适的点是：0   6   G  H分别是多少呢 ？：54  120还在while里我们本次找到的最合适的点是：0   7   G  H分别是多少呢 ？：64  110还在while里我们本次找到的最合适的点是：0   8   G  H分别是多少呢 ？：74  100还在while里我们本次找到的最合适的点是：0   9   G  H分别是多少呢 ？：84  90还在while里我们本次找到的最合适的点是：4   0   G  H分别是多少呢 ？：34  140还在while里我们本次找到的最合适的点是：0   1   G  H分别是多少呢 ？：10  170还在while里我们本次找到的最合适的点是：1   0   G  H分别是多少呢 ？：10  170还在while里我们本次找到的最合适的点是：7   1   G  H分别是多少呢 ？：82  100我们给can集合添加新的点，把周围的点给赋值 :8   0   96   100  父节点：7 1我们给can集合添加新的点，把周围的点给赋值 :8   1   92   90  父节点：7 1还在while里我们本次找到的最合适的点是：8   1   G  H分别是多少呢 ？：92  90我们给can集合添加新的点，把周围的点给赋值 :9   0   106   90  父节点：8 1我们给can集合添加新的点，把周围的点给赋值 :9   1   102   80  父节点：8 1我们给can集合添加新的点，把周围的点给赋值 :9   2   106   70  父节点：8 1还在while里我们本次找到的最合适的点是：9   2   G  H分别是多少呢 ？：106  70我们给can集合添加新的点，把周围的点给赋值 :9   3   116   60  父节点：9 2还在while里我们本次找到的最合适的点是：9   3   G  H分别是多少呢 ？：116  60我们给can集合添加新的点，把周围的点给赋值 :9   4   126   50  父节点：9 3还在while里我们本次找到的最合适的点是：9   4   G  H分别是多少呢 ？：126  50我们给can集合添加新的点，把周围的点给赋值 :9   5   136   40  父节点：9 4还在while里我们本次找到的最合适的点是：9   5   G  H分别是多少呢 ？：136  40我们给can集合添加新的点，把周围的点给赋值 :8   6   150   40  父节点：9 5我们给can集合添加新的点，把周围的点给赋值 :9   6   146   30  父节点：9 5还在while里我们本次找到的最合适的点是：9   6   G  H分别是多少呢 ？：146  30我们给can集合添加新的点，把周围的点给赋值 :8   7   160   30  父节点：9 6我们给can集合添加新的点，把周围的点给赋值 :9   7   156   20  父节点：9 6还在while里我们本次找到的最合适的点是：9   7   G  H分别是多少呢 ？：156  20我们给can集合添加新的点，把周围的点给赋值 :8   8   170   20  父节点：9 7我们给can集合添加新的点，把周围的点给赋值 :9   8   166   10  父节点：9 7还在while里我们本次找到的最合适的点是：9   8   G  H分别是多少呢 ？：166  10我们给can集合添加新的点，把周围的点给赋值 :8   9   180   10  父节点：9 8我们给can集合添加新的点，把周围的点给赋值 :9   9   176   0  父节点：9 8还在while里我们本次找到的最合适的点是：9   9   G  H分别是多少呢 ？：176  0终点现在包含在can集合里9   9地图路线赋值9   8地图路线赋值9   7地图路线赋值9   6地图路线赋值9   5地图路线赋值9   4地图路线赋值9   3地图路线赋值9   2地图路线赋值8   1地图路线赋值7   1地图路线赋值6   2地图路线赋值5   3地图路线赋值4   3地图路线赋值3   3地图路线赋值2   3地图路线赋值1   2地图路线赋值

地图截图：