游戏中的自动寻路-A*算法（第一版优化——走直角篇）

• G 表示从起点移动到网格上指定方格的移动距离 (暂时不考虑沿斜向移动，只考虑上下左右移动)。
• H 表示从指定的方格移动到终点的预计移动距离，只计算直线距离，走直角篇走的是直角路线。
• 令 F = G + H ，F 即表示从起点经过此点预计到终点的总移动距离接下来我们从起点开始，按照以下寻路步骤，直至找到目标。

1. 从起点开始, 把它作为待处理的方格存入一个预测可达的节点列表，简称 openList, 即把起点放入“预测可达节点列表”， 可达节点列表 openList 就是一个等待检查方格的列表。
2. 寻找 openList 中 F 值最小的点 min（一开始只有起点）周围可以到达的方格（可到达的意思是其不是障碍物，也不存 在关闭列表中的方格,即不是已走过的方格）。计算 min 周围可到达的方格的 F 值。将还没在 openList 中点放入其中, 并 设置它们的"父方格"为点 min,表示他们的上一步是经过 min 到达的。如果 min 下一步某个可到达的方格已经在 openList 列表那么并且经 min 点它的 F 值更优,则修改 F 值并把其"父方格"设为点 min。
3. 把 2 中的点 min 从"开启列表"中删除并存入"关闭列表"closeList 中, closeList 中存放的都是不需要再次检查的方格。如 果 2 中点 min 不是终点并且开启列表的数量大于零，那么继续从第 2 步开始。如果是终点执行第 4 步，如果 openList 列 表数量为零，那么就是找不到有效路径。
4. 如果第 3 步中 min 是终点，则结束查找，直接追溯父节点到起点的路径即为所选路径

以下图的坐标行走

