A*寻路算法

参考博客

A星算法详解
英文详解

代码部分（之前的那个有点问题）

#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

const int maxn = 100;
struct node{
	int x, y;
	int f, g, h;
	char dir; 
	node(){
		f = 0;
		g = 0;
		h = 0;
	}
};
int dirx[8] = {-1, -1, -1,  0, 0,  1, 1, 1}; // 8个方向 
int diry[8] = {-1,  0,  1, -1, 1, -1, 0, 1};
int n, m;
string ans;
class aStart{
public:
	list openSet;
	list closeSet;
	char maze[maxn][maxn];
	node map[maxn][maxn], road[maxn][maxn], start, fin;
	
	
	void initMap();
	void drawMap();
	void debug();
	int getH(node a, node b);
	int getG(int i);
	void getDir(int i, node nei);
	bool isInclude(list arr, node qur);
	void dfs(int x, int y);	
	void aStartSearch();		
};

void color(int x) // 给文字上色 
{
	if(x >= 0 && x <= 15)
		SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),x);
	else
		SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),7);	
}

void gotoxy(int x,int y) // 将光标移动到指定坐标 
{
	COORD pos = {x,y};
	HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
	SetConsoleCursorPosition(hOut,pos);
}

void aStart::initMap(){ // 初始化地图 
	memset(map, 0, sizeof(map));
	memset(road, 0, sizeof(road));
	memset(maze, 0, sizeof(maze));
	cin >> n;
	m = n;
	for(int i = 0; i <= n+1; i++){
		maze[i][0] = '#';
		maze[0][i] = '#';
		maze[n+1][i] = '#';
		maze[i][n+1] = '#';
	}
	for(int i = 1; i <= n; i++){
		for(int j = 1; j <= m; j++){
			cin >> maze[i][j];
			if(maze[i][j] == 's'){
				start.x = i;
				start.y = j;
			}
			if(maze[i][j] == 'e'){
				fin.x = i;
				fin.y = j;
			}
			map[i][j].x = i;
			map[i][j].y = j;
		}
	}
}

void aStart::drawMap(){ // 打印地图 
	gotoxy(0, 0);
	for(int i = 0; i <= n+1; i++){
		for(int j = 0; j <= m+1; j++){
			if(maze[i][j] == '@'){
				color(4);
				cout << '@';
			}else if(maze[i][j] == '#'){
				color(7);
				cout << maze[i][j];
			}
			else{
				cout << ' ';
			}
		}
		cout << endl;
	}
}

void aStart::debug(){ // 打印openSet和closeSet中的点 
	list::iterator it;
	for(it = openSet.begin(); it != openSet.end(); it++){
		gotoxy(it->y, it->x);
		color(4);
		cout << '*';
	}
	for(it = closeSet.begin(); it != closeSet.end(); it++){
		gotoxy(it->y, it->x);
		color(5);
		cout << '$';
	}
}

int aStart::getH(node a, node b){ // 计算当前点到终点的距离 h
	int h_dia = min(abs(a.x-b.x), abs(a.y-b.y));
	int h_str = abs(a.x-b.x) + abs(a.y-b.y);
	return (14 * h_dia + 10 * (h_str - 2*h_dia));
}

int aStart::getG(int i){ // 走下一步需要的花费 g 
	switch(i){
		case 0:
		case 2:
		case 5:
		case 7:return 14; // 走斜线 
		case 1:
		case 3:
		case 4:
		case 6:return 10; // 走直线 
	}
}

bool cmp(node a, node b){ // 对 list中的f继续排序  
	//if(a.f != b.f)
		return a.f < b.f;
	//return a.h < b.h;
}

bool aStart::isInclude(list arr, node qur){ // 判断一个节点是否在list中 
	list::iterator it;
	for(it = arr.begin(); it != arr.end(); it++){
		if(it->x == qur.x && it->y == qur.y){
			return true;
		}
	}
	return false;
}

