A_Star算法C++实现

关于A星算法，在这里提两个关键点：

   (1).适应用于静态场景。A星算法需要提前构建场景地图（SLAM），在此基础之上做路径规划。

   (2).启发式搜索。采用位置评估手段计算路径代价。比如我们要用的曼哈顿距离。

#include <iostream>
#include <algorithm>
#include <queue>
#include <vector>
#include <cmath>
#define N 6  // 棋盘/迷宫 的阶数 
using namespace std;
class Node {
public:
	int x, y; // 节点坐标
	//f=g+h,总代价；
	//g，上个点到该点的代价；
	//h,该点到终点的估计值
	int f, g, h;    
	Node(int a, int b) {
		this->x = a;
		this->y = b;
	}
	//重载'<'运算符，因为队列不知道怎么对Node排序
	bool operator  < (const Node& a)const {
		//优先队列是大顶堆，因此这里反过来成为小顶堆。Set是小顶堆，如果用Set，这里的判定用'<'
		return this->f > a.f;
	}
};
bool visit[N][N];  //记录节点是否访问过
int path[N][N][2];  //记录父节点坐标，用于找路径
int realF[N][N];    //记录节点的实际f值
int addr[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} };
int att[8][2]= { {-1,-1}, {-1, 0}, {-1, 1}, {0, -1},//八个方向
		 {0, 1},  {1, -1}, {1, 0},  {1, 1} };
priority_queue<Node>que;  
void PrintPath(int a, int b);  //打印函数
void AStar(int x0, int y0, int x1, int y1);  //A星算法部分
bool NodeIsLegal(int x, int y, int x0, int y0);  //合理性检验
int Mahuattan(int x, int y, int x1, int y1);    //启发性估值，曼哈顿函数
int main() {
	//初始化填充
	fill(visit[0],visit[0]+N*N,false); 
	fill(path[0][0], path[0][0] + N * N * 2, -1);
	fill(realF[0], realF[0] + N * N, 0);
	int x0, y0, x1, y1;
	cout << "请输入起点：" << endl;
	cin >> x0 >> y0;
	cout << "请输入重点：" << endl;
	cin >> x1 >> y1;
	if (!NodeIsLegal(x0, y0, x0, y0)) {
		cout << "输入有误！" << endl;
		return 0;
	}
	AStar(x0, y0, x1, y1);
	PrintPath(x1, y1);
	return 0;
}
void AStar(int x0, int y0, int x1, int y1) {
	//初始化起点
	Node node(x0, y0);
	node.g = 0;
	node.h = Mahuattan(x0, y0, x1, y1);
	node.f = node.g + node.h;
	realF[x0][y0] = node.f;
	que.push(node);  //加入队列
	while (!que.empty()) {
		//大循环，队列的Top节点即为代价最小点。
		Node node_top = que.top();
		visit[node_top.x][node_top.y] = true; //设置访问
		if (node_top.x == x1 && node_top.y == y1) { //终止条件之一，已经到达
			break;
		}
		que.pop();//从待处理队列中删掉
		//循环处理八个方向相邻的节点
		for (int i = 0; i < 8; i++) {
			Node node_next(node_top.x + att[i][0], node_top.y + att[i][1]);
			// 该节点坐标合法 且 未加入close表 
			if (NodeIsLegal(node_next.x, node_next.y, node_top.x, node_top.y)) {
				// 计算从起点并经过node_top节点到达该节点所花费的代价 
				node_next.g = node_top.g + int(10 * (sqrt(pow(att[i][0], 2) + pow(att[i][i], 2))));
				// 计算该节点到终点的曼哈顿距离
				node_next.h = Mahuattan(node_next.x, node_next.y, x1, y1);
				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 < realF[node_next.x][node_next.y] || !visit[node_next.x][node_next.y]) {
					realF[node_next.x][node_next.y] = node_next.f;
					//设置父节点坐标信息
					path[node_next.x][node_next.y][0] = node_top.x;
					path[node_next.x][node_next.y][1] = node_top.y;
					que.push(node_next);//加入队列
				}
			}
		}
	}
}
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) {
		addr[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[addr[i][j]];
		cout << endl;
	}
}
bool NodeIsLegal(int x0, int y0, int x1, int y1) {
	//边界判断
	if (x0<0 || y0<0 || x0>N - 1 || y0>N - 1) return false;
	//障碍物判断
	if (addr[x0][y0] == 1) return false;
	// 两节点成对角型且它们的公共相邻节点存在障碍物 ，就是移动不能穿模
	if (x0 != x1 && y0 != y1 && (addr[x0][y1]==1||addr[x1][y0]==1))return false;
	return true;
}
int Mahuattan(int x, int y, int x1, int y1) {
	return (abs(x - x1) + abs(y - y1))*10;
}