A*算法尋路（C++代碼實現）

A*（A-Star)算法是一種靜態路網中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。算法中的距離估算值與實際值越接近，最終搜索速度越快。——來自百度百科。
我在網上看了不少關於A*尋路的文章，基本都能看懂。但是大多數文章中沒有代碼實現，或者是一些我不會的某些語言，還有的代碼注釋太少了而且太長，我看着看着就看不下去了。所以我就自己寫了A*算法尋路的C++代碼。

A*尋路

A*尋路算法詳解推薦閱讀博文鏈接：http://www.cppblog.com/mythit/archive/2009/04/19/80492.aspx
A*尋路的講解文章網上很多，有的文章講的很好，有圖可以讓讀者一步步理解。其中我本人覺得這篇文章講的最好，這篇文章應該是翻譯的英文原文。我覺得我再寫關於A*尋路的講解肯定也沒人家寫的條理清晰，索性干脆推薦大家讀別人的文章。但是，它只有講解，沒有代碼，所以想看代碼的同學就可以看本文的C++代碼。

根據那篇文章，我對下面的代碼做一些簡單介紹：

1. 每個節點我們定義成一個類，其中包含坐標x、y，評估函數F、G、H，它們的含義同原文一致。
2. 定義一個三維數組path，用於存儲每個位置的方格對應的“父方格”的坐標。
3. 二維數組valF保序每個方格目前情況下最小的F值。
4. 由於每次需要從open表中彈出的是F值最小的節點，我們選擇使用優先隊列來作為open表。
5. 定義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;
}