【轉】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