A*启发式搜索

转自：http://blog.youkuaiyun.com/sxy_cnyali/article/details/50951326

#include<bits/stdc++.h>
#define REP(i,j,k) for (int i=(j);i<=(k);++i)
#define abs(x) ((x)>0?(x):-(x))
using namespace std;

const int maxn=1010,dmax=maxn*maxn,INF=0x3f3f3f3f,d[5][2]={{0,0},{1,0},{0,1},{-1,0},{0,-1}};
int a[maxn][maxn],f[maxn][maxn],n,m,sx,sy,ex,ey;
bool p[maxn][maxn];
struct node{
	int x,y,w;
	node(){
		x=y=w=0;
	}
};
struct node pre[maxn][maxn],s;
int value(node h){ return h.w+abs(h.x-ex)+abs(h.y-ey); }
struct heap{
		struct node q[dmax];
		int t=0;
	public:
		void push(node x){
			q[++t]=x;
			int k=t;
			while (k>1 && q[k].w<q[k/2].w){
				swap(q[k],q[k/2]);
				k/=2;
			}
		}
		void pop(){
			if (t==0) return;
			q[1]=q[t--];
			int k=1;
			while ((k*2<=t && q[k].w>q[k*2].w) || (k*2+1<=t && q[k].w>q[k*2+1].w)){
				int m=k*2;
				if (m+1<=t && q[m].w>q[m+1].w) ++m;
				swap(q[k],q[m]);
				k=m;
			}
		}
		node top(){ return q[1]; }
};
heap q;

int main(){
	int ans=-1;
	scanf("%d%d",&n,&m);
	REP(i,1,n)
		REP(j,1,m)
			scanf("%d",&a[i][j]);
	scanf("%d%d%d%d",&sx,&sy,&ex,&ey);
	if (sx==ex && sy==ey){
		puts("0");
		return 0;
	}
	bool flag=1;
	node tmp;
	tmp.x=sx;
	tmp.y=sy;
	tmp.w=0;
	pre[sx][sy]=tmp;
	q.push(tmp);
	do{
		node h=q.top();
		q.pop();
		p[h.x][h.y]=1;
		REP(i,1,4){
			int x=h.x+d[i][0],
				y=h.y+d[i][1];
			node t;
			t.x=x;
			t.y=y;
			t.w=h.w+1;
			if (a[x][y] || p[x][y] || x<1 || y<1 || x>n || y>m) continue;
			if (f[x][y]==0 || f[x][y]>value(t)){
				f[x][y]=value(t);
				pre[x][y]=t;
				p[x][y]=1;
				if (x==ex && y==ey){
					s=t;
					ans=h.w+1;
					flag=0;
					break;
				}
				q.push(t);
			}
		}
	}while (flag);
	printf("%d\n",ans);
	return 0;
}