A*地图寻路算法实现

上一段时间在学习WPF，用到一位前辈讲解的A*地图算法，于是自己根据算法实现了一下，具体代码如下，可供大家使用，代码大家可以再优化，具体用法就不写了，注释很清楚，另外提供源码下载：https://files.cnblogs.com/wangweixznu/PathFinder.rar 具体算法大家可参考深蓝翻译的http://data.gameres.com/message.asp?TopicID=25439：

 

  1using System;
  2using System.Collections.Generic;
  3using System.Text;
  4using System.Drawing;
  5
  6namespace PathFinder
  7{
  8    public class PathFinder
  9    {
 10        private PathNode startNode, endNode, currentNode;//开始点、终点、当前节点
 11        private byte[,] map;//矩阵地图0表示可行、1表示障碍物
 12        private List<PathNode> closePath, openPath;//  关闭列表、开启列表
 13        private List<Point> bestPaht;//最终保存最佳路径
 14        private bool findSuccess = false;//是否寻路成功
 15        /**//// <summary>
 16        /// 开始寻路
 17        /// </summary>
 18        /// <param name="map">二维地图矩阵</param>
 19        /// <param name="startPoint">开始坐标点</param>
 20        /// <param name="endPoint">终点</param>
 21        /// <returns></returns>
 22        public List<Point> FindBestPaht(byte[,] map, Point startPoint, Point endPoint)
 23        {
 24            closePath = new List<PathNode>();
 25            openPath = new List<PathNode>();
 26            bestPaht = new List<Point>();
 27            this.map = map;
 28            startNode = new PathNode(startPoint.X, startPoint.Y);
 29            startNode.ParentPoint = new Point(-1, -1);
 30            endNode = new PathNode(endPoint.X, endPoint.Y);
 31            currentNode = startNode;//开始节点设置为默认当前节点
 32            currentNode.ParentG = currentNode.G = 0;
 33            openPath.Add(startNode);//添加到开启列表
 34            TryToFindPaht(startNode);//开始寻找路径
 35            if (findSuccess)
 36            {
 37                InsertPahtNode(endNode);//如果寻找成功，则从终点开始从关闭列表中把最近路径添加到最佳路径列表
 38                return bestPaht;
 39            }
 40            else
 41                return null;
 42        }
 43        /**//// <summary>
 44        /// 尝试寻路
 45        /// </summary>
 46        /// <param name="node">开始节点</param>
 47        private void TryToFindPaht(PathNode node)
 48        {
 49            if (openPath.Count > 0)
 50            {
 51                closePath.Add(openPath[0]);//每次从开启列表中取第一个添加到关闭列表，注意这个列表已经是排序后的，第一个既是F值最小的
 52                openPath.RemoveAt(0);//把该节点从开启列表中移除
 53            }
 54            AddOpenNode(node);//以这个节点为基准，把它四周的节点添加到开启列表中，排除障碍物、已经在开启列表或关闭列表中的
 55           //当目标节点已经在开启列表中时表示寻路成功
 56            if (IsInOpenPaht(endNode))
 57            {
 58                closePath.Add(endNode);//将目标节点添到关闭列表中
 59                endNode.ParentPoint = currentNode.Point;//目标节点的父节点为当前节点
 60                findSuccess = true;
 61                return;
 62            }
 63            SortPathByF();//按F值从小到大排序
 64            if (openPath.Count > 0)
 65            {
 66                currentNode = openPath[0];//从开启列表中取F值最小的作为当前节点
 67                TryToFindPaht(currentNode);//递归调用，再次寻路
 68            }
 69        }
 70        /**//// <summary>
 71        /// 把当前节点四周的八个节点尝试全部添加到开启列表中
 72        /// </summary>
 73        /// <param name="node">当前节点</param>
 74        private void AddOpenNode(PathNode node)
 75        {
 76            AddOneOpenNode(node.X, node.Y + 1, node.Point);
 77            AddOneOpenNode(node.X, node.Y - 1, node.Point);
 78            AddOneOpenNode(node.X + 1, node.Y, node.Point);
 79            AddOneOpenNode(node.X - 1, node.Y, node.Point);
 80            AddOneOpenNode(node.X - 1, node.Y - 1, node.Point);
 81            AddOneOpenNode(node.X - 1, node.Y + 1, node.Point);
 82            AddOneOpenNode(node.X + 1, node.Y + 1, node.Point);
 83            AddOneOpenNode(node.X + 1, node.Y - 1, node.Point);
 84        }
 85        /**//// <summary>
 86        /// 添加一个新节点到开启列表中
 87        /// </summary>
 88        /// <param name="x">新节点X坐标</param>
 89        /// <param name="y">新节点Y坐标</param>
 90        /// <param name="point">父节点坐标</param>
 91        private void AddOneOpenNode(int x, int y, Point point)
 92        {
 93            PathNode node = new PathNode(x, y);
 94            //添加前提是:x、y在当前地图二维矩阵范围内、且是可行路径、不再关闭列表中也不再开启列表中
 95            if (x >= 0 && y >= 0 && x <= map.GetUpperBound(1) && y <= map.GetUpperBound(0) && map[y, x] == 0 && !IsInClosePaht(node) && !IsInOpenPaht(node))
 96            {
 97                node.X = x;
 98                node.Y = y;
 99                node.ParentPoint = point;// 新节点的父节点为当前节点
100                node.ParentG = currentNode.G;//记录新节点的父节点G值、以便与当前节点做比较
101                node.G = (currentNode.X == node.X || currentNode.Y == node.Y ? 10 : 14) + node.ParentG;//当前节点的G值为父节点G值+10(非对角线)或14 (对角线)
102                node.H = 10 * (Math.Abs(endNode.X - node.X) + Math.Abs(endNode.Y - node.Y));//更新H值
103                openPath.Add(node);
104            }
105        }
106        /**//// <summary>
107        /// 从关闭列表中，从目标点开始倒着查找所有父节点并将其添加到最佳路径中
108        /// </summary>
109        /// <param name="node"></param>
110        private void InsertPahtNode(PathNode node)
111        {
112            bestPaht.Insert(0, node.Point);
113            foreach (PathNode item in closePath)
114            {
115                if (item.Point == node.ParentPoint)
116                    InsertPahtNode(item);
117            }
118        }
119   /**//// <summary>
120   /// 按F值从小到大排序
121   /// </summary>
122        private void SortPathByF()
123        {
124            for (int i = 0; i < openPath.Count; i++)
125            {
126                for (int j = 0; j < openPath.Count - i - 1; j++)
127                {
128                    if (openPath[j].F > openPath[j + 1].F)
129                    {
130                        PathNode temp = openPath[j + 1];
131                        openPath[j + 1] = openPath[j];
132                        openPath[j] = temp;
133                    }
134                }
135            }
136        }
137    /**//// <summary>
138    /// 判断某个阶段是否在关闭列表中
139    /// </summary>
140    /// <param name="node"></param>
141    /// <returns></returns>
142        private bool IsInClosePaht(PathNode node)
143        {
144            foreach (PathNode item in closePath)
145            {
146                if (item.X == node.X && item.Y == node.Y)
147                    return true;
148            }
149            return false;
150        }
151        /**//// <summary>
152        /// 判断某个节点是否在开启列表中
153        /// </summary>
154        /// <param name="node"></param>
155        /// <returns></returns>
156        private bool IsInOpenPaht(PathNode node)
157        {
158            foreach (PathNode item in openPath)
159            {
160                if (item.X == node.X && item.Y == node.Y)
161                    return true;
162            }
163            return false;
164        }
165    }
166    /**//// <summary>
167    /// 定义节点类
168    /// </summary>
169    public class PathNode
170    {
171        public PathNode(int x, int y)
172        {
173            this.X = x;
174            this.Y = y;
175        }
176        /**//// <summary>
177        /// 节点坐标
178        /// </summary>
179        public Point Point { get { return new Point(X,Y); } }
180        /**//// <summary>
181        ///  父节点坐标
182        /// </summary>
183        public Point ParentPoint { get; set; }
184        /**//// <summary>
185        /// X坐标点
186        /// </summary>
187        public int X { get; set; }
188        /**//// <summary>
189        /// Y坐标点
190        /// </summary>
191        public int Y { get; set; }
192        /**//// <summary>
193        /// F值=G+H
194        /// </summary>
195        public int F { get { return G + H; } }
196        /**//// <summary>
197        /// G值，即从起始节点到当前节点的距离，直线规定为10，对角线规定为14
198        /// </summary>
199        public int G { get; set; }
200        /**//// <summary>
201        /// 从当前节点到目标节点纵向坐标距离和横向坐标距离之和
202        /// </summary>
203        public int H { get; set; }
204        /**//// <summary>
205        /// 该节点父节点的G值
206        /// </summary>
207        public int ParentG { get; set; }
208    }
209}
210