【转】A*寻路算法 C++实现

时间 2019-12-06

标签算法 c++ 实现栏目 C&C++ 繁體版

原文原文链接

头文件：AStarPathFindingnode

 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

源文件：ios

  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布局