启发式搜索——A*算法的实现

算法原理参考自 http://blog.youkuaiyun.com/mynamelj/article/details/3071867

算法实现比较粗浅，但符合A*算法要求。

#define MAX_SIZE 800
#include <fstream>
#include <iostream> 
#include <cmath> 
#include <malloc.h>
#include <time.h>
using namespace std; 
struct tnode{ 
	int gvalue;//以下3个参数是估计函数 
	int hvalue; 
	int fvalue; 
	tnode* parent;//不是父节点，而是指向当前节点 
	tnode* next;//指向链表的下一个节点 
	int pass; 
	int x;
	int y;
}; 

tnode table[MAX_SIZE][MAX_SIZE];//存储地图
int MAX_SIZE_X,MAX_SIZE_Y;
int startx,starty,endx,endy; 
tnode openlist,closelist; 


//遍历链表，判段是否在列表中，找到返回1，否则返回0 
int search(tnode *plist,int x,int y) 
{
	tnode *p=plist;
	while(1)
	{
		if(p->next==NULL)
		{
			return 0;
		}
		if(p->next->x==x&&p->next->y==y)
		{
			return 1;
		}
		p=p-> next; 
	}
	return 0;
} 
//把table[hx][hy]加入openlist链表 
int addtoopenlist(int hx,int hy) 
{ 
	//cout << "add to openlist "<< hx<<","<<hy<<endl;
	tnode *plist,*qlist=0; 
	plist=&openlist;
	while(1) 
	{ 
		if(plist->next==NULL)
		{
			break;
		}
		if(plist->next->x==hx&&plist->next->y==hy)
		{
			return 0;
		}
		plist=plist-> next; 
	}
	plist->next=&table[hx][hy];
	return 1;
} 
//把table[hx][hy]加入closelist链表 
int addtocloselist(int hx,int hy) 
{ 
	//cout << "add to closelist "<< hx<<","<<hy<<endl;
	tnode *plist,*qlist=0; 
	plist=&closelist;
	while(1) 
	{ 
		if(plist->next==NULL)
		{
			break;
		}
		if(plist->next->x==hx&&plist->next->y==hy)
		{
			return 0;
		}
		plist=plist-> next; 
	}
	plist->next=&table[hx][hy];
	return 1;
} 
//把table[hx][hy]从openlist中移除
int delfromopenlist(int hx,int hy) 
{ 
	//cout << "delete from openlist "<< hx<<","<<hy<<endl;
	tnode *plist,*qlist=0;
	plist=&openlist; 
	while(1) 
	{
		if(plist->next==0)
		{
			return 0;
		}
		if(plist->next->x==hx&&plist->next->y==hy)
		{
			qlist=plist->next->next;
			plist->next->next=NULL;
			plist->next=qlist;
			break;
		}
		plist=plist-> next; 
	}
	return 1;
} 
int calculateGvalue(int hx,int hy,int px,int py,int value)
{
	if(abs(hx-px)==1&&abs(hy-py)==1)
	{
		return value+14;//斜着走
	}
	else
	{
		return value+10;//直着走
	}
}
int calculateHvalue(int hx,int hy)
{
	return (abs(hx-endx)+abs(hy-endy))*10;
}
int handlenode(int hx,int hy,int curx,int cury) 
{
	//cout << "handle node " << hx <<"," << hy << endl;
	if(hx<0||hy<0||hx>=MAX_SIZE_X||hy>=MAX_SIZE_Y)
		return 0;
	把目标格添加进开启列表
	//addtoopenlist(hx,hy);
	//这时候路径被找到
	if(hx==endx&&hy==endy)
	{
		table[hx][hy].parent=&table[curx][cury];
		return 1;
	}
	//如果它不可通过或者已经在关闭列表中,略过它
	if(table[hx][hy].pass==0)
	{
		return 0;
	}
	//cout << "search closelist " << hx <<"," << hy << endl;
	if(search(&closelist,hx,hy)==1)
	{
		//cout << "hx hy already in closelist " << hx <<"," << hy << endl;
		return 0;
	}
	//如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
	if(search(&openlist,hx,hy)==0)
	{
		if(addtoopenlist(hx,hy)==1)
		{
			//cout << "add to openlist success " << hx <<"," << hy << endl;
		}
		table[hx][hy].parent=&table[curx][cury];
		table[hx][hy].gvalue=calculateGvalue(hx,hy,table[curx][cury].x,table[curx][cury].y,table[curx][cury].gvalue);
		table[hx][hy].hvalue=calculateHvalue(hx,hy);
		table[hx][hy].fvalue=table[hx][hy].gvalue+table[hx][hy].hvalue;
	}
	else
	{
		//cout << "hx hy already in openlist "<< hx <<"," << hy << endl;
		//更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。
		//如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
		if(calculateGvalue(hx,hy,table[curx][cury].x,table[curx][cury].y,table[curx][cury].gvalue)<table[hx][hy].gvalue)
		{
			table[hx][hy].parent=&table[curx][cury];
			table[hx][hy].gvalue=calculateGvalue(hx,hy,table[curx][cury].x,table[curx][cury].y,table[curx][cury].gvalue);
			table[hx][hy].hvalue=calculateHvalue(hx,hy);
		}
	}
	return 0;
} 
tnode* findminFinopenlist()
{
	tnode *p,*q=0;
	p = openlist.next;
	q = openlist.next;
	if(q==NULL)
		return q;
	int minvalue=q->fvalue;
	while(1)
	{
		if(p==NULL)
		{
			break;
		}
		if(p->fvalue<minvalue)
		{
			minvalue=p->fvalue;
			q = p;
		}
		p = p->next;
	}
	return q;
}
int computervalue(int curx,int cury) 
{//对每一个当前节点执行以下操作 
	if(handlenode(curx-1,cury-1,curx,cury)==1)
		return 1;
	if(handlenode(curx-1,cury,curx,cury)==1)
		return 1;
	if(handlenode(curx-1,cury+1,curx,cury)==1)
		return 1;
	if(handlenode(curx,cury-1,curx,cury)==1)
		return 1;
	if(handlenode(curx,cury+1,curx,cury)==1)
		return 1;
	if(handlenode(curx+1,cury-1,curx,cury)==1)
		return 1;
	if(handlenode(curx+1,cury,curx,cury)==1)
		return 1;
	if(handlenode(curx+1,cury+1,curx,cury)==1)
		return 1;	
	return 0;
}
void readmap()
{
	ifstream infile;
	infile.open("map.txt"); //注意文件的路径
	infile>>MAX_SIZE_X>>MAX_SIZE_Y; //两行数据可以连续读出到变量里
	for(int i=0;i<MAX_SIZE_X;i++)
	{
		for(int j=0;j<MAX_SIZE_Y;j++)
		{
			int x=1;
			infile >>  x;
			table[i][j].pass=x; 
		}
	}
	infile.close();
	return ;
}

