A*寻路

A星寻路

关键词

Opened 表

Closed表

F(x)=G(x)+H(x)

#include <iostream>
#include <algorithm>
#include <cstdio>
#include <cstring>

using namespace std;

int w,h;
int x,y;
int map[100][100];
int dx[]={1,0,-1,0};
int dy[]={0,1,0,-1};

struct Data
{
	int x,y;
	int G,H;
	int F;
	Data *pNext;
} pOpened;

void Insert(int x,int y,int G,int H)
{
	Data *pNew=new Data;
	pNew->pNext=pOpened.pNext;
	pNew->x=x;pNew->y=y;pNew->G=G;pNew->H=H;
	pNew->F=pNew->G+pNew->H;
	pOpened.pNext=pNew;
}

void Delete(Data *pFat)
{
	Data *q;

	q=pFat->pNext;
	pFat->pNext=q->pNext;
	delete q;
}

bool Opened[100][100];
bool Closed[100][100];

int H(int x,int y,int gx,int gy)
{
	return abs(x-gx)+abs(y-gy);
}
int t=0;
Data *GetNode(int x,int y)
{
	Data *pNode=pOpened.pNext;
	while(pNode->x!=x || pNode->y!=y)
	{
		pNode=pNode->pNext;
	}

	return pNode;
}

Data *GetMinF(Data *pHead)
{
	int minf=9999999;
	Data *pF;
	Data *q=pHead;
	Data *pNode=pHead->pNext;

	while(pNode)
	{
		if(pNode->F<minf)
		{
			pF=q;
			minf=pNode->F;
		}
		q=pNode;
		pNode=pNode->pNext;
	}

	return pF;
}

int AStar(int sx,int sy,int gx,int gy)
{
	int x,y;
	memset(Opened,0,sizeof(Opened));
	memset(Closed,0,sizeof(Closed));

	pOpened.pNext=NULL;

	Insert(sx,sy,0,H(sx,sy,gx,gy));

	while(pOpened.pNext!=NULL)
	{
		Data *pF=GetMinF(&pOpened);

		Data pDat=*(pF->pNext);

		x=pDat.x;y=pDat.y;

		Delete(pF);
		Closed[y][x]=true;

		if(x==gx && y==gy)
		{
			cout<<pDat.G;
			return pDat.G;
		}

		for(int i=0;i<4;i++)
		{
			int nx=x+dx[i],ny=y+dy[i];

			if(nx>=0 && nx<w && ny>=0 && ny<h && !Closed[ny][nx] && !map[ny][nx])
			{
				if(Opened[ny][nx])
				{
					Data *pNode=GetNode(nx,ny);

					pNode->G=min(pDat.G+1,pNode->G);
					pNode->F=pNode->G+pNode->H;
				}
				else
				{
					Insert(nx,ny,pDat.G+1,H(nx,ny,gx,gy));
					Opened[ny][nx]=true;
				}
			}

		}
	}
	return -1;
}

int main()
{
	FILE *fp=fopen("my.map","r");

	fread(&w,sizeof(w),1,fp);
	fread(&h,sizeof(h),1,fp);

	for(y=0;y<h;y++)
	{
		for(x=0;x<w;x++)
		{
			fread(&map[y][x],sizeof(int),1,fp);
			cout<<map[y][x]<<" ";
		}
		cout<<endl;
	}

	fclose(fp);

	cout<<AStar(3,3,6,3);
	cout<<"map"<<map[4][3]<<endl;

	return 0;
}