void aStart::dfs(int x, int y){ // 回溯路径 
	if(x == start.x && y == start.y)
		return;
	else
		dfs(road[x][y].x, road[x][y].y);
	maze[x][y] = '@';
	ans += road[x][y].dir; 
}

void aStart::getDir(int i, node nei){
	switch(i){
		case 0: road[nei.x][nei.y].dir = 'Q'; break;
		case 1: road[nei.x][nei.y].dir = 'W'; break;
		case 2: road[nei.x][nei.y].dir = 'E'; break;
		case 3: road[nei.x][nei.y].dir = 'A'; break;
		case 4: road[nei.x][nei.y].dir = 'D'; break;
		case 5: road[nei.x][nei.y].dir = 'Z'; break;
		case 6: road[nei.x][nei.y].dir = 'X'; break;
		case 7: road[nei.x][nei.y].dir = 'C'; break;
	}
}

void aStart::aStartSearch(){
	int cnt = 0, cou = n, flag = 0;
	openSet.clear();
	closeSet.clear();
	openSet.push_front(start);
	while(!openSet.empty()){
		openSet.sort(cmp);
		node now = openSet.front();
		/*if(cnt >= 50){
			cou += n;
			cnt = 0;
		}
		gotoxy(cou+2, cnt++);
		cout << now.f << "--" << now.x << "--" << now.y << endl; 
		gotoxy(cou+2, cnt++);
		cout << "--------------------" << endl;
		*/
		if(now.x == fin.x && now.y == fin.y){ // 当终点在openSet中时结束循环 
			dfs(fin.x, fin.y);
			drawMap();
			cout << ans << endl;
			cout << "DONE" << endl;
			break;
		}
		openSet.pop_front();
		closeSet.push_back(now);
		for(int i = 0; i < 8; i++){
			flag = 0;
			node nei = map[now.x+dirx[i]][now.y+diry[i]];
			if(maze[now.x+dirx[i]][now.y+diry[i]] == '#' || now.x+dirx[i] >= n+1 || now.x+dirx[i] <= 0 || now.y+diry[i] >= m+1 || now.y+diry[i] <= 0)
		 		continue;
 			// 判断改节点是否在closeSet中 
		 	if(isInclude(closeSet, nei))
		 		continue;
	 		node tmp; // 临时节点,用来判断是否需要改变当前邻居的属性 
	 		tmp.g = now.g + getG(i); // 计算新的g值 
	 		//cout << now.g << endl;
	 		if(!isInclude(openSet, nei)){ // 如果不在openSet中，则加入并更新父节点 
	 			getDir(i, nei);
	 			road[nei.x][nei.y].x = now.x; 
				road[nei.x][nei.y].y = now.y;
	 			openSet.push_front(nei);
	 			flag = 1;
	 		}
		 	else if(tmp.g >= nei.g) // 如果当前节点的g比原来的g大，直接跳过 
		 		continue;
	 		// 如果比较小
	 		if(flag == 1) // 如果当前节点原来不在openSet中，且g更小 
		 		openSet.pop_front(); // 先把之前的节点删除
			// 更新父节点信息 
			getDir(i, nei);
			road[nei.x][nei.y].x = now.x; 
			road[nei.x][nei.y].y = now.y;
			// 更新改邻居节点的属性
		 	nei.g = tmp.g;
		 	nei.h = getH(nei, fin); 
		 	nei.f = nei.g + nei.h; // f = g + h;
		 	map[now.x+dirx[i]][now.y+diry[i]] = nei;
		 	openSet.push_front(nei); // 加入到openSet中 
 			//gotoxy(cou+2, cnt++);
		 	//cout << map[now.x+dirx[i]][now.y+diry[i]].f << '-' << map[now.x+dirx[i]][now.y+diry[i]].x << '-' << map[now.x+dirx[i]][now.y+diry[i]].y << endl;
		}
		//gotoxy(cou+2, cnt++);
		//cout << "--------------------" << endl;
		debug();
		Sleep(100);
	}
}

int main(){
	aStart t;
	while(1){
		t.initMap();
		t.drawMap();
		t.aStartSearch();
	}
}