A* 算法实现与解析

【算法分析】
A*算法适用于结点特别多的场景，常借助于优先队列 priority_queue 实现。
在A*算法的实现中，形如下方的代码，为“结构体内嵌比较函数”。之所以这样写，是因为其执行速度快。
bool operator<(const NODE &a) const {
    return F==a.F?G>a.G:F>a.F;
}

下面算法代码执行后，将输出一条如下图所示的绿色最短路径。

 

A*算法的经典解析可参见：
https://cloud.tencent.com/developer/article/2040931
https://blog.csdn.net/m0_46304383/article/details/113457800
https://blog.csdn.net/dujuancao11/article/details/109749219
https://www.acwing.com/blog/content/352/
https://blog.csdn.net/qq_43827595/article/details/99546055


【算法代码】
下面代码改编自：https://blog.csdn.net/m0_46304383/article/details/113457800

#include <bits/stdc++.h>
using namespace std;

const int N=10; //Map order
bool close[N][N]; //Access records, close lists
int valueF[N][N]; //Record the F value of each node
int pre[N][N][2]; //Stores the parent node of each node

int env[N][N]= { //Scene Map
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	{0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
	{0, 0, 1, 1, 0, 0, 0, 1, 0, 0},
	{0, 0, 0, 1, 0, 0, 0, 1, 0, 0},
	{0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
	{0, 0, 1, 1, 0, 1, 0, 0, 0, 0},
	{0, 0, 1, 0, 1, 0, 1, 0, 0, 0},
	{0, 0, 1, 0, 0, 0, 0, 1, 0, 0},
	{0, 1, 0, 1, 0, 0, 0, 0, 0, 0},
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};

struct NODE {
	int x,y;   //Node location
	int F,G,H; //F=G+H
	NODE(int a, int b) { //constructor function
		x=a,y=b;
	}
	//struct's nested comparison function,speed is faster.
	bool operator<(const NODE &a) const {
		return F==a.F?G>a.G:F>a.F;
	}
};

//define the direction
//const int ne_pos[8][2]={{-1,-1},{-1,0},{-1,1},{0,1},{1,1},{1,0},{1,-1},{0,-1}};
const int ne_pos[4][2]= {{-1,0},{0,1},{1,0},{0,-1}};
priority_queue<NODE> open;

int Manhattan(int x0,int y0,int x1,int y1) {
	return (abs(x0-x1)+abs(y0-y1));
}

bool isValidNode(int x,int y,int u,int v) {
	if(x<0||x>=N||y<0||y>=N) return false; //Judge boundary
	if(env[x][y]==1) return false; //Judge obstacle
	/*When two nodes are diagonal and
	their common adjacent nodes have obstacles, return false.
	The rule is used when the scene map has 8 directions.*/
	if(x!=u && y!=v && (env[x][v]==1 || env[u][y]==1)) return false;
	return true;
}

void Astar(int x0,int y0,int x1,int y1) {
	NODE node(x0,y0); //Starting point is added into the open list.
	node.G=0;
	node.H=Manhattan(x0,y0,x1,y1);
	node.F=node.G+node.H;
	valueF[x0][y0]=node.F;
	open.push(node);

	while(!open.empty()) {
		/*Take the head element of priority queue,
		which is the point with the least cost in the surrounding cells*/
		NODE cur_node=open.top();
		open.pop(); //Remove from the open list
		//Access this point and add it to the close list
		close[cur_node.x][cur_node.y]=true;
		if(cur_node.x==x1 && cur_node.y==y1) break; //Reach the finish line

		/*Traverse the four directions around the top_node,
		or if the array ne_pos has eight values,
		then you need to traverse the eight directions around the top_node.
		*/
		for(int i=0; i<4; i++) {
			//Create a point around top_node
			NODE ne_node(cur_node.x+ne_pos[i][0],cur_node.y+ne_pos[i][1]);
			//The node coordinate is valid and has not been accessed
			if(isValidNode(ne_node.x,ne_node.y,cur_node.x,cur_node.y) && !close[ne_node.x][ne_node.y]) {
				/*Calculate the cost to reach the node
				from the starting point and through the top_node node*/
				ne_node.G=cur_node.G+int(sqrt(pow(ne_pos[i][0],2)+pow(ne_pos[i][1],2)));
				//Calculate the Manhattan distance from the node to the destination
				ne_node.H=Manhattan(ne_node.x,ne_node.y,x1,y1);
				/*The estimated cost to reach the destination
				 from the starting point through the top_node
				 and the current node*/
				ne_node.F=ne_node.G+ne_node.H;

				/*ne_node.F<valueF[ne_node.x][ne_node.y],
				  When the condition is met, a better path is found and updated.
				  valueF[ne_node.x][ne_node.y]==0,
				  When the condition is met, the node is not added to the OPEN table,
				  it is needed to be added to the open table*/
				if(ne_node.F<valueF[ne_node.x][ne_node.y] || valueF[ne_node.x][ne_node.y]==0) {
					//Save the parent node of the node
					pre[ne_node.x][ne_node.y][0]=cur_node.x;
					pre[ne_node.x][ne_node.y][1]=cur_node.y;
					//Modify the valueF value of the node
					valueF[ne_node.x][ne_node.y]=ne_node.F;
					open.push(ne_node);
				}
			}
		}
	}
}

void PrintPath(int x1,int y1) {
	if(pre[x1][y1][0]==-1 || pre[x1][y1][1]==-1) {
		printf("No path to get.\n");
		return;
	}
	int x=x1,y=y1;
	int a,b;
	while(x!=-1 || y!=-1) {
		env[x][y]=2; //Assign a value of 2 to the node on the feasible path
		a=pre[x][y][0];
		b=pre[x][y][1];
		x=a;
		y=b;
	}

	/*
	"  " represents an unpassed node,
	" #" represents an obstacle,
	" @" represents a feasible node.
	*/
	string s[3]= {"  "," #", " @"};
	for(int i=0; i<N; i++) {
		for(int j=0; j<N; j++) {
			cout<<s[env[i][j]]; //s[] is string, must use cout to output
		}
		cout<<endl;
	}
}

int main(){
	memset(close,false,sizeof close); //Initialize close array to false
	memset(valueF,0,sizeof valueF); //Initialize F to 0
	memset(pre,-1,sizeof pre); //Initialize pre array to -1

	int x0=2,y0=4; //start point
	int x1=8,y1=6; //end point

	if(!isValidNode(x0,y0,x0,y0)){
		printf("Invalid input.\n");
		return 0;
	}

	Astar(x0,y0,x1,y1); //A* algorithm
	PrintPath(x1,y1); //Print the path

	return 0;
}


/*
output:
             @ @ @
   #         @ # @
     # # @ @ @ # @
       #       # @
             #   @
     # #   #     @
     #   #   #   @
     #         # @
   #   #     @ @ @
*/

【未加注释的算法代码】
鉴于上面代码注释过多，为方便大家快速阅读代码，下面给出的是未加注释版本的算法代码。

#include <bits/stdc++.h>
using namespace std;

const int N=10;
bool close[N][N];
int valueF[N][N];
int pre[N][N][2];

int env[N][N]= {
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	{0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
	{0, 0, 1, 1, 0, 0, 0, 1, 0, 0},
	{0, 0, 0, 1, 0, 0, 0, 1, 0, 0},
	{0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
	{0, 0, 1, 1, 0, 1, 0, 0, 0, 0},
	{0, 0, 1, 0, 1, 0, 1, 0, 0, 0},
	{0, 0, 1, 0, 0, 0, 0, 1, 0, 0},
	{0, 1, 0, 1, 0, 0, 0, 0, 0, 0},
	{0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
};

struct NODE {
	int x,y;
	int F,G,H;
	NODE(int a, int b) {
		x=a,y=b;
	}
	bool operator<(const NODE &a) const {
		return F==a.F?G>a.G:F>a.F;
	}
};

const int ne_pos[4][2]= {{-1,0},{0,1},{1,0},{0,-1}};
priority_queue<NODE> open;

int Manhattan(int x0,int y0,int x1,int y1) {
	return (abs(x0-x1)+abs(y0-y1));
}

bool isValidNode(int x,int y,int u,int v) {
	if(x<0||x>=N||y<0||y>=N) return false;
	if(env[x][y]==1) return false;
	if(x!=u && y!=v && (env[x][v]==1 || env[u][y]==1)) return false;
	return true;
}

void Astar(int x0,int y0,int x1,int y1) {
	NODE node(x0,y0);
	node.G=0;
	node.H=Manhattan(x0,y0,x1,y1);
	node.F=node.G+node.H;
	valueF[x0][y0]=node.F;
	open.push(node);

	while(!open.empty()) {
		NODE cur_node=open.top();
		open.pop();
		close[cur_node.x][cur_node.y]=true;
		if(cur_node.x==x1 && cur_node.y==y1) break;

		for(int i=0; i<4; i++) {
			NODE ne_node(cur_node.x+ne_pos[i][0],cur_node.y+ne_pos[i][1]);
			if(isValidNode(ne_node.x,ne_node.y,cur_node.x,cur_node.y) && !close[ne_node.x][ne_node.y]) {
				ne_node.G=cur_node.G+int(sqrt(pow(ne_pos[i][0],2)+pow(ne_pos[i][1],2)));
				ne_node.H=Manhattan(ne_node.x,ne_node.y,x1,y1);
				ne_node.F=ne_node.G+ne_node.H;
				if(ne_node.F<valueF[ne_node.x][ne_node.y] || valueF[ne_node.x][ne_node.y]==0) {
					pre[ne_node.x][ne_node.y][0]=cur_node.x;
					pre[ne_node.x][ne_node.y][1]=cur_node.y;
					valueF[ne_node.x][ne_node.y]=ne_node.F;
					open.push(ne_node);
				}
			}
		}
	}
}

void PrintPath(int x1,int y1) {
	if(pre[x1][y1][0]==-1 || pre[x1][y1][1]==-1) {
		printf("No path to get.\n");
		return;
	}
	int x=x1,y=y1;
	int a,b;
	while(x!=-1 || y!=-1) {
		env[x][y]=2;
		a=pre[x][y][0];
		b=pre[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[env[i][j]];
		}
		cout<<endl;
	}
}

int main() {
	memset(close,false,sizeof close);
	memset(valueF,0,sizeof valueF);
	memset(pre,-1,sizeof pre);

	int x0=2,y0=4;
	int x1=8,y1=6;
	if(!isValidNode(x0,y0,x0,y0)) {
		printf("Invalid input.\n");
		return 0;
	}

	Astar(x0,y0,x1,y1);
	PrintPath(x1,y1);

	return 0;
}


/*
output:
             @ @ @
   #         @ # @
     # # @ @ @ # @
       #       # @
             #   @
     # #   #     @
     #   #   #   @
     #         # @
   #   #     @ @ @
*/

【参考文献】
https://cloud.tencent.com/developer/article/2040931
https://blog.csdn.net/m0_46304383/article/details/113457800
https://blog.csdn.net/dujuancao11/article/details/109749219
https://www.acwing.com/blog/content/352/
https://blog.csdn.net/qq_43827595/article/details/99546055