void main()
{
	tnode *pp;
	openlist.next=NULL;
	closelist.next=NULL;
	MAX_SIZE_X=500;
	MAX_SIZE_Y=500;
	for (int i=0;i <MAX_SIZE_X;i++)//初始化 
	{
		for (int j=0;j <MAX_SIZE_Y;j++) 
		{
			table[i][j].gvalue=0; 
			table[i][j].hvalue=0; 
			table[i][j].fvalue=0; 
			table[i][j].parent=NULL;
			table[i][j].next=NULL;
			table[i][j].pass=1; 
			table[i][j].x=i;
			table[i][j].y=j;
		}
	}
	readmap();
	//int x,y;
	//cout<<"输入不可以通过的位置，格式为坐标X 坐标Y ,-1 -1结束" <<endl; 
	//for(;;) 
	//{ 
	//	cin>> x>> y;
	//	if(x==-1&&y==-1)
	//	{
	//		break;
	//	}
	//	table[x][y].pass=0; 
	//} 
	
	cout << "输入起始的位置，格式为坐标X 坐标Y " <<endl; 
	cin>> startx>> starty; 
	cout << "输入结束的位置，格式为坐标X 坐标Y " <<endl; 
	cin>> endx>> endy; 
	if(MAX_SIZE<=100)
	{
		for (int i=0;i <MAX_SIZE;i++)//打印地图结构 
		{ 
			cout <<endl; 
			for (int k=0;k <MAX_SIZE;k++) 
				cout <<table[i][k].pass; 
		}
	}
	cout << endl;
	clock_t start, finish;
	double duration;
	start = clock();
	addtoopenlist(startx,starty);//把起始格添加到开启列表。
	while(1)
	{
		//寻找开启列表中F值最低的格子。我们称它为当前格。
		tnode *p = findminFinopenlist();
		//没有找到目标格，开启列表已经空了。这时候，路径不存在。
		if(p==NULL)
		{
			break;
		}
		if(delfromopenlist(p->x,p->y)==1)
		{
			//cout << "delete from openlist success " << p->x <<"," << p->y << endl;
		}
		if(addtocloselist(p->x,p->y)==1)
		{
			//cout << "add to closelist success " << p->x <<"," << p->y << endl;
		}
		//把目标格添加进了开启列表，这时候路径被找到
		//break;
		if(computervalue(p->x,p->y)==1)
		{
			break;
		}
	}
	finish = clock();
	duration = (double)(finish - start)/CLOCKS_PER_SEC; 
	printf( "using %lf seconds\n", duration );   
	pp=&table[endx][endy]; 
	pp->next=NULL;
	while(1) 
	{ 
		if(pp->parent==NULL)
			break;
		pp->parent->next=pp;
		pp=pp-> parent; 
	} 
	while(1)
	{
		if(pp->next==NULL)
			break;
		cout << "("<<pp-> x<<","<< pp-> y <<")"<< " => "; 
		pp=pp-> next;
	}
	cout <<"("<<endx<<","<<endy <<")"<<endl;
	system( "pause ");
}

附：地图文件

50 50
1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1