C++实现A*算法基于路网图

A*寻路算法

算法概述

1. 从起点start到达目标点target并经过点x的估计距离长度表示为f(x) = g(x) + h(x)，该公式是A*算法的核心公式。

2. g(x)表示实际路网距离，h(x)表示从点x到target的估值，一般用欧式距离或者曼哈顿距离表示。

3. A*算法通过不断的选择估计距离f(x)最小的节点，逐渐构建最短路径。

4. 用两个表open表（按照F值从大到小排序，若相等按照G值从大到小排序）和close表来存储选择过程。

5. 算法搜索过程
   核心

   两个集合:Open,Closed和一个公式：f(n)=g(n)+h(n)。其中Open,Closed都是存储节点的集合，Open存储可到达的节点，Closed存储已经到达的节点；而公式f(n)=g(n)+h(n)则是对节点价值的评估，g(n)代表从起点走到当前节点的成本，也就是走了多少步，h(n)代表从当前节点走到目标节点的距离，即不考虑障碍的情况下，离目标还有多远。至于f(n)，则是对g(n)和h(n)的综合评估，我们应该尽量选择步数更少，离目标更近的节点，那么f(n)的值越小越好。

   搜索思路

   开始时，Closed表为空，Open表仅包括起始节点，每次迭代中，A*算法将Open表中具有最小代价之的节点去除进行检查，检查后的节点放入Closed表，如果这个节点不是目标节点，那么考虑该节点的所有相邻节点。对于每个相邻节点按下列规则处理；

   （1）如果相邻节点既不在Open表中，又不在Closed表中，则将它加入Open表中；
   （2）如果相邻节点已经在Open表中，并且新的路径具有更低的代价值，则更新它的信息；
   （3）如果相邻节点已经在Closed表中，那么需要检查新的路径是否具有更低的代价值，如果是，那么将它从Closed表中移出，加入到Open表中，否则忽略。（这里需要检查Closed表是因为要防止由于h(n)计算不准确而导致的误差）

   重复上述步骤，直到到达目标节点。如果在到达目标之前，Open表就已经变空，则意味着在起始位置和目标位置之间没有可达的路径。

流程图

引用博客链接: link.

伪代码

// A* algorithm Pseudo code block
 A*(int start ,int target)
{
	Open = [start];
	Closed = []
while (Open表非空)
{
	从Open中取得一个节点X，并从OPEN表中删除。
	 if (X是目标节点)
　　{
　　  根据前向father求得路径PATH以及最短路径cost；
　　  return 路径PATH；
　　}
	Open.pop();
	Close.push(X);
　　for (每一个X的邻节点Y)
　　{
		if (Y不在Open表和Close表中)
　　	  {
         	求Y的估价值G并计算FH；
			并将Y插入Open表中；
	  }else if (Y在Open表中)
　		{
				if (Y的估价值小于Open表的估值G)
				update Open表中的估值；  
		}  else //Y在Close表中
　     {
         if (Y的估值小于CLOSE表的估值G)
　　     {
			update Close表中的估值；
			从Close表中移出节点，并放入Open表中；
		}
	}	//end for
	sort(),按照估价值将Open表中的节点排序；
　}   //end while
}     //end function

C++实现

// An highlighted block
struct Node{
	int id;
	double x,y;
	//int id,x,y;
	vector<int> adjnodes;
	vector<int> adjweight;
	double F = 0.0;
	double G = 0.0;
	double H = 0.0;
	int father;
	//Constructors
	Node(): id(-1),x(-1), y(-1){}
	Node(int a,double b,double c): id(a),x(b), y(c){}
};
map<Node>Nodes;
// return Astar_p2p(int s, int t)
double Astar(int S,int T){
	if(S == T) return 0.0; 
	vector<Node> openlist;
	vector<Node> closelist;
	openlist.clear();
	closelist.clear();
	Nodes[S].father = -1; 
	Nodes[S].H = Euclidean_Dist(S,T);
	Nodes[S].F = Nodes[S].G + Nodes[S].H; 
	openlist.push_back(Nodes[S]);
	while(!openlist.empty()){
		Node current = openlist.back();//min_F node
		if(current.id == T){
			return current.F;
		}
		openlist.pop_back();
		closelist.push_back(Nodes[current.id]);
		vector<int>neighbors;
		for(int i=0;i<Nodes[current.id].adjnodes.size();i++){
			neighbors.push_back(Nodes[current.id].adjnodes[i]);
		}
		
		for(int neighbor:neighbors){
			bool b1 = false,b2 = false;
			if( !(b1 = in_list(neighbor,openlist))&&!(b2 = in_list(neighbor,closelist))){//not in both
				Nodes[neighbor].father = current.id;
				double w = 0.0;
				for (int i = 0; i < Nodes[current.id].adjnodes.size(); i++) {
            		if(Nodes[current.id].adjnodes[i] == neighbor)
            			w = (double)Nodes[current.id].adjweight[i];
            	}
            	Nodes[neighbor].G = Nodes[current.id].G + w; 
            	Nodes[neighbor].H = Euclidean_Dist(neighbor,T);
            	Nodes[neighbor].F = Nodes[neighbor].G + Nodes[neighbor].H; 
            	openlist.push_back(Nodes[neighbor]);
			}else if(b1){// in open update G & F
				double w = 0.0;
				for (int i = 0; i < Nodes[current.id].adjnodes.size(); i++) {
            		if(Nodes[current.id].adjnodes[i] == neighbor)
            			w = (double)Nodes[current.id].adjweight[i];
            	}
				double g = w + Nodes[current.id].G;
				double f = g + Nodes[neighbor].H;			
				if(g < Nodes[neighbor].G){
						for(vector<Node>::iterator it = openlist.begin();it!=openlist.end();){
							if((*it).id == neighbor)
								it = openlist.erase(it);
							else
								++it;
						}
					Nodes[neighbor].father = current.id;
					Nodes[neighbor].F = f;
					Nodes[neighbor].G = g ;
					openlist.push_back(Nodes[neighbor]);
				}
			}else{//in close if g<G delete and into open
				double w = 0.0;
				for (int i = 0; i < Nodes[current.id].adjnodes.size(); i++) {
            		if(Nodes[current.id].adjnodes[i] == neighbor)
            			w = (double)Nodes[current.id].adjweight[i];
            	}
				double g = w + Nodes[current.id].G;
				double f = g + Nodes[neighbor].H;			
				if(g < Nodes[neighbor].G){
						for(vector<Node>::iterator it = closelist.begin();it!=closelist.end();){
							if((*it).id == neighbor)
								it = closelist.erase(it);
							else
								++it;
						}
					Nodes[neighbor].father = current.id;
					Nodes[neighbor].F = f;
					Nodes[neighbor].G = g ;
					openlist.push_back(Nodes[neighbor]);
				}
			}// end else	
	   }//end for
		sort(openlist.begin(),openlist.end(),cmp);	
	}//end while
}