A*寻路算法的探寻与改良（二）
by:田宇轩
第二部分：这部分内容主要是使用C语言编程实现A*，想了解A*算法的优化内容的朋友们可以跳过这部分并阅读稍后更新的其他内容
2.1 回顾
在我的上一篇文章中，我们通过抽象的思维方式得出了A*算法的概念和原理，这一章内容中主要探讨如何用编程实现A*算法。
在数据结构与算法的学习中，每个算法都应该结合一定的数据结构在计算机中存储，然后用对应的函数操控这些数据结构，A*算法也不例外，从上一篇文章中，我们知道，A*算法需要：
（1）地图，这是一个存储静态路网的结构，由格子组成
（2）格子，格子是组成地图的基本单位，每个格子都有坐标，F，G，H，父节点这五种属性；
（3）开启列表，用于记录等待处理的格子；
（4）关闭列表，用于记录已经处理的格子；
（5）起点和终点，用于接受用户输入指定哪个点为起点，哪个点为终点；
这些存储结构都是A*算法需要的，其实为了实现A*算法，我们还需要更多的存储结构，这些结构我们将会在用到的时候抽象出来的。弄清思路之后，我们先用C语言定义一下这些结构，如果您是其他语言的使用者，也可以按照这些结构的描述用其他语言的定义和实现。下面就是C语言对A*所需结构的实现，下面这段代码可以单独定义在一个头文件中，拥有全局作用域，其实让这些代码拥有全局作用域的方式我们并不提倡，这里只是方便教学和理解用。
1 #define MAX_number 5 //这是地图的最大值，可以自己修改以符合实际需求
2
3 //一个比较基础的结构体，用于和地图联动
4 struct baseNode
5 {
6 int i;
7 int j;
8 int weigt;
9
10 int f;
11 int g;
12 int h;
13
14 struct baseNode *father;
15 };
16
17 //定义了一个全局变量用来存储二维矩阵地图
18 struct baseNode map[MAX_number][MAX_number];
19
20 //记录起点和终点的数组，起点为Ascenery[0],终点为Ascenery[1]
21 struct baseNode AsceneryList[2];
22
23 //开启列表，用于A*算法
24 struct baseNode OpenList[MAX_number*MAX_number];
25
26 //关闭列表，用于A*算法
27 struct baseNode CloseList[MAX_number*MAX_number];
28
29 //用于记录开启列表中元素的个数
30 int openListCount = 0;
31
32 //用于记录关闭列表中元素的个数
33 int closeListCount = 0;
代码2.1.1—A*的基本存储结构
2.2 A*算法的流程
2.2.1 设计代码体系结构
为了方便用户输入和输出，也方便我们直观地看到A*的结果，在代码结构上，我们准备设计三个头文件：data.h，用于存储A*依赖的结构，func.h，用于写一些功能，process_control.h，用于显示一个简单的用户界面，控制用户流程，识别用户的输入是否在合法范围内，最后，我们用main.c调用所有这些内容。主函数很简单，这里先写出来：
1 #include "process_control.h"
2
3 int main()
4 {
5 testStart();
6
7 return 0;
8 }
代码2.2.1.1—主函数
2.2.2 流程控制函数
我们把前面定义的存储结构写在了data.h里面。然后我们稍微设计一下控制流程的process.h。这是main.c唯一引用的头文件，里面包含一个testStart函数。
1 #pragma once
2
3 #include "funcs.h"
4
5 //用于控制整个校园导航系统流程
6 void testStart()
7 {
8 //flag用于辅助程序判断有没有足够条件执行各功能
9 int flag1 = 0, flag2 = 0;
10
11 //不断的让用户选择菜单
12 for (;;)
13 {
14 printf("基于A*算法的校园导航系统程序\n\n");
15
16 printf("你可以进行以下操作：\n");
17 printf("1.设定校园地图地形\n");
18 printf("2.设定寻径的起点和终点\n");
19 printf("3.找出最佳路径\n");
20 printf("0.退出系统\n\n");
21
22 //让用户输入自己的选择
23 int userInput = 0;
24 scanf("%d", &userInput);
25
26 //根据自己的选择分别执行inputmap,setroad,Astar三个函数
27 switch (userInput)
28 {
29 case 1:
30 inputmap();
31 flag1 = 1;
32 printf("设定校园地图地形成功\n\n");
33 break;
34
35 case 2:
36 if (flag1 == 1)
37 {
38 setRoad();
39 flag2 = 1;
40 printf("起点终点设定完毕\n\n");
41 }
42 else
43 {
44 printf("请先完成地图设定\n");
45 }
46 break;
47
48 case 3:
49 if (flag1 == 1&&flag2==1)
50 {
51 Astar();
52 printf("寻径完毕\n\n");
53 }
54 else
55 {
56 printf("请先完成地图、起点终点设定\n");
57 }
58 break;
59
60 case 0:
61 exit(0);
62 break;
63
64 default:
65 printf("输入不在指定范围内，请重新输入\n\n");
66 break;
67 }
68 }
69 }
代码2.2.2.1—流程控制函数
2.2.3 设定地图样式的函数inputmap和设定起点终点的函数setroad
让我们先设定好地图再进行A*算法本体的编写，这部分没有什么难度，因此也不先写伪代码和分析逻辑了，对此不感兴趣的朋友可以直接往后看Astar函数的实现。
1 #pragma once
2
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <math.h>
7 #include "data_define.h"
8
9 //设定地图的函数
10 void inputmap()
11 {
12 printf("目前地图大小为%d * %d。\n",MAX_number,MAX_number);
13 printf("请通过输入整数的形式填充地图，0代表地形不可通过，\n其他正整数代表权值，权值越大，地形越不方便通过\n\n");
14
15 for (int i = 0; i <MAX_number; ++i)
16 {
17 for (int j = 0; j < MAX_number; ++j)
18 {
19 scanf("%d", &map[i][j].weigt);
20
21 map[i][j].i = i;
22 map[i][j].j = j;
23 }
24 printf("第%d行输入完毕,共%d行\n\n",i+1,MAX_number);
25 }
26 }
27
28 //设定起点和终点的函数
29 void setRoad()
30 {
31 int i = 0, j = 0, p = 0, q = 0;
32
33 printf("地图坐标从【1，1】开始\n");
34 printf("请输入起点的横坐标:\n");
35 scanf("%d", &i);
36 printf("请输入起点的纵坐标:\n");
37 scanf("%d", &j);
38
39 AsceneryList[0].i = i - 1;
40 AsceneryList[0].j = j - 1;
41
42 printf("请输入终点的横坐标:\n");
43 scanf("%d", &p);
44 printf("请输入终点的纵坐标:\n");
45 scanf("%d", &q);
46
47 AsceneryList[1].i = p - 1;
48 AsceneryList[1].j = q - 1;
49
50 }
代码2.2.3.1—设定地图样式和起点终点的函数
2.2.4 Astar函数的设计
由于Astar算法需要对每个点的邻居都分析其F值，而且这里我们用二维矩阵来设定地图，因此一个格子最多有8个邻居格子，为了一一处理这些邻居格子，我们设计一种大小为8的邻居数组，用循环从邻居数组的第一个元素处理到邻居数组的最大第八个元素。邻居数组定义如下：
1 //邻居列表，用于记录每个当前点的所有邻居 2 struct baseNode Neibo[8]; 3 4 //用于记录邻居的个数 5 int neibNum = 0;
代码2.2.4.1—邻居列表的定义
接下来我们按照上前文章总结的流程编写一个代码框架，先不实现各个函数的具体功能。先回顾一下上一篇文章的A*寻路流程：
2.2.4.1—A*寻路流程
复习了流程之后，我们就可以按照流程上面的大致打一个框架：
1 //假设我们预先把起点放入迭代器iter，终点设为ender
2 //把起点放入开启列表
3 putInOpenList(iter);
4
5 //当开启列表为空或者终点在关闭列表中，结束寻径
6 for (; openListCount != 0 && isInCloseList(ender)==0;)
7 {
8 //取出开启列表中f值最小的节点（之一），并设为iter（当前点）
9 iter = readTopOpenList();
10
11 //把当前点从开启列表中删除
12 outOpenList(iter);
13
14 //把当前点记录在关闭列表中
15 putInCloseList(iter);
16
17 //把当前点的邻居加入邻居列表
18 addNeibo(iter);
19
20 //对于每个邻居，分三种情况进行操作
21 for (int i = 0; i < neibNum; ++i)
22 {
23 //如果这个邻居节点不可通过，或者这个邻居节点在关闭列表中，略过它
24 if (Neibo[i].weigt==0 || isInCloseList(Neibo[i]))
25 {
26 }
27 //如果这个邻居节点已经在开启列表中
28 else if(isInOpenList(Neibo[i]))
29 { //看看以当前格子为父节点，算出来的新G值是不是比原来的G值小，如果更小，就改变这一格的父节点，G值，重新计算F值
30 if (NewG(Neibo[i],iter)<Neibo[i].g)
31 {
32 map[Neibo[i].i][Neibo[i].j].father = &map[iter.i][iter.j];
33 map[Neibo[i].i][Neibo[i].j].g = map[iter.i][iter.j].g + increment(Neibo[i]);
34 map[Neibo[i].i][Neibo[i].j].f = map[Neibo[i].i][Neibo[i].j].g + map[Neibo[i].i][Neibo[i].j].h;
35 //把这一格的旧记录从开启列表删除，把更新后的这一格的值加入开启列表等待处理
36 outOpenList(Neibo[i]);
37 putInOpenList(Neibo[i]);
38 }
39 }
40 //如果这个邻居节点不在开启列表中
41 else
42 {
43 map[Neibo[i].i][Neibo[i].j].father= Neibo[i].father = &map[iter.i][iter.j];
44 map[Neibo[i].i][Neibo[i].j].g = map[iter.i][iter.j].g + increment(Neibo[i]);
45 map[Neibo[i].i][Neibo[i].j].f = map[Neibo[i].i][Neibo[i].j].g + map[Neibo[i].i][Neibo[i].j].h;
46
47 Neibo[i] = map[Neibo[i].i][Neibo[i].j];
48 putInOpenList(Neibo[i]);
49 }
50 }
51 }
代码2.2.4.2—A*寻路流程的代码
然后，只要分别实现上面代码逻辑中的函数就好了：
1 //以下函数都是A*算法的一部分///////////////////////////
2
3 //把一个元素插入开启列表中/////////
4 void putInOpenList(struct baseNode inList)
5 {
6 OpenList[openListCount] = inList;
7 ++openListCount;
8 }
9
10 //取出开启列表中最小的数
11 struct baseNode readTopOpenList()
12 {
13 struct baseNode temp;
14
15 for (int i = 0; i < openListCount-1; ++i)
16 {
17 if (OpenList[i].f<OpenList[i+1].f)
18 {
19 temp=OpenList[i];
20 OpenList[i]=OpenList[i + 1];
21 OpenList[i + 1] = temp;
22 }
23 }
24
25 return OpenList[openListCount-1];
26 }
27
28 //把一个元素加入关闭列表中
29 void putInCloseList(struct baseNode temp)
30 {
31 CloseList[closeListCount] = temp;
32
33 ++closeListCount;
34 }
35
36 //把开启列表中的当前节点删除
37 void outOpenList(struct baseNode iter)
38 {
39 int i = openListCount - 1;
40 for (; i >= 0;--i)
41 {
42 if (OpenList[i].i==iter.i&&OpenList[i].j==iter.j)
43 {
44 break;
45 }
46 }
47
48 for (int j = i; j < openListCount-1; ++j)
49 {
50 OpenList[j] = OpenList[j+1];
51 }
52 --openListCount;
53 }
54
55 //对于一路上的每个点，分析它的最多八个邻居，并加入邻居列表
56 void addNeibo(struct baseNode iter)
57 {
58 neibNum = 0;
59
60 for (int i = iter.i - 1; i <= iter.i + 1; ++i)
61 {
62 for (int j = iter.j - 1; j <= iter.j + 1; ++j)
63 {
64 if (i >= 0 && i <= MAX_number - 1 && j >= 0 && j <= MAX_number - 1)
65 {
66 if (i == iter.i&&j == iter.j)
67 {
68 }
69 else
70 {
71 map[i][j].h = manhatten(i, j);
72
73 Neibo[neibNum] = map[i][j];
74 ++neibNum;
75 }
76 }
77 }
78 }
79 }
80
81 //查看临近格在不在开启列表中的函数
82 int isInOpenList(struct baseNode neibo)
83 {
84 for (int i = 0; i < openListCount - 1; ++i)
85 {
86 if (OpenList[i].i == neibo.i&&OpenList[i].j == neibo.j)
87 {
88 return 1;
89 }
90 }
91 return 0;
92 }
93
94 //查看指定的temp在不在关闭列表中的函数
95 int isInCloseList(struct baseNode temp)
96 {
97 for (int i = 0; i < closeListCount-1; ++i)
98 {
99 if (CloseList[i].i == temp.i&&CloseList[i].j == temp.j)
100 {
101 return 1;
102 }
103 }
104 return 0;
105 }
106
107 //A*中的启发式函数，用于求指定位置和终点之间的曼哈顿距离
108 int manhatten(int i, int j)
109 {
110 return (abs(AsceneryList[1].node.i - i) + abs(AsceneryList[1].node.j - j))*10;
111 }
112
113 //求当前点与父亲节点的距离
114 int increment(struct baseNode this)
115 {
116 if ((abs(this.father->i-this.i)==1) && (abs(this.father->j - this.j) == 1))
117 {
118 return 14*this.weigt;
119 }
120 else if ((this.father->i - this.i) == 0 && (this.father->j - this.j) == 0)
121 {
122 return 0;
123 }
124 else
125 {
126 return 10*this.weigt;
127 }
128 }
129
130 //求出用当前点作为父节点时这个点的G值
131 int NewG(struct baseNode this,struct baseNode father)
132 {
133 if (abs(father.i - this.i) == 1 && abs(father.j - this.j) == 1)
134 {
135 return father.g+14;
136 }
137 else if (abs(father.i - this.i) == 0 && abs(father.j - this.j) == 0)
138 {
139 return father.g;
140 }
141 else
142 {
143 return father.g+10;
144 }
145 }
代码2.2.4.3—刚才代码中具体函数的分别实现
经过函数实现之后，我们就得出来了一条最短的从起点A到终点B的路径，这条路径上（包含A和B），每一个格子都指向它的父节点，因此我们可以从终点开始，一直遍历父节点，并设置一个迭代器，把每个格子的父节点赋值给迭代器，再储存入一个存储路径的数组里，我们就得到了这条路径：
1 //用来记录路径经过的点的个数 2 int AstackCount = 0; 3 4 //用来储存整理后的路径 5 struct baseNode Astack[MAX_number*MAX_number];
代码2.2.4.4—存储最佳路线的数组
1 //把A*算法的节点按倒序整理到Astack里面
2 void arrange(struct baseNode iter)
3 {
4 AstackCount = 0;
5 for (; ; iter=map[iter.father->i][iter.father->j])
6 {
7 Astack[AstackCount] = iter;
8 ++AstackCount;
9 if (iter.i == AsceneryList[0].node.i&&iter.j == AsceneryList[0].node.j)
10 {
11 break;
12 }
13 }
14 }
15
16 //打印出A*算法的路径矩阵
17 printAstar()
18 {
19 printf("A为最佳路径，Q为不经过区域\n\n");
20 int boole = 0;
21
22 for (int i = 0; i < MAX_number; ++i)
23 {
24 for (int j = 0; j < MAX_number; ++j)
25 {
26 for (int w=0; w<AstackCount; ++w)
27 {
28 if (Astack[w].i==i&&Astack[w].j==j)
29 {
30 boole = 1;
31 break;
32 }
33 }
34
35 if (boole==1)
36 {
37 printf("A ");
38 boole = 0;
39 }
40 else
41 {
42 printf("Q ");
43 }
44 }
45 printf("\n");
46 }
47 }
代码2.2.4.5—迭代整理和输出
大功告成......等等，还差一步，我们在做A*操作时曾经假设iter初始化为起点，ender为终点，记得吗？因此，我们在做A*算法之前还要做这种类似初始化的操作：
//每次执行A*算法，都初始化开启/关闭列表
openListCount = 0;
closeListCount = 0;
//创建一个迭代器,每次都等于f值最小的节点
struct baseNode iter;
//让这个迭代器的初值为起点
iter.i = AsceneryList[0].i;
iter.j = AsceneryList[0].j;
iter.weigt = map[AsceneryList[0].i][AsceneryList[0].j].weigt;
//起点的没有父节点，且为唯一G值为0的点
iter.g = 0;
iter.h = manhatten(iter.i,iter.j);
iter.f = iter.g + iter.h;
//创建终点
struct baseNode ender;
ender.i = AsceneryList[1].i;
ender.j = AsceneryList[1].j;
//把起点放入开启列表
putInOpenList(iter);
代码2.2.4.6—初始化A*
2.3 A*算法总结
这里我们按顺序写一遍A*算法的完整代码，默认是折叠的。看了上文自己做代码的朋友如果实际操作遇到问题，就可以参考以下代码：（下面的代码因为课设加了一些无关紧要的功能）
1 #pragma once
2
3 #define MAX_number 5
4
5 //一个比较基础的结构体，用于和地图联动
6 struct baseNode
7 {
8 int i;
9 int j;
10 int weigt;
11
12 int f;
13 int g;
14 int h;
15
16 struct baseNode *father;
17 };
18
19 //定义了一个全局变量用来存储二维矩阵地图
20 struct baseNode map[MAX_number][MAX_number];
21
22 //用于记录景点的数组元素
23 struct scenerySpotsList
24 {
25 struct baseNode node;
26 char placeName[20];
27 };
28
29 //邻居列表，用于记录每个当前点的所有邻居
30 struct baseNode Neibo[8];
31
32 //记录景点，起点和终点的数组
33 struct scenerySpotsList AsceneryList[MAX_number*MAX_number];
34
35 //开启列表，用于A*算法
36 struct baseNode OpenList[MAX_number*MAX_number];
37
38 //关闭列表，用于A*算法
39 struct baseNode CloseList[MAX_number*MAX_number];
40
41 //用于记录现在的景点个数，第1个景点记录在AsceneryList【2】里，AsceneryList【0】和AsceneryList【1】分别用来记录起点和终点
42 int sceneryCount = 2;
43
44 //用于记录开启列表中元素的个数
45 int openListCount = 0;
46
47 //用于记录关闭列表中元素的个数
48 int closeListCount = 0;
49
50 //用于记录邻居的个数
51 int neibNum = 0;
52
53 //用来储存整理后的路径
54 struct baseNode Astack[MAX_number*MAX_number];
55
56 //用来记录路径经过的点的个数
57 int AstackCount = 0;
part1
part1为数据定义的头文件
1 #pragma once
2
3 #include <stdio.h>
4 #include <stdlib.h>
5 #include <string.h>
6 #include <math.h>
7 #include "data_define.h"
8
9 //设定地图的函数
10 void inputmap()
11 {
12 printf("目前地图大小为%d * %d。\n",MAX_number,MAX_number);
13 printf("请通过输入整数的形式填充地图，0代表地形不可通过，\n其他正整数代表权值，权值越大，地形越不方便通过\n\n");
14
15 for (int i = 0; i <MAX_number; ++i)
16 {
17 for (int j = 0; j < MAX_number; ++j)
18 {
19 scanf("%d", &map[i][j].weigt);
20
21 map[i][j].i = i;
22 map[i][j].j = j;
23 }
24 printf("第%d行输入完毕,共%d行\n\n",i+1,MAX_number);
25 }
26 }
27
28 //设定校园景点的函数
29 void setView()
30 {
31 for (; ; )
32 {
33 int inputI = 0;
34 int inputJ = 0;
35 char inputS[20];
36
37 printf("请输入景点的名称，输入done结束景点录入\n");
38 scanf("%s", inputS);
39
40 if (strcmp(inputS,"done")==0)
41 {
42 printf("结束输入:\n");
43 break;
44 }
45 else
46 {
47 strcpy(AsceneryList[sceneryCount].placeName,inputS);
48
49 printf("地图坐标从【1，1】开始\n");
50 printf("请输入景点的横坐标:\n");
51 scanf("%d", &inputI);
52 AsceneryList[sceneryCount].node.i = inputI-1;
53
54 printf("请输入景点的纵坐标:\n");
55 scanf("%d", &inputJ);
56 AsceneryList[sceneryCount].node.j = inputJ-1;
57
58 printf("成功输入一个景点:\n");
59 ++sceneryCount;
60 }
61 }
62 }
63
64 //设定起点和终点的函数
65 void setRoad()
66 {
67 printf("你可以进行以下操作\n");
68 printf("1.寻找一个景点\n");
69 printf("2.手动设定起点终点坐标\n");
70
71 int user_input = 0;
72 scanf("%d",&user_input);
73
74 if (user_input==1)
75 {
76 int i = 2;
77
78 char inputS[20];
79 printf("请输入想查找的景点的名称:\n这个景点将作为起点\n");
80 scanf("%s", inputS);
81
82 for (; i < sceneryCount; ++i)
83 {
84 if (strcmp(AsceneryList[i].placeName, inputS) == 0)
85 {
86 AsceneryList[0].node = AsceneryList[i].node;
87
88 i = 0;
89 printf("成功把%s设置为起点\n",inputS);
90 break;
91 }
92 }
93 if (i != 0)
94 {
95 printf("找不到这个景点\n");
96 }
97
98 int j = 2;
99
100 char inputM[20];
101 printf("请输入想查找的景点的名称:\n这个景点将作为终点\n");
102 scanf("%s", inputM);
103
104 for (; j < sceneryCount; ++j)
105 {
106 if (strcmp(AsceneryList[j].placeName, inputM) == 0)
107 {
108 AsceneryList[1].node = AsceneryList[j].node;
109
110 j = 0;
111 printf("成功把%s设置为终点\n",inputM);
112 break;
113 }
114 }
115 if (j!=0)
116 {
117 printf("找不到这个景点\n");
118 }
119 }
120 else if(user_input==2)
121 {
122 int i = 0, j = 0, p = 0, q = 0;
123
124 printf("地图坐标从【1，1】开始\n");
125 printf("请输入起点的横坐标:\n");
126 scanf("%d", &i);
127 printf("请输入起点的纵坐标:\n");
128 scanf("%d", &j);
129
130 AsceneryList[0].node.i = i - 1;
131 AsceneryList[0].node.j = j - 1;
132
133 printf("请输入终点的横坐标:\n");
134 scanf("%d", &p);
135 printf("请输入终点的纵坐标:\n");
136 scanf("%d", &q);
137
138 AsceneryList[1].node.i = p - 1;
139 AsceneryList[1].node.j = q - 1;
140 }
141 else
142 {
143 printf("输入错误\n");
144 }
145 }
146
147 //以下函数都是A*算法的一部分///////////////////////////
148
149 //把一个元素插入开启列表中/////////
150 void putInOpenList(struct baseNode inList)
151 {
152 OpenList[openListCount] = inList;
153 ++openListCount;
154 }
155
156 //取出开启列表中最小的数
157 struct baseNode readTopOpenList()
158 {
159 struct baseNode temp;
160
161 for (int i = 0; i < openListCount-1; ++i)
162 {
163 if (OpenList[i].f<OpenList[i+1].f)
164 {
165 temp=OpenList[i];
166 OpenList[i]=OpenList[i + 1];
167 OpenList[i + 1] = temp;
168 }
169 }
170
171 return OpenList[openListCount-1];
172 }
173
174 //把一个元素加入关闭列表中
175 void putInCloseList(struct baseNode temp)
176 {
177 CloseList[closeListCount] = temp;
178
179 ++closeListCount;
180 }
181
182 //把开启列表中的当前节点删除
183 void outOpenList(struct baseNode iter)
184 {
185 int i = openListCount - 1;
186 for (; i >= 0;--i)
187 {
188 if (OpenList[i].i==iter.i&&OpenList[i].j==iter.j)
189 {
190 break;
191 }
192 }
193
194 for (int j = i; j < openListCount-1; ++j)
195 {
196 OpenList[j] = OpenList[j+1];
197 }
198 --openListCount;
199 }
200
201 //对于一路上的每个点，分析它的最多八个邻居，并加入邻居列表
202 void addNeibo(struct baseNode iter)
203 {
204 neibNum = 0;
205
206 for (int i = iter.i - 1; i <= iter.i + 1; ++i)
207 {
208 for (int j = iter.j - 1; j <= iter.j + 1; ++j)
209 {
210 if (i >= 0 && i <= MAX_number - 1 && j >= 0 && j <= MAX_number - 1)
211 {
212 if (i == iter.i&&j == iter.j)
213 {
214 }
215 else
216 {
217 map[i][j].h = manhatten(i, j);
218
219 Neibo[neibNum] = map[i][j];
220 ++neibNum;
221 }
222 }
223 }
224 }
225 }
226
227 //查看临近格在不在开启列表中的函数
228 int isInOpenList(struct baseNode neibo)
229 {
230 for (int i = 0; i < openListCount - 1; ++i)
231 {
232 if (OpenList[i].i == neibo.i&&OpenList[i].j == neibo.j)
233 {
234 return 1;
235 }
236 }
237 return 0;
238 }
239
240 //查看指定的temp在不在关闭列表中的函数
241 int isInCloseList(struct baseNode temp)
242 {
243 for (int i = 0; i < closeListCount-1; ++i)
244 {
245 if (CloseList[i].i == temp.i&&CloseList[i].j == temp.j)
246 {
247 return 1;
248 }
249 }
250 return 0;
251 }
252
253 //A*中的启发式函数，用于求指定位置和终点之间的曼哈顿距离
254 int manhatten(int i, int j)
255 {
256 return (abs(AsceneryList[1].node.i - i) + abs(AsceneryList[1].node.j - j))*10;
257 }
258
259 //求当前点与父亲节点的距离
260 int increment(struct baseNode this)
261 {
262 if ((abs(this.father->i-this.i)==1) && (abs(this.father->j - this.j) == 1))
263 {
264 return 14*this.weigt;
265 }
266 else if ((this.father->i - this.i) == 0 && (this.father->j - this.j) == 0)
267 {
268 return 0;
269 }
270 else
271 {
272 return 10*this.weigt;
273 }
274 }
275
276 //求出用当前点作为父节点时这个点的G值
277 int NewG(struct baseNode this,struct baseNode father)
278 {
279 if (abs(father.i - this.i) == 1 && abs(father.j - this.j) == 1)
280 {
281 return father.g+14;
282 }
283 else if (abs(father.i - this.i) == 0 && abs(father.j - this.j) == 0)
284 {
285 return father.g;
286 }
287 else
288 {
289 return father.g+10;
290 }
291 }
292
293 //把A*算法的节点按倒序整理到Astack里面
294 void arrange(struct baseNode iter)
295 {
296 AstackCount = 0;
297 for (; ; iter=map[iter.father->i][iter.father->j])
298 {
299 Astack[AstackCount] = iter;
300 ++AstackCount;
301 if (iter.i == AsceneryList[0].node.i&&iter.j == AsceneryList[0].node.j)
302 {
303 break;
304 }
305 }
306 }
307
308 //打印出A*算法的路径矩阵
309 printAstar()
310 {
311 printf("A为最佳路径，Q为不经过区域\n\n");
312 int boole = 0;
313
314 for (int i = 0; i < MAX_number; ++i)
315 {
316 for (int j = 0; j < MAX_number; ++j)
317 {
318 for (int w=0; w<AstackCount; ++w)
319 {
320 if (Astack[w].i==i&&Astack[w].j==j)
321 {
322 boole = 1;
323 break;
324 }
325 }
326
327 if (boole==1)
328 {
329 printf("A ");
330 boole = 0;
331 }
332 else
333 {
334 printf("Q ");
335 }
336 }
337 printf("\n");
338 }
339 }
340
341 //Astar的本体
342 void Astar()
343 {
344 //每次执行A*算法，都初始化开启/关闭列表
345 openListCount = 0;
346 closeListCount = 0;
347
348 //创建一个迭代器,每次都等于f值最小的节点
349 struct baseNode iter;
350
351 //让这个迭代器的初值为起点
352 iter.i = AsceneryList[0].node.i;
353 iter.j = AsceneryList[0].node.j;
354 iter.weigt = map[AsceneryList[0].node.i][AsceneryList[0].node.j].weigt;
355
356 //起点的没有父节点，且为唯一G值为0的点
357 iter.g = 0;
358 iter.h = manhatten(iter.i,iter.j);
359 iter.f = iter.g + iter.h;
360
361 //创建终点
362 struct baseNode ender;
363
364 ender.i = AsceneryList[1].node.i;
365 ender.j = AsceneryList[1].node.j;
366
367 //把起点放入开启列表
368 putInOpenList(iter);
369
370 //当开启列表为空或者终点在关闭列表中，结束寻径
371 for (; openListCount != 0 && isInCloseList(ender)==0;)
372 {
373 //取出开启列表中f值最小的节点（之一），并设为iter（当前点）
374 iter = readTopOpenList();
375
376 //把当前点从开启列表中删除
377 outOpenList(iter);
378
379 //把当前点记录在关闭列表中
380 putInCloseList(iter);
381
382 //把当前点的邻居加入邻居列表
383 addNeibo(iter);
384
385 //对于每个邻居，分三种情况进行操作
386 for (int i = 0; i < neibNum; ++i)
387 {
388 //如果这个邻居节点不可通过，或者这个邻居节点在关闭列表中，略过它
389 if (Neibo[i].weigt==0 || isInCloseList(Neibo[i]))
390 {
391 }
392 //如果这个邻居节点已经在开启列表中
393 else if(isInOpenList(Neibo[i]))
394 {
395 if (NewG(Neibo[i],iter)<Neibo[i].g)
396 {
397 map[Neibo[i].i][Neibo[i].j].father = &map[iter.i][iter.j];
398 map[Neibo[i].i][Neibo[i].j].g = map[iter.i][iter.j].g + increment(Neibo[i]);
399 map[Neibo[i].i][Neibo[i].j].f = map[Neibo[i].i][Neibo[i].j].g + map[Neibo[i].i][Neibo[i].j].h;
400
401 outOpenList(Neibo[i]);
402 putInOpenList(Neibo[i]);
403 }
404 }
405 //如果这个邻居节点不在开启列表中
406 else
407 {
408 map[Neibo[i].i][Neibo[i].j].father= Neibo[i].father = &map[iter.i][iter.j];
409 map[Neibo[i].i][Neibo[i].j].g = map[iter.i][iter.j].g + increment(Neibo[i]);
410 map[Neibo[i].i][Neibo[i].j].f = map[Neibo[i].i][Neibo[i].j].g + map[Neibo[i].i][Neibo[i].j].h;
411
412 Neibo[i] = map[Neibo[i].i][Neibo[i].j];
413 putInOpenList(Neibo[i]);
414 }
415 }
416 }
417
418 arrange(map[ender.i][ender.j]);
419 printAstar();
420 }
part2
part2为函数的头文件
1 #pragma once
2
3 #include "funcs.h"
4
5 //用于控制整个校园导航系统流程
6 void testStart()
7 {
8 int flag1 = 0, flag3 = 0;
9
10 for (;;)
11 {
12 printf("基于A*算法的校园导航系统程序\n\n");
13
14 printf("你可以进行以下操作：\n");
15 printf("1.设定校园地图地形\n");
16 printf("2.设定校园景点\n");
17 printf("3.设定寻径的起点和终点\n");
18 printf("4.找出最佳路径\n");
19 printf("0.退出系统\n\n");
20
21 int userInput = 0;
22 scanf("%d", &userInput);
23
24 switch (userInput)
25 {
26 case 1:
27 inputmap();
28 flag1 = 1;
29 printf("设定校园地图地形成功\n\n");
30 break;
31
32 case 2:
33 if (flag1==1)
34 {
35 setView();
36 printf("校园景点设定完毕\n\n");
37 }
38 else
39 {
40 printf("请先完成地图设定\n");
41 }
42 break;
43
44 case 3:
45 if (flag1 == 1)
46 {
47 setRoad();
48 flag3 = 1;
49 printf("起点终点设定完毕\n\n");
50 }
51 else
52 {
53 printf("请先完成地图设定\n");
54 }
55 break;
56
57 case 4:
58 if (flag1 == 1&&flag3==1)
59 {
60 Astar();
61 printf("寻径完毕\n\n");
62 }
63 else
64 {
65 printf("请先完成地图、起点终点设定\n");
66 }
67 break;
68
69 case 0:
70 exit(0);
71 break;
72
73 default:
74 printf("输入不在指定范围内，请重新输入\n\n");
75 break;
76 }
77 }
78 }
part3
part3为控制流程的头文件，被主函数调用
1 #include "process_control.h"
2
3 int main()
4 {
5 testStart();
6
7 return 0;
8 }
part4
part4为主函数
其实，了解数据结构的人会看出来，A*里的开启列表每次都要找出里面的最小值，本文中逐个搜索取最小值的方法并不是最好的方法，这涉及到查找，二叉排序树等等知识，在下一篇文章中，我们开始正式分析如何优化这个算法。
作为参考，这篇文章里的程序在VS2015中运行的结果差不多是这样的：
时间： 2024-10-06 20:24:45