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