A*寻路算法 C++实现

头文件：AStarPathFinding

 1 #ifndef ASTARPATHFINDING_H

 2 #define ASTARPATHFINDING_H

 3 

 4 #include <queue>//为了使用优先级队列priority_queue

 5 #include <stack>

 6 #include <vector>

 7 

 8 //迷宫地图中节点类型标记

 9 enum{

10  NODE_EMPTY,//可以通过的节点

11  NODE_OBSTACLE,//障碍物，不可通过

12  NODE_PATH//路径上的点

13 };

14 

15 //记录路径上的点的坐标

16 typedef struct tagpathNode{

17  int x,y;

18 }PathNode;

19 

20 //节点数据结构定义

21 typedef struct tagNode{

22  int x,y;//当前点在迷宫中的位置坐标

23  int g;//起始点到当前点实际代价

24  int h;//当前节点到目标节点最佳路径的估计代价

25  int f;//估计函数：f = g + h。

26  struct tagNode *father;//指向其父节点的指针

27 }Node;

28 

29 //定义STL优先队列的排序方式

30 class HeapCompare_f{

31 public:

32  bool operator()(Node* x,Node* y) const

33  {

34   return x->f > y->f;//依据估价函数进行排序：升序排列

35  }

36 };

37 

38 //迷宫寻路：A*算法

39 class AStarPathFinding{

40 public:

41 

42 private:

43  char *m_mapFileName;//存储地图信息的文件名

44  int m_rows,m_cols;//迷宫的高度和宽度

45  int **m_maze;//迷宫布局

46  int m_startX,m_startY;//起始点坐标

47  int m_endX,m_endY;//目标点坐标

48  int dx[8],dy[8];//8个子节点移动方向：上、下、左、右、左上、右上、右下、左下

49 

50  Node *startNode,*endNode;//起始节点和目标节点

51  int **m_path;//记录路径信息

52  int m_steps;//搜索所花费的总步数

53 

54  //OPEN表：采用C++ STL中vector实现优先级队列功能

55  //注意：存储的是Node*指针

56  std::vector<Node*> OPENTable;

57  //CLOSED表：存储的也是Node*指针

58  std::vector<Node*> CLOSEDTable;

59 

60 public:

61  //构造函数

62  AStarPathFinding(char* mapFileName);

63  ~AStarPathFinding();//析构函数

64  void init();//初始化

65  //读取地图信息

66  bool readMap();

67  //寻路主函数

68  bool pathFinding();

69  //产生路径信息

70  void generatePath();

71  //打印路径信息

72  void printPath();

73  //估计当前点到目标点的距离：曼哈顿距离

74  int judge(int x,int y);

75  //判断某一节点是否合法

76  bool isIllegle(int x,int y);

77 };

78 

79 #endif

