最近實現了一下A*算法，感覺蠻好的，尤其是修改地圖然后看電腦正確尋路后的那種成就感，有點像小時候蹲在地上，看著一堆螞蟻搬家，然后故意在他們的路上設置障礙物，然后看螞蟻不停的探索，然后重新找到新的路線的感覺，真是很有意思。
好！把代碼記錄在此，便于以后參考。
1
#include <iostream>
2#include <string>
3#include <vector>
4#include <list>
5#include <map>
6#include <set>
7
8using namespace std;
9
10
/**//************************************************************************/
11/**//* A*算法實現，測試A*算法地圖
12
40X10的地圖，B代表起始點，E代表終點；
13空格代表能通過；|代表是墻，不能通過；
14xx0123456789012345678901234567890123456789xx
15xx————————————————————xx
160| |0
171| |1
182| | |2
193| | |3
204| | E |4
215| B | |5
226| | |6
237| |7
248| |8
259| |9
26xx————————————————————xx
27xx0123456789012345678901234567890123456789xx
28
/************************************************************************/
29const static int X = 40;
30const static int Y = 10;
31//地圖上的情況
32enum E_Map
33
{
34 E_River=-2, //有河流，無法通過，在地圖上用 ~ 標出
35 E_Wall=-1, //有墻，無法通過，在地圖上用 | 標出
36 E_Road = 0, //沒有障礙物，能最快的順利通行，代價為0，在地圖上用空格標出
37 E_Sand=1, //是沙地，能通過，但是相對難一些，代價為1，在地圖上用 * 標出
38};
39
40struct Point
41{
42 int x;
43 int y;
44
Point(int i=0,int j=0):x(i),y(j){}
45 bool operator==(const Point & r)
46 {
47 return (x==r.x)&&(y==r.y);
48
}
49};
50bool operator==(const Point& l,const Point & r)
51{
52 return (l.x==r.x)&&(l.y==r.y);
53}
54bool operator<(const Point& l,const Point & r)
55{
56 if(l.y<r.y) return true;
57 else if(l.y>r.y) return false;
58 else return (l.x < r.x);
59}
60
61//const static int GameMap[Y][X] = {
62// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
63// 0,0,-2,-2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
64// 0,0,0,-2,-2,-2,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
65// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
66// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
67// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
68// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
69// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
70// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
71// 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
72//};
73
74//const static int GameMap[Y][X] = {
75// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
76// 0,0,-2,-2,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
77// 0,0,0,-2,-2,-2,0,0,-1,0,-1,0,0,0,0,0,0,-1,-1,0,0,0,-1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
78// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,-1,0,0,0,0,0,-1,-1,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
79// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
80// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
81// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
82// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,
83// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
84// 0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
85//};
86
87const static int GameMap[Y][X] = {
88 0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
89 0,0,-2,-2,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
90 0,0,0,-2,-2,-2,0,-1,-1,0,0,0,0,0,0,0,0,-1,-1,0,0,0,-1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
91 0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,-1,-1,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
92 0,0,0,0,0,-1,0,0,-1,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
93 0,0,0,0,0,-1,0,0,-1,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
94 0,0,0,0,0,-1,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
95 0,0,0,0,0,-1,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,
96 0,0,0,0,0,0,-1,0,0,-1,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
97 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
98};
99
100//打印地圖
101void PrintMap(const Point &B,const Point &E,const vector<Point>& path=vector<Point>());
102void PrintMap(const Point &B,const Point &E,const vector<Point>& path)
103{
104 int LastMap[Y][X] = {0};
105 for (int y=0;y<Y;++y)
106 {
107 for (int x=0;x<X;++x)
108 {
109 LastMap[y][x] = GameMap[y][x];
110 }
111 }
112 //路徑
113 vector<Point>::const_iterator itr = path.begin();
114 for (;itr != path.end();++itr)
115 {
116 LastMap[(*itr).y][(*itr).x] = '&';
117 }
118 //起始和目標
119 LastMap[B.y][B.x] = 'B';
120 LastMap[E.y][E.x] = 'E';
121
122 cout<<"A*尋路路徑為："<<endl;
123 cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
124 cout<<"xx————————————————————xx"<<endl;
125 for (int y=0;y<Y;++y)
126 {
127 cout<<y<<"[";
128 for (int x=0;x<X;++x)
129 {
130 if (LastMap[y][x] == E_Road)
131 {
132 cout<<" ";
133 }
134 else if (LastMap[y][x] == E_Wall)
135 {
136 cout<<"|";
137 }
138 else if (LastMap[y][x] == E_River)
139 {
140 cout<<"~";
141 }
142 else if (LastMap[y][x] == E_Sand)
143 {
144 cout<<"*";
145 }
146 else if (LastMap[y][x] == 'B')
147 {
148 cout<<"B";
149 }
150 else if (LastMap[y][x] == 'E')
151 {
152 cout<<"E";
153 }
154 else if (LastMap[y][x] == '&')
155 {
156 cout<<"&";
157 }
158 }
159 cout<<"]"<<y<<endl;
160 }
161 cout<<"xx————————————————————xx"<<endl;
162 cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
163}
164
165//計算當前位置與終點位置的Hn
166double Hn(const Point & E,const Point&p)
167{
168 return abs(E.y-p.y) + abs(E.x-p.x);
169}
170
171//計算相鄰兩個位置(即包括對象線在內的周圍8個坐標)的相對Gn
172double Gg(const Point & p1,const Point& p2)
173{
174 double d = GameMap[p2.y][p2.x];
175 return ((p1.x-p2.x)!=0&&(p1.y-p2.y)!=0)?(1.5+d):(1.0+d);
176}
177
178//探測位置p的下一步(p.x + addx,p.y + addy)的Gn,Hn
179void testNext(const Point & E,const Point&p,const set<Point>& closeTbl,
180 map<Point,double> &mapGn,map<Point,double> &mapGnTemp,
181 multimap<double,Point> &HnPoint,int addx,int addy)
182{
183 int x = p.x + addx;
184 int y = p.y + addy;
185 if (x>=0 && y>=0 && x<X && y<Y && GameMap[y][x]>=0)
186 {
187 Point t = Point(x,y);
188 if (closeTbl.find(t) != closeTbl.end())
189 {
190 return;
191 }
192 //得到對應本次遍歷的Gn
193 double dgn = mapGn[p] + Gg(p,t);
194 mapGnTemp[t] = dgn;
195
196 map<Point,double>::iterator itr = mapGn.find(t);
197 if (itr == mapGn.end() || itr->second > dgn)
198 {
199 mapGn[t] = dgn;
200 }
201 HnPoint.insert(make_pair(Hn(E,t),t));
202 }
203}
204
205bool APath(const Point & B,const Point & E,vector<Point>&path)
206{
207 //A*算法：Fn = Gn + Hn
208 path.clear();
209 vector<Point> openTbl;
210 openTbl.push_back(B);
211 set<Point> closeTbl;
212 closeTbl.insert(B);
213 map<Point,double> mapGn;
214 mapGn[B] = 0;
215 while(!openTbl.empty())
216 {
217 Point p = openTbl.back();
218 openTbl.pop_back();
219 if (p == E)
220 {
221 path.push_back(E);
222 break;
223 }
224 //multimap<double,Point> FnPoint;
225 multimap<double,Point> HnPoint; //當前位置周圍所有可選擇的位置到終點的Hn
226 map<Point,double> mapGnTemp; //當前位置周圍所有可選擇的位置到起點的Gn
227
228 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,-1); //左上位置
229 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,0); //左邊位置
230 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,1); //左下位置
231 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,0,-1); //上面位置
232 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,0,1); //下面位置
233 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,-1); //右上位置
234 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,0); //右邊位置
235 testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,1); //右下位置
236 if (HnPoint.empty())
237 {
238 //無路可走了，只能一步一步回退。。。
239
240 //PrintMap(B,E,path);
241 //標記該點已經被探測，使得下次不再被選擇
242 closeTbl.insert(p);
243 //回退一步，重新探測之前走過的一個點
244 if (path.empty())
245 {
246 return false;
247 }
248 p = path.back();
249 path.pop_back();
250 closeTbl.erase(p);
251 openTbl.push_back(p);
252 continue;
253 }
254
255 //HnPoint的第一維就是從p點開始到達終點（Hn）最高效的周圍坐標點pt
256 multimap<double,Point>::iterator itr = HnPoint.begin();
257 double hn = itr->first;
258 Point pt = itr->second;
259
260 map<Point,double>::iterator itrFind = mapGn.find(pt);
261 while (itrFind != mapGn.end() && (itrFind->second) < mapGnTemp[pt])
262 {
263 //如果這個不是最優，則優先試探其他的相同Hn的路徑
264 ++itr;
265 if (itr != HnPoint.end() )
266 {
267 if (hn < itr->first)
268 {
269 break;
270 }
271 pt = itr->second;
272 itrFind = mapGn.find(pt);
273 }
274 else
275 break;
276 }
277 //判斷pt是否被探測過
278 if (itrFind != mapGn.end() && (itrFind->second) < mapGnTemp[pt])
279 {
280 /**//**************************************************************************************
281 pt已經被探測過，并且之前的Gn更小，說明可以用更小的代價到達pt。
282 所以說明我們之前選擇的p點不是最佳選擇，首先應該標記p無效，然后回退到之前的坐標重新選擇！
283 ****************************************************************************************/
284 //PrintMap(B,E,path);
285 //標記該點已經被探測，使得下次不再被選擇
286 closeTbl.insert(p);
287 //回退一步，重新探測之前走過的一個點
288 p = path.back();
289 path.pop_back();
290 closeTbl.erase(p);
291 openTbl.push_back(p);
292 continue;
293 }
294
295 //pt沒有被探測過，或者是最優選項，所以將p加入路徑，然后下一步探測pt
296 openTbl.push_back(pt);
297
298 closeTbl.insert(p);
299 path.push_back(p);
300 }
301 return !path.empty();
302}
303
304int main(int argc, char * argv[])
305{
306 const Point B(4,5),E(36,4);
307 vector<Point> path;
308 bool bFind = APath(B,E,path);
309
310 PrintMap(B,E,path);
311 if (!bFind)
312 {
313 cout<<"++++++找不到可達路徑!+++++++++"<<endl;
314 }
315
316 return 0;
317}
最上面注釋里面畫的地圖是為了簡單演示地圖最終的顯示效果，其實代碼二維數組給出的地圖有趣多了。