A*搜索四方向寻路 C++代码实现

眼馋A*算法很久了，可是很久却还不能入门。昨天终于狠下心来，花一天使劲折腾它，终于写出了A*搜索，（我写的是自动寻路系统，很多网游里面用的那种)。我也在网上搜了很久。但一直没有找到c++的实现，于是就自己造了这个轮子。下面是几张运行的截图:









关于A*算法的理论原理，网上已经有很多文章介绍了，而我只是初学，理论不是特别清晰，所以也不在此献丑了。仅在此贴上代码（已添加注释）（代码是在手机上完成的，所以缩进可能有些地方怪怪的。。。)，之后附上个人的心得。

/*
A*寻路.cpp 四方向版
--sxysxy 2016-2-15

常数挺大的。。。可能小图跑不过dij 或者 spfa......
代码写得灰常乱，因为本就不熟。。。然后还不想麻烦...
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <time.h>
#include <queue>
using namespace std;
#define INF (0x23333333)
#define MAXN (302)

//寻四方向用
const int dl[] = {1,-1,0,0};
const int dc[] = {0,0,-1,1};

int map[MAXN][MAXN]; //地图
int n,m;   //地图的行数列数
int g[MAXN][MAXN]; //已用代价，参数为行列坐标
int h[MAXN][MAXN]; //估计还需代价，参数为行列坐标
bool in_close[MAXN][MAXN]; //在关闭列表中，参数为行列坐标
bool in_open[MAXN][MAXN]; //在开启列表中，参数为行列坐标
bool ok = false;  //立一个找到解的flag
struct point
{
int l,c;
point(){}
point(int _l, int _c){l=_l;c=_c;}
bool operator<(const point &o) const
{
return g[l][c]+h[l][c]>g[o.l][o.c]+h[o.l][o.c];
//启发数值在优先队列里面从小到大...
}
};
point path[MAXN][MAXN];  //记录路径用
int sl,sc,tl,tc; //出发/目标点的位置
#define mkpt(a,b) (point{a,b})
void find_path()
{
int i,j;
int cl,cc,nl,nc;
//cl,cc当前处理的点的位置，
//nl,nc当前处理的点的邻居的位置
priority_queue<point> open_list;
//用优先队列维护开启列表
if(map[sl][sc] || map[tl][tc])return; //出发点或者目标点不可通行，返回
for(i = 1; i <= n; i++)
{
for(j = 1; j <= m; j++)
{
h[i][j] = (int)sqrt((double)(i-tl)*(i-tl)+(double)(j-tc)*(j-tc))+1;  //估计代价 = 这个点到目标点的几何距离向上取整
g[i][j] = INF; //已用代价初始化为INF
}
}

//出发点扔到open_list
g[sl][sc] = 0;
in_open[sl][sc] = true;
open_list.push(mkpt(sl,sc));

while(open_list.size())
{
cl = open_list.top().l;
cc = open_list.top().c;
open_list.pop();
if(cl == tl && cc == tc)
{  //找到目标，跳出循环
ok = true;
break;
}
//当前点算入关闭列表中
in_close[cl][cc] = true;
for(i = 0; i < 4; i++)
{
nl = cl+dl[i];
nc = cc+dc[i];
if(map[nl][nc] == 1)continue; //不可通行
if(in_close[nl][nc])continue; //在关闭列表
if(! in_open[nl][nc])
{  //发现新节点
open_list.push(mkpt(nl,nc));
}else if(g[cl][cc]+1 >= g[nl][nc])
continue; //不是更好的解，continue

//找到一个更优的解，记录路径，更新g值
path[nl][nc] = mkpt(cl,cc);
g[nl][nc] = g[cl][cc] + 1;
}
}
}

void print_res()
{
int l,c;
int i,j;
if(!ok)
{
printf("无法到达!\n");
return;
}
printf("从出发点到目标点所需步数为:%d\n", g[tl][tc]);
//回溯路径
l = tl;
c = tc;
while(true)
{
map[l][c] = '*'-'0';
if(l == sl && c == sc)break;
i = path[l][c].l;
j = path[l][c].c;
l = i;
c = j;
}
map[sl][sc] = '%'-'0';
//打印解法
printf("走法如下 %%是出发点，*是路径:\n");
for(i = 1; i <= n; i++)
{
for(j = 1; j <= m; j++)
{
printf("%c ", map[i][j]+'0');
}
putchar('\n');
}

}

int main()
{
int i, j;
printf("=======A*搜索 四方向寻路系统=======\n");
printf("先输入两个整数，n,m，表示下面将要输入一个n行m列的迷宫\n");
scanf("%d %d", &n, &m);
for(i = 0; i <= n+1; i++)
for(j = 0; j <= m+1; j++)
map[i][j] = 1;  //先初始化为全部为不可通行路块，便于处理
printf("接下来输入这n行m列的迷宫，用0表示可通行的路块，1表示不可通行的路块\n");
for(i = 1; i <= n; i++)
for(j = 1; j <= m; j++)
scanf("%d", &map[i][j]);
printf("接下来输入起点和终点的位置，位置用行列来表示，行列从1开始计算\n");
printf("起点位置:");
scanf("%d %d", &sl, &sc);
printf("终点位置:");
scanf("%d %d", &tl, &tc);
find_path();
print_res();
return 0;
}

心得体会方面，那就先说说心灵上的收获，那就是，不要怕一些东西难，真的狠心去学它了，总是能学到一些知识的，，，（怎么感觉跟小学生作文似的）

关于这个算法的方面：玄学！这跑起来来复杂度真是个玄学！启发式函数真是太奇妙，程序猿们要敢想，只要能够让优秀的解先被选到，即可。