最近实现了一下A*算法,感觉蛮好的,尤其是修改地图然后看电脑正确寻路后的那种成就感,有点像小时候蹲在地上,看着一堆蚂蚁搬家,然后故意在他们的路上设置障碍物,然后看蚂蚁不停的探索,然后重新找到新的路线的感觉,真是很有意思。
好!把代码记录在此,便于以后参考。
1
#include <iostream>
2
#include <string>
3
#include <vector>
4
#include <list>
5
#include <map>
6
#include <set>
7![](/Images/OutliningIndicators/None.gif)
8
using namespace std;
9![](/Images/OutliningIndicators/None.gif)
10![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
/**//************************************************************************/
11![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
/**//* A*算法实现,测试A*算法地图
12
40X10的地图,B代表起始点,E代表终点;
13
空格代表能通过;|代表是墙,不能通过;
14
xx0123456789012345678901234567890123456789xx
15
xx————————————————————xx
16
0| |0
17
1| |1
18
2| | |2
19
3| | |3
20
4| | E |4
21
5| B | |5
22
6| | |6
23
7| |7
24
8| |8
25
9| |9
26
xx————————————————————xx
27
xx0123456789012345678901234567890123456789xx
28
/************************************************************************/
29
const static int X = 40;
30
const static int Y = 10;
31
//地图上的情况
32
enum E_Map
33![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
34
E_River=-2, //有河流,无法通过,在地图上用 ~ 标出
35
E_Wall=-1, //有墙,无法通过,在地图上用 | 标出
36
E_Road = 0, //没有障碍物,能最快的顺利通行,代价为0,在地图上用空格标出
37
E_Sand=1, //是沙地,能通过,但是相对难一些,代价为1,在地图上用 * 标出
38
};
39![](/Images/OutliningIndicators/None.gif)
40
struct Point
41![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
42
int x;
43
int y;
44![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
Point(int i=0,int j=0):x(i),y(j)
{}
45
bool operator==(const Point & r)
46![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
47
return (x==r.x)&&(y==r.y);
48
}
49
};
50
bool operator==(const Point& l,const Point & r)
51![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
52
return (l.x==r.x)&&(l.y==r.y);
53
}
54
bool operator<(const Point& l,const Point & r)
55![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
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![](/Images/OutliningIndicators/None.gif)
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![](/Images/OutliningIndicators/None.gif)
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![](/Images/OutliningIndicators/None.gif)
87![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
const 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![](/Images/OutliningIndicators/None.gif)
100
//打印地图
101
void PrintMap(const Point &B,const Point &E,const vector<Point>& path=vector<Point>());
102
void PrintMap(const Point &B,const Point &E,const vector<Point>& path)
103![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
104![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
int LastMap[Y][X] =
{0};
105
for (int y=0;y<Y;++y)
106![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
107
for (int x=0;x<X;++x)
108![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
116
LastMap[(*itr).y][(*itr).x] = '&';
117
}
118
//起始和目标
119
LastMap[B.y][B.x] = 'B';
120
LastMap[E.y][E.x] = 'E';
121![](/Images/OutliningIndicators/InBlock.gif)
122
cout<<"A*寻路路径为:"<<endl;
123
cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
124
cout<<"xx————————————————————xx"<<endl;
125
for (int y=0;y<Y;++y)
126![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
127
cout<<y<<"[";
128
for (int x=0;x<X;++x)
129![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
130
if (LastMap[y][x] == E_Road)
131![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
132
cout<<" ";
133
}
134
else if (LastMap[y][x] == E_Wall)
135![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
136
cout<<"|";
137
}
138
else if (LastMap[y][x] == E_River)
139![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
140
cout<<"~";
141
}
142
else if (LastMap[y][x] == E_Sand)
143![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
144
cout<<"*";
145
}
146
else if (LastMap[y][x] == 'B')
147![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
148
cout<<"B";
149
}
150
else if (LastMap[y][x] == 'E')
151![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
152
cout<<"E";
153
}
154
else if (LastMap[y][x] == '&')
155![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
156
cout<<"&";
157
}
158
}
159
cout<<"]"<<y<<endl;
160
}
161
cout<<"xx————————————————————xx"<<endl;
162
cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
163
}
164![](/Images/OutliningIndicators/None.gif)
165
//计算当前位置与终点位置的Hn
166
double Hn(const Point & E,const Point&p)
167![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
168
return abs(E.y-p.y) + abs(E.x-p.x);
169
}
170![](/Images/OutliningIndicators/None.gif)
171
//计算相邻两个位置(即包括对象线在内的周围8个坐标)的相对Gn
172
double Gg(const Point & p1,const Point& p2)
173![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
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![](/Images/OutliningIndicators/None.gif)
178
//探测位置p的下一步(p.x + addx,p.y + addy)的Gn,Hn
179
void 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![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
187
Point t = Point(x,y);
188
if (closeTbl.find(t) != closeTbl.end())
189![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
190
return;
191
}
192
//得到对应本次遍历的Gn
193
double dgn = mapGn[p] + Gg(p,t);
194
mapGnTemp[t] = dgn;
195![](/Images/OutliningIndicators/InBlock.gif)
196
map<Point,double>::iterator itr = mapGn.find(t);
197
if (itr == mapGn.end() || itr->second > dgn)
198![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
199
mapGn[t] = dgn;
200
}
201
HnPoint.insert(make_pair(Hn(E,t),t));
202
}
203
}
204![](/Images/OutliningIndicators/None.gif)
205
bool APath(const Point & B,const Point & E,vector<Point>&path)
206![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
217
Point p = openTbl.back();
218
openTbl.pop_back();
219
if (p == E)
220![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
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![](/Images/OutliningIndicators/InBlock.gif)
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
238
//无路可走了,只能一步一步回退。。。
239![](/Images/OutliningIndicators/InBlock.gif)
240
//PrintMap(B,E,path);
241
//标记该点已经被探测,使得下次不再被选择
242
closeTbl.insert(p);
243
//回退一步,重新探测之前走过的一个点
244
if (path.empty())
245![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
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![](/Images/OutliningIndicators/InBlock.gif)
255
//HnPoint的第一维就是从p点开始到达终点(Hn)最高效的周围坐标点pt
256
multimap<double,Point>::iterator itr = HnPoint.begin();
257
double hn = itr->first;
258
Point pt = itr->second;
259![](/Images/OutliningIndicators/InBlock.gif)
260
map<Point,double>::iterator itrFind = mapGn.find(pt);
261
while (itrFind != mapGn.end() && (itrFind->second) < mapGnTemp[pt])
262![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
263
//如果这个不是最优,则优先试探其他的相同Hn的路径
264
++itr;
265
if (itr != HnPoint.end() )
266![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
267
if (hn < itr->first)
268![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
280![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
/**//**************************************************************************************
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![](/Images/OutliningIndicators/InBlock.gif)
295
//pt没有被探测过,或者是最优选项,所以将p加入路径,然后下一步探测pt
296
openTbl.push_back(pt);
297![](/Images/OutliningIndicators/InBlock.gif)
298
closeTbl.insert(p);
299
path.push_back(p);
300
}
301
return !path.empty();
302
}
303![](/Images/OutliningIndicators/None.gif)
304
int main(int argc, char * argv[])
305![](/Images/OutliningIndicators/ExpandedBlockStart.gif)
![](/Images/OutliningIndicators/ContractedBlock.gif)
{
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![](/Images/OutliningIndicators/ExpandedSubBlockStart.gif)
{
313
cout<<"++++++找不到可达路径!+++++++++"<<endl;
314
}
315![](/Images/OutliningIndicators/InBlock.gif)
316
return 0;
317
}
最上面注释里面画的地图是为了简单演示地图最终的显示效果,其实代码二维数组给出的地图有趣多了。