A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。——来自百度百科。

我在网上看了不少关于A*寻路的文章，基本都能看懂。但是大多数文章中没有代码实现，或者是一些我不会的某些语言，还有的代码注释太少了而且太长，我看着看着就看不下去了。所以我就自己写了A*算法寻路的C++代码。

A*寻路

A*寻路算法详解推荐阅读博文链接：http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx

A*寻路的讲解文章网上很多，有的文章讲的很好，有图可以让读者一步步理解。其中我本人觉得这篇文章讲的最好，这篇文章应该是翻译的英文原文。我觉得我再写关于A*寻路的讲解肯定也没人家写的条理清晰，索性干脆推荐大家读别人的文章。但是，它只有讲解，没有代码，所以想看代码的同学就可以看本文的C++代码。

根据那篇文章，我对下面的代码做一些简单介绍：

每个节点我们定义成一个类，其中包含坐标x、y，评估函数F、G、H，它们的含义同原文一致。
定义一个三维数组path，用于存储每个位置的方格对应的“父方格”的坐标。
二维数组valF保序每个方格目前情况下最小的F值。
由于每次需要从open表中弹出的是F值最小的节点，我们选择使用优先队列来作为open表。
定义visit二维数组作为close表，初始值false，对应位置为true时表示已经加入close表。

具体代码中的每个操作的含义请看代码中的注释。

C++代码

本人水平较浅，代码质量不高，但是我觉得帮助理解A*算法应该没问题。

#include<iostream>

#include<algorithm>

#include<string>

#include<vector>

#include<cmath>

#include<queue>

#define N 6 // 棋盘/迷宫 的阶数

using namespace std;

class Node

{

	public:

		int x, y; // 节点所在位置

		int F, G, H; // G:从起点开始，沿着产的路径，移动到网格上指定方格的移动耗费。

					 // H:从网格上那个方格移动到终点B的预估移动耗费，使用曼哈顿距离。

					 // F = G + H

		Node(int a, int b):x(a), y(b){}

		// 重载操作符，使优先队列以F值大小为标准维持堆

		bool operator < (const Node &a) const

		{

			return F > a.F;

		}

}; 

// 定义八个方向

int dir[8][2] = {{-1,-1}, {-1, 0}, {-1, 1}, {0, -1},

		 {0, 1},  {1, -1}, {1, 0},  {1, 1}};

// 优先队列，就相当于open表

priority_queue<Node>que;

// 棋盘

int qp[N][N] = { {0,0,0,0,0,0},

		 {0,1,1,0,1,1},

		 {0,0,1,0,0,0},

	         {0,0,1,1,1,0},

		 {0,1,1,0,0,0},

		 {1,1,0,0,0,0} };

bool visit[N][N]; // 访问情况记录，close表

int valF[N][N];   // 记录每个节点对应的F值

int path[N][N][2]; // 存储每个节点的父节点

int Manhuattan(int x, int y, int x1, int y1); // 计算曼哈顿距离

bool NodeIsLegal(int x, int y, int xx, int yy); // 判断位置合法性

void A_start(int x0, int y0, int x1, int y1); // A*算法

void PrintPath(int x1, int y1); // 打印路径

/* ----------------主函数------------------- */

int main()

{

	fill(visit[0], visit[0]+N*N, false); // 将visit数组赋初值false

	fill(valF[0], valF[0]+N*N, 0); // 初始化F全为0

	fill(path[0][0], path[0][0]+N*N*2, -1); // 路径同样赋初值-1 

	//  // 起点 // 终点

	int x0, y0, x1, y1;

	cout<<"输入起点：";

	cin>>x0>>y0;

	cout<<"输入终点：";

	cin>>x1>>y1;

	x0--; y0--; x1--; y1--;

	if(!NodeIsLegal(x0, y0, x0, y0))

	{

		cout<<"非法起点！"<<endl;

		return 0;

	}

	A_start(x0, y0, x1, y1);  // A*算法

	PrintPath(x1, y1);        // 打印路径

}

/* ----------------自定义函数------------------ */

void A_start(int x0, int y0, int x1, int y1)

{

	// 初始化起点

	Node node(x0, y0);

	node.G = 0;

	node.H = Manhuattan(x0, y0, x1, y1);

	node.F = node.G + node.H;

	valF[x0][y0] = node.F;

	// 起点加入open表

	que.push(node); 

	while(!que.empty())

	{

		Node node_top = que.top(); que.pop();

		visit[node_top.x][node_top.y] = true; // 访问该点，加入closed表

		if(node_top.x == x1 && node_top.y == y1) // 到达终点

			break;

		// 遍历node_top周围的8个位置

		for(int i=0; i<8; i++)

		{

			Node node_next(node_top.x + dir[i][0], node_top.y + dir[i][1]); // 创建一个node_top周围的节点

			// 该节点坐标合法 且 未加入close表

			if(NodeIsLegal(node_next.x, node_next.y, node_top.x, node_top.y) && !visit[node_next.x][node_next.y])

			{

				// 计算从起点并经过node_top节点到达该节点所花费的代价

				node_next.G = node_top.G + int(sqrt(pow(dir[i][0],2)+pow(dir[i][1],2))*10);

				// 计算该节点到终点的曼哈顿距离

				node_next.H = Manhuattan(node_next.x, node_next.y, x1, y1);

				// 从起点经过node_top和该节点到达终点的估计代价

				node_next.F = node_next.G + node_next.H; 

				// node_next.F < valF[node_next.x][node_next.y] 说明找到了更优的路径，则进行更新

				// valF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入

				if(node_next.F < valF[node_next.x][node_next.y] || valF[node_next.x][node_next.y] == 0)

				{

					// 保存该节点的父节点

					path[node_next.x][node_next.y][0] = node_top.x;

					path[node_next.x][node_next.y][1] = node_top.y;

					valF[node_next.x][node_next.y] = node_next.F; // 修改该节点对应的valF值

					que.push(node_next); // 加入open表

				}

			}

		}

	}

}

void PrintPath(int x1, int y1)

{

	if(path[x1][y1][0] == -1 || path[x1][y1][1] == -1)

	{

		cout<<"没有可行路径！"<<endl;

		return;

	}

	int x = x1, y = y1;

	int a, b;

	while(x != -1 || y != -1)

	{

		qp[x][y] = 2; // 将可行路径上的节点赋值为2

		a = path[x][y][0];

		b = path[x][y][1];

		x = a;

		y = b;

	}

	// □表示未经过的节点， █表示障碍物， ☆表示可行节点

	string s[3] = {"□", "█", "☆"};

	for(int i=0; i<N; i++)

	{

		for(int j=0; j<N; j++)

			cout<<s[qp[i][j]];

		cout<<endl;

	}

}

int Manhuattan(int x, int y, int x1, int y1)

{

	return (abs(x - x1) + abs(y - y1)) * 10;

}

bool NodeIsLegal(int x, int y, int xx, int yy)

{

	if(x < 0 || x >= N || y < 0 || y >= N) return false; // 判断边界

	if(qp[x][y] == 1) return false; // 判断障碍物

	// 两节点成对角型且它们的公共相邻节点存在障碍物

	if(x != xx && y != yy && (qp[x][yy] == 1 || qp[xx][y] == 1)) return false;

	return true;

}

巴特西

A*算法寻路（C++代码实现）

A*寻路

C++代码

最新文章

热门文章