源文件：

  1 #include "AStarPathFinding.h"

  2 

  3 #include <iostream>

  4 #include <cstdio>

  5 #include <cmath>

  6 #include <string>

  7 #include <fstream>

  8 

  9 using namespace std;

 10 

 11 const int MaxDistance = 9999;

 12 

 13 AStarPathFinding::AStarPathFinding(char* mapFileName)

 14 :m_steps(0)

 15 {

 16  m_mapFileName = (char *)malloc((strlen(mapFileName) + 1) * sizeof(char));

 17  strcpy(m_mapFileName,mapFileName);

 18 }

 19 

 20 AStarPathFinding::~AStarPathFinding()

 21 {

 22  free(m_mapFileName);

 23 

 24  //千万不能有这句代码，因为startNode已加入OPEN表，会在释放OPEN表

 25  //的时候释放，否则会造成重复释放，出现bug

 26  //delete startNode;

 27  delete endNode;

 28 

 29  ////释放迷宫布局数组：注意多维数组空间释放

 30  for (int i = 0;i < m_rows;++i)

 31  {

 32   delete[] m_maze[i];

 33  }

 34  delete[] m_maze;

 35 

 36  for (int i = 0;i < m_rows;++i)

 37  {

 38   delete[] m_path[i];

 39  }

 40  delete[] m_path;

 41 

 42  //释放OPEN表以及CLOSED表内存空间

 43  vector<Node*>::iterator iter;

 44  for (iter = OPENTable.begin();iter != OPENTable.end();++iter)

 45  {

 46   delete (*iter);

 47  }

 48  OPENTable.clear();

 49 

 50  vector<Node*>::iterator iter2;

 51  for (iter2 = CLOSEDTable.begin();iter2 != CLOSEDTable.end();++iter2)

 52  {

 53   delete (*iter2);

 54  }

 55  CLOSEDTable.clear();

 56 }

 57 

 58 void AStarPathFinding::init()

 59 {

 60  dx[0] =dx[4] = dx[5] = -1;

 61  dx[1] =dx[3] = 0;

 62  dx[2] =dx[6] = dx[7] = 1;

 63 

 64  dy[3] = dy[4] = dy[7] = -1;

 65  dy[0] =dy[2] = 0;

 66  dy[1] =dy[5] = dy[6] = 1;

 67 

 68  readMap();

 69 

 70  //分配空间

 71  m_path = new int *[m_rows];

 72  for (int i = 0;i < m_rows;++i)

 73  {

 74   m_path[i] = new int[m_cols];

 75  }

 76 

 77  startNode = new Node;

 78  startNode->x = m_startX;

 79  startNode->y = m_startY;

 80  startNode->g = 0;

 81  startNode->h = judge(startNode->x,startNode->y);

 82  startNode->f = startNode->g + startNode->h;

 83  startNode->father = NULL;

 84 

 85  endNode = new Node;

 86  endNode->x = m_endX;

 87  endNode->y = m_endY;

 88  endNode->father = NULL;

 89 }

 90 

 91 bool AStarPathFinding::pathFinding()

 92 {

 93  //判断起始点和目标点是否是同一点

 94  if (m_startX == m_endX && m_startY == m_endY)

 95  {

 96   cout << "WARNNING : The start point is the same as th destination " << endl;

 97   return true;

 98  }

 99 

100  OPENTable.push_back(startNode);//起始点装入OPEN表

101 

102  //对vector中元素进行排序：将最后一个元素加入原本已序的heap内

103  push_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());

104 

105  Node *tempNode = new Node;

106 

107  //开始遍历

108  for (;;)

109  {

110   if (OPENTable.empty())//判断OPEN表是否为空

111   {

112    cout << "ERROR : unable to find the destination" << endl;

113    return false;

114   }

115 

116   tempNode = OPENTable.front();//注意：OPEN表为空会导致未定义行为

117   ++m_steps;

118   //将第一个元素移到最后，并将剩余区间重新排序，组成新的heap

119   pop_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());

120   OPENTable.pop_back();//删除最后一个元素

121 

122   //判断是否已经搜寻到目标节点

123   if (tempNode->x == m_endX && tempNode->y == m_endY)

124   {

125    cout << "OK : success to find the destination" << endl;

126    endNode->g = tempNode->g;

127    endNode->h = tempNode->h;

128    endNode->f = tempNode->f;

129    endNode->father = tempNode->father;

130 

131    generatePath();

132 

133    return true;

134   }

135   

136   for (int i = 0;i < 8;++i)//针对每个子节点

137   {

138    int nextX = tempNode->x + dx[i];

139    int nextY = tempNode->y + dy[i];

140    if (isIllegle(nextX,nextY))

141    {

142     //注意：障碍物角落不能直接通过

143     if (1 == *(*(m_maze + tempNode->x) + nextY) ||

144      1 == *(*(m_maze + nextX) + tempNode->y))

145     {

146      continue;

147     }

148     //计算此子节点的g值

149     int newGVal;

150     if (!dx[i] && !dy[i])//位于对角线上

151     {

152      newGVal = tempNode->g + 14;

153     }

154     else

155      newGVal = tempNode->g + 10;

156 

157     //搜索OPEN表，判断此点是否在OPEN表中

158     vector<Node*>::iterator OPENTableResult;

159     for (OPENTableResult = OPENTable.begin();

160      OPENTableResult != OPENTable.end();++OPENTableResult)

161     {

162      if ((*OPENTableResult)->x == nextX &&

163       (*OPENTableResult)->y == nextY)

164      {

165       break;

166      }

167     }

168 

169     //此子节点已经存在于OPEN表中

170     if (OPENTableResult != OPENTable.end())

171     {

172      //OPEN表中节点的g值已经是最优的，则跳过此节点

173      if ((*OPENTableResult)->g <= newGVal)

174      {

175       continue;

176      }

177     }

178 

179     //搜索CLOSED表，判断此节点是否已经存在于其中

180     vector<Node*>::iterator CLOSEDTableResult;

181     for (CLOSEDTableResult = CLOSEDTable.begin();

182      CLOSEDTableResult != CLOSEDTable.end();++CLOSEDTableResult)

183     {

184      if ((*CLOSEDTableResult)->x == nextX &&

185       (*CLOSEDTableResult)->y == nextY)

186      {

187       break;

188      }

189     }

190 

191     //此节点已经存在于CLOSED表中

192     if (CLOSEDTableResult != CLOSEDTable.end())

193     {

194      //CLOSED表中的节点已经是最优的，则跳过

195      if ((*CLOSEDTableResult)->g <= newGVal)

196      {

197       continue;

198      }

199     }

200 

201     //此节点是迄今为止的最优节点

202     Node *bestNode = new Node;

203     bestNode->x = nextX;

204     bestNode->y = nextY;

205     bestNode->father = tempNode;

206     bestNode->g = newGVal;

207     bestNode->h = judge(nextX,nextY);

208     bestNode->f = bestNode->g + bestNode->h;

209 

210     //如果已经存在于CLOSED表中，将其移除

211     if (CLOSEDTableResult != CLOSEDTable.end())

212     {

213      delete (*CLOSEDTableResult);

214      CLOSEDTable.erase(CLOSEDTableResult);

215     }

216 

217     //如果已经存在于OPEN表，更新

218     if (OPENTableResult != OPENTable.end())

219     {

220      delete (*OPENTableResult);

221      OPENTable.erase(OPENTableResult);

222 

223      //重新建堆，实现排序。注意不能用sort_heap，因为如果容器为空的话会出现bug

224      make_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());

225     }

226 

227     OPENTable.push_back(bestNode);//将最优节点放入OPEN表

228 

229     push_heap(OPENTable.begin(),OPENTable.end(),HeapCompare_f());//重新排序

230    }

