A*算法实现

最近实现了一下A*算法，感觉蛮好的，尤其是修改地图然后看电脑正确寻路后的那种成就感，有点像小时候蹲在地上，看着一堆蚂蚁搬家，然后故意在他们的路上设置障碍物，然后看蚂蚁不停的探索，然后重新找到新的路线的感觉，真是很有意思。
好！把代码记录在此，便于以后参考。

#include <iostream>
2

#include <string>
3

#include <vector>
4

#include <list>
5

#include <map>
6

#include <set>
7

using namespace std;
9

/************************************************************************/
11

/* 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

{
34

E_River=-2, //有河流，无法通过，在地图上用 ~ 标出
35

E_Wall=-1, //有墙，无法通过，在地图上用 | 标出
36

E_Road = 0, //没有障碍物，能最快的顺利通行，代价为0，在地图上用空格标出
37

E_Sand=1, //是沙地，能通过，但是相对难一些，代价为1，在地图上用 * 标出
38

};
39

struct 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

};
50

bool operator==(const Point& l,const Point & r)
51

{
52

return (l.x==r.x)&&(l.y==r.y);
53

}
54

bool 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

//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

//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

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

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

{
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
166

double 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
172

double 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
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

{
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

205

bool 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

304

int 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

}

最上面注释里面画的地图是为了简单演示地图最终的显示效果，其实代码二维数组给出的地图有趣多了。

posted on 2012-11-05 09:04 Tim 阅读(1827) 评论(0) 编辑收藏引用所属分类: 游戏编程、C/C++语言

只有注册用户登录后才能发表评论。
【推荐】100%开源！大型工业跨平台软件C++源码提供，建模，组态！

相关文章: A*算法实现 directX 3D游戏图形引擎编程 DirectDraw编程方法与技巧 BMP位图文件结构及VC操作

网站导航: 博客园 IT新闻 BlogJava 博问 Chat2DB 管理

无我

A*算法实现

导航

统计

公告

留言簿(9)

随笔分类(173)

IT

Life

搜索

积分与排名

最新随笔

最新评论

阅读排行榜