C++实现A星算法自动寻路

虽然网上已经有许多A星算法的实现，但大多过于繁琐。这是实现的精简版，必须要吐槽一下C++对象的指针和引用，还有那让人蛋疼的while和递归居然是两码事（这bug调了我一晚上）。不行你试试，把下面的start()的方法改下成while() 注释可以参考http://blog.csdn.net/liucanlong/article/details/10334035

 

 

#include "iostream"
#include "set"
#include "stdlib.h"
#include "vector"
#define N 1000
using namespace std;
class Node{
	public:
		int x;
		int y;
		int g;
		int h;
		int f;
		Node *parent;
		Node(int,int);
		Node(){}
		bool operator <(const Node &other)const{
			if(this->f!=other.f){
				return this->f<other.f;
			}else {
				return true;//允许相同 
			}
			
		}
}; 
set<Node> full;
set<Node> openL;
set<Node> closeL;
vector<Node> resultL;
int map[N][N]={ 	{0,0,0,0,0,0,0,0,0,0},

	                {0,0,0,0,1,0,0,0,0,0},

	                {0,0,0,0,1,0,0,0,0,0},

	                {0,0,0,0,1,0,0,0,0,0},

	                {0,0,0,0,1,0,0,0,0,0},

	                {0,0,0,0,1,0,0,0,0,0}};
int col;
int row;
int line = 10;
int xie = 14;

int isContains(int x,int y,set<Node> *pL){
	set<Node>::iterator iter = pL->begin();
	for(int i=0;iter!=pL->end();iter++,i++){
		if(iter->x==x&&iter->y==y)
			return i;
	}
	return -1;
}
int isContains(int x,int y,vector<Node> *pL){
	vector<Node>::iterator iter = pL->begin();
	for(int i=0;iter!=pL->end();iter++,i++){
		if(iter->x==x&&iter->y==y)
			return 1;
	}
	return 0;
}
int countH(int x,int y,int endX,int endY){
	int xSub = abs(x-endX);
    int ySub = abs(y-endY);
    int minXY = xSub>ySub?ySub:xSub;
    return minXY*xie+abs(xSub-ySub)*line;
}

void check(int x,int y,int cost,Node *parent,int endX,int endY){
	if(x<0||x>=row||y<0||y>=col) return;
	Node node(x,y);
	node.parent = parent;
	
	node.g = parent->g+cost;
	if(isContains(x,y,&closeL)!=-1) return;
	if(map[x][y]==1){
		closeL.insert(node);
		return;
	}
	int index = isContains(x,y,&openL);
	if(index!=-1){
		set<Node>::iterator iter = openL.begin();
		for(int i=0;i<index;i++)
			iter++;
		if(node.g<iter->g){
			node.f = node.g+iter->h;
			node.h = iter->h;
			openL.erase(iter);
			openL.insert(node);
		}
	}else{
		node.h = countH(x,y,endX,endY);
		node.f = node.h+node.g;
		openL.insert(node);
	}
}
void getPath(Node *node){
	while(node->parent==NULL){
		resultL.push_back(*node);
		return;
	}
	getPath(node->parent);
	resultL.push_back(*node);
}

int start(int endX,int endY){
	if(openL.empty()) return 0;
	set<Node>::iterator iter = openL.begin();
	
	Node n = *iter;
	if(n.x==endX&&n.y==endY){
		getPath(&n);
		return 1;
	}
	check(n.x,n.y-1,line,&n,endX,endY); //左 
	check(n.x,n.y+1,line,&n,endX,endY); //右 
	check(n.x-1,n.y,line,&n,endX,endY); //上
	check(n.x+1,n.y,line,&n,endX,endY); //下
	check(n.x-1,n.y-1,xie,&n,endX,endY); //左上
	check(n.x-1,n.y+1,xie,&n,endX,endY); //右上 
	check(n.x+1,n.y-1,xie,&n,endX,endY); //左下
	check(n.x+1,n.y+1,xie,&n,endX,endY); //右下 
	closeL.insert(n);
	openL.erase(iter);
	return start(endX,endY);
}

int search(int startX,int startY,int endX,int endY){
	if(map[startX][startY]==1||map[endX][endY]==1) return -1;
	Node *root = new Node(startX,startY);
	openL.insert(*root);
	return start(endX,endY);
}

int main(){		
	col = 10;
	row = 6;
	int flag = search(3, 0, 4, 7);
	if(flag==0){
		cout<<"no find!"<<endl;
	}else if(flag==-1){
		cout<<"format error"<<endl;
	}else if(flag==1){
		for(int i=0;i<row;i++){
			for(int j=0;j<col;j++){
				if(map[i][j]==1) cout<<"#";
				else{
					if(isContains(i,j,&resultL)){
						cout<<"@";
					}else{
						cout<<" ";
					}
				}
			}
			cout<<endl;
		}
	}
	system("pause");
	return 0;
}

Node::Node(int x,int y){
	this->x = x;
	this->y = y;
	this->g = 0;
	this->h = 0;
	this->f = 0;
	this->parent = NULL;
}