231   }

232 

233   CLOSEDTable.push_back(tempNode);

234  }

235 

236  return false;

237 }

238 

239 void AStarPathFinding::generatePath()

240 {

241 

242  Node *nodeChild = endNode;

243  Node *nodeParent = endNode->father;

244  do

245  {

246   *(*(m_path + nodeChild->x) + nodeChild->y) = NODE_PATH;//标记为路径上的点

247   nodeChild = nodeParent;

248   nodeParent = nodeParent->father;

249  } while (nodeChild != startNode);

250 

251  *(*(m_path + startNode->x) + startNode->y) = NODE_PATH;//标记为路径上的点

252 }

253 

254 void AStarPathFinding::printPath()

255 {

256  cout << "The path is " << endl;

257  for (int i = 0;i < m_rows;++i)

258  {

259   for (int j = 0;j < m_cols;++j)

260   {

261    if (NODE_PATH == *(*(m_path + i) + j))

262    {

263     cout << "# ";

264    }

265    else

266     cout << *(*(m_maze + i) + j) << " ";

267   }

268   cout << endl;

269  }

270 

271  cout << "搜索总步数："  << m_steps << endl;

272 }

273 

274 bool AStarPathFinding::readMap()

275 {

276  //从文本文件读取迷宫布局信息

277  ifstream mapFileStream(m_mapFileName);

278  if (!mapFileStream)

279  {

280   cerr << "ERROR : unable to open map file" << endl;

281   return false;

282  }

283 

284  mapFileStream >> m_rows >> m_cols;

285 

286  //多维数组空间分配

287  m_maze = new int *[m_rows];

288  for (int i = 0;i < m_rows;++i)

289  {

290   m_maze[i] = new int[m_cols];

291  }

292 

293  mapFileStream >> m_startX >> m_startY;

294  mapFileStream >> m_endX >> m_endY;

295 

296  for (int i = 0;i < m_rows;++i)

297  {

298   for (int j = 0;j < m_cols;++j)

299   {

300    mapFileStream >> *(*(m_maze + i) + j);

301   }

302  }

303 

304  return true;

305 }

306 

307 int AStarPathFinding::judge(int x, int y)

308 {

309  return (10 * (abs(m_endX - x) + abs(m_endY - y)));

310 }

311 

312 bool AStarPathFinding::isIllegle(int x, int y)

313 {

314  if (x >= 0 && x < m_rows &&

315   y >= 0 && y < m_cols &&

316   *(*(m_maze + x) + y) == 0)

317   return true;

318  else

319   return false;

320 }

321 

322 int main()

323 {

324   AStarPathFinding astar("map.txt");

325   astar.init();

326   astar.pathFinding();

327   astar.generatePath();

328   astar.printPath();

329   return 0;

330 }

地图文件：

10 10

0 0 9 9

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0 0

 

以上三个文件放在同一个目录下

打开终端：

g++ AStarPathFinding.cpp

./a.out