Astar.h

 1 #pragma once
 2 
 3 #include 
 4 
 5 const int kCost1 = 10;        //直移一格消耗
 6 const int kCost2 = 14;        //斜移一格消耗
 7 
 8 typedef struct _Point
 9 {
10     int x, y;                                //点坐标，这里为了方便按照 C++的数组来计算，x 代表横排，y 代表竖列
11     int F, G, H;                        //F=G+H
12     struct _Point* parent;            //parent 的坐标
13 }Point;
14 
15 /*分配一个节点(格子)*/
16 Point* AllocPoint(int x, int y);
17 
18 /*初始化地图*/
19 void InitAstarMaze(int* _maze, int _lines, int _colums);
20 
21 /*通过 A* 算法寻找路径*/
22 std::list GetPath(Point* startPoint, Point* endPoint);
23 
24 /*清理资源，结束后必须调用*/
25 void ClearAstarMaze();

 Astar.cpp

  1 #include 
  2 #include "Astar.h"
  3 #include 
  4 #include 
  5 
  6 static int* maze;                        //迷宫对应的二维数组，使用一级指针表示
  7 static int lines;                            //二维数组对应的行数
  8 static int cols;                            //二维数组对应的列数
  9 static std::list openList;        //开放列表
 10 static std::list closeList;            //关闭列表
 11 
 12 /*搜索从起点到终点的最佳路径*/
 13 static Point* findPath(Point* startPoint, Point* endPoint);
 14 
 15 /*从开启列表中返回 F 值最小的节点*/
 16 static Point* getLeastFpoint();
 17 
 18 /*获取当前点周围可达的节点*/
 19 static std::vector getSurroundPoints(const Point* point);
 20 
 21 /*判断某点是否可以用于下一步判断 */
 22 static bool isCanreach(const Point* point, const Point* target);
 23 
 24 /*判断开放/关闭列表中是否包含某点*/
 25 static Point* isInList(const std::list& list, const Point* point);
 26 
 27 //计算 FGH 值
 28 static int calcG(Point* temp_start, Point* point);
 29 static int calcH(Point* point, Point* end);
 30 static int calcF(Point* point);
 31 
 32 /*分配一个节点(格子)*/
 33 Point* AllocPoint(int x, int y)
 34 {
 35     Point* temp = new Point;
 36     memset(temp, 0, sizeof(Point));        //C语法 初始值清零
 37     temp->x = x;
 38     temp->y = y;
 39     return temp;
 40 }
 41 
 42 /*初始化 A*搜索的地图*/
 43 void InitAstarMaze(int* _maze, int _lines, int _colums)
 44 {
 45     maze = _maze;
 46     lines = _lines;
 47     cols = _colums;
 48 }
 49 
 50 /*通过 A* 算法寻找路径*/
 51 std::list GetPath(Point* startPoint, Point* endPoint)
 52 {
 53     Point* result = findPath(startPoint, endPoint);
 54     std::list path;
 55 
 56     //返回路径，如果没找到路径，返回空链表
 57     while (result)
 58     {
 59         path.push_front(result);
 60         result = result->parent;
 61     }
 62     return path;
 63 }
 64 
 65 /*搜索从起点到终点的最佳路径*/
 66 static Point* findPath(Point* startPoint, Point* endPoint)
 67 {
 68     openList.push_back(AllocPoint(startPoint->x, startPoint->y));    //置入起点,拷贝开辟一个节点，内外隔离
 69     while (!openList.empty())
 70     {
 71         //第一步，从开放列表中取最小 F 的节点
 72         Point* curPoint = getLeastFpoint();            //找到 F 值最小的点
 73 
 74         //第二步，把当前节点放到关闭列表中
 75         openList.remove(curPoint);
 76         closeList.push_back(curPoint);
 77 
 78         //第三步，找到当前节点周围可达的节点，并计算 F 值
 79         std::vector surroundPoints = getSurroundPoints(curPoint);
 80         for (std::vector::const_iterator iter = surroundPoints.begin(); iter != surroundPoints.end(); iter++)
 81         {
 82             Point* target = *iter;
 83 
 84             /*对某一个格子，如果它不在开放列表中，加入到开启列表，设置当前格为其父节点，计算 F G H
 85             */
 86             Point* exist = isInList(openList, target);
 87             if (!exist)
 88             {
 89                 target->parent = curPoint;
 90                 target->G = calcG(curPoint, target);
 91                 target->H = calcH(target, endPoint);
 92                 target->F = calcF(target);
 93                 openList.push_back(target);
 94             }
 95             else
 96             {
 97                 int tempG = calcG(curPoint, target);
 98                 if (tempG < target->G)
 99                 {
100                     exist->parent = curPoint;
101                     exist->G = tempG;
102                     exist->F = calcF(target);
103                 }
104                 delete target;
105             }
106         }//end for
107 
108         surroundPoints.clear();
109         Point* resPoint = isInList(openList, endPoint);
110         if (resPoint)
111         {
112             return resPoint;
113         }
114     }
115     return NULL;
116 }
117 
118 /*从开启列表中返回 F 值最小的节点*/
119 static Point* getLeastFpoint()
120 {
121     if (!openList.empty())
122     {
123         Point* resPoint = openList.front();
124 
125         for (std::list::const_iterator itor = openList.begin(); itor != openList.end(); itor++)
126         {
127             if ((*itor)->F < resPoint->F)
128             {
129                 resPoint = *itor;
130             }
131         }
132         return resPoint;
133     }
134     return NULL;
135 }
136 
137 /*获取当前点周围可达的节点*/
138 static std::vector getSurroundPoints(const Point* point)
139 {
140     std::vector surroundPoints;
141     for (int x = point->x - 1; x <= point->x + 1; x++)
142     {
143         for (int y = point->y - 1; y <= point->y + 1; y++)
144         {
145             Point* temp = AllocPoint(x, y);
146             if (isCanreach(point, temp))
147             {
148                 surroundPoints.push_back(temp);
149             }
150             else
151             {
152                 delete temp;
153             }
154         }
155     }
156     return surroundPoints;
157 }
158 
159 /*判断某点是否可以用于下一步判断 */
160 static bool isCanreach(const Point* point, const Point* target)
161 {
162     if (target->x<0
163         || target->x>(lines - 1)
164         || target->y<0 
165         || target->y>(cols - 1)
166         || maze[target->x * cols + target->y] == 1            //为 1 的障碍物判断
167         || maze[target->x * cols + target->y] == 2            //为 2 的障碍物判断
168         || (target->x == point->x && target->y == point->y)
169         || isInList(closeList, target))
170     {
171         return false;
172     }
173     if (abs(point->x - target->x) + abs(point->y - target->y) == 1)
174     {
175         return true;
176     }
177     else
178     {
179         return false;
180     }
181 }
182 
183 static Point* isInList(const std::list& list, const Point* point)
184 {
185     //判断某个节点是否在列表中，这里不能比较指针，因为每次加入列表是新开辟的节点，只能比较坐标
186     for (std::list::const_iterator itor = list.begin(); itor != list.end(); itor++)
187     {
188         if ((*itor)->x == point->x && (*itor)->y == point->y)
189         {
190             return *itor;
191         }
192     }
193     return NULL;
194 }
195 
196 /*计算节点的 G 值*/
197 static int calcG(Point* temp_start, Point* point)
198 {
199     int    extraG = (abs(point->x - temp_start->x) + abs(point->y - temp_start->y)) == 1 ? kCost1 : kCost2;
200 
201     //如果是初始节点，则其父节点是空
202     int parentG = (point->parent == NULL ? NULL : point->parent->G);
203 
204     return parentG + extraG;
205 }
206 
207 static int calcH(Point* point, Point* end)
208 {
209     //A^2 + B^2 == C^2 得出H的值
210     return (int)sqrt((double)(end->x - point->x) *
211         (double)(end->x - point->x) + (double)(end->y - point->y) *
212         (double)(end->y - point->y)) * kCost1;
213 }
214 
215 /*计算节点的 F 值*/
216 static int calcF(Point* point)
217 {
218     return point->G + point->H;
219 }
220 
221 /*清理资源，结束后必须调用*/
222 void ClearAstarMaze()
223 {
224     maze = NULL;
225     lines = 0;
226     cols = 0;
227     std::list::iterator itor;
228 
229     //清除 openList 中的元素
230     for (itor = openList.begin(); itor != openList.end();)
231     {
232         delete* itor;
233         itor = openList.erase(itor);//获取到下一个节点
234     }
235 
236     //清理 closeList 中的元素
237     for (itor = closeList.begin(); itor != closeList.end();)
238     {
239         delete* itor;
240         itor = closeList.erase(itor);
241     }
242 }

 main.cpp

 1 #include "Astar.h"
 2 #include 
 3 #include 
 4 #include 
 5 
 6 using namespace std;
 7 
 8 //定义地图数组,二维数组在内存顺序存储的
 9 int map[13][13] = {
10 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
11 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
12 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
13 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
14 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
15 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
16 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
17 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
18 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
19 { 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0 },
20 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
21 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
22 { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }
23 };
24 
25 void AStarTest()
26 {
27     InitAstarMaze(&map[0][0], 13, 13);
28 
29     //设置起始和结束点
30     Point* start = AllocPoint(5, 2);
31     Point* end = AllocPoint(5, 10);
32 
33     //A*算法找寻路径
34     list path = GetPath(start, end);
35     cout << "寻路结果:" << endl;
36     list::const_iterator iter;
37 
38     for (iter = path.begin(); iter != path.end(); iter++)
39     {
40         Point* cur = *iter;
41         cout << '(' << cur->x << ',' << cur->y << ')' << endl;
42         Sleep(800);
43     }
44 
45     ClearAstarMaze();
46 }
47 
48 int main(void)
49 {
50     AStarTest();
51     system("pause");
52     return 0;
53 }

============================================================================================================