astar寻路算法

#include<stdio.h>
#include<queue>
#include<stack>
using namespace std;
#define Maxn 509
#define Maxm 509
#define inf (1<<29)
#define rep(i,j,k,l) for(int i=(j);(k);i+=l)
const int d[8][2]={{1,0},{-1,0},{0,-1},{0,1},{1,-1},{1,1},{-1,1},{-1,-1}};
struct node
{
int f,g,h,father1,father2,data,a,b;
int operator<(node b)const{
return (*this).f>b.f;
}
};
node map[Maxn][Maxm];
priority_queue<node> OL;
int n,m,s1,s2,t1,t2,z1,z2,DebugCounter,DebugOLCur,isOL[Maxn][Maxm],CL[Maxn][Maxm],PrintList[Maxn][Maxm];


void Debug(){printf("%d Debuging %d\n",++DebugCounter,OL.empty());}
void DebugOL(){
stack<node> a;
printf("%d OlDebug Begin!\n",++DebugOLCur); 
while(!OL.empty()){
node tmp=OL.top();OL.pop();
printf("OLDebuging!(%d,%d)\n",tmp.a,tmp.b);
a.push(tmp);
}
while(!a.empty()){
node tmp=a.top();a.pop();
OL.push(tmp);
}//printf(" *******fawd\n");
}
void showOL(){
rep(i,0,i<=n,1)
rep(j,0,j<=m,1)
printf("%d%c",isOL[i][j],j==m?'\n':' ');printf("*");
}
void DT(int s){
for(int i=1;i<=s*5000000;i++);
}
void showf(){
rep(i,1,i<=n,1)
rep(j,1,j<=m,1)
printf("%d%s",map[i][j].f,j==m?"\n":"\t");printf("\n");
}


int abs(int a){
if(a<0) return -a;else return a;
}
void CalcF(int a,int b,int k){
node tmp=map[a][b];
if(!k) return;
else if(k>=1 && k<=4) tmp.g=map[tmp.father1][tmp.father2].g+10;
else if(k>=5 && k<=8) tmp.g=map[tmp.father1][tmp.father2].g+14;//1.4 is near sqrt(2)
tmp.h=(abs(tmp.a-t1)+1+abs(tmp.b-t2))*10;
tmp.f=tmp.g+tmp.h;
map[a][b]=tmp;
}
node CalcNewF(int a,int b,int c,int d,int e){
//Debug();
node tmp=map[a][b];
if(c>=1 && c<=4) tmp.g=map[d][e].g+10;
 else if(c>=5 && c<=8) tmp.g=map[d][e].g+14;
tmp.f=tmp.g+tmp.h;
return tmp;
}
void ChangeOL(node goal,int a,int b){
stack<node> c;
while(1){
node tmp1=OL.top();OL.pop();
if(tmp1.a==a && tmp1.b==b) break;else c.push(tmp1);
}
OL.push(goal);
while(!c.empty()){
node tmp1=c.top();c.pop();
OL.push(tmp1);
}
}
int EmptyOL(){
rep(i,1,i<=n,1)
rep(j,1,j<=m,1)
if(map[i][j].data)
if(!isOL[i][j]) return 0;
return 1;
}
void Print(int t1,int t2,int flag){
if(flag) printf("You are stupid!");
else{
node tmp;
rep(i,1,i<=n,1)
rep(j,1,j<=m,1)
PrintList[i][j]=0;
PrintList[s1][s2]=0;
tmp=map[t1][t2];int cur=0;
while(tmp.father1!=-1 && tmp.father2!=-1){
PrintList[tmp.a][tmp.b]=++cur;//printf("(%d,%d)",tmp.a,tmp.b);DT(1);
tmp=map[tmp.father1][tmp.father2];
}
PrintList[s1][s2]=++cur;
rep(i,1,i<=n,1){
rep(j,1,j<=m,1)
printf("%d\t",PrintList[i][j]);
printf("\n");
}
}
}




int main(){
freopen("astar.in","r",stdin);
/// freopen("astar.out","w",stdout);
scanf("%d %d",&n,&m);
rep(i,0,i<Maxn,1)
rep(j,0,j<Maxm,1) {
isOL[i][j]=1;
map[i][j].data=-1;
CL[i][j]=1;
}
rep(i,1,i<=n,1)
rep(j,1,j<=m,1){
map[i][j].a=i;map[i][j].b=j;isOL[i][j]=0;CL[i][j]=0;
scanf("%d",&map[i][j].data);
}
scanf("%d %d %d %d",&s1,&s2,&t1,&t2);
map[s1][s2].father1=-1;map[s1][s2].father2=-1;
CalcF(s1,s2,0);
OL.push(map[s1][s2]);isOL[s1][s2]=1;int flag=0;
//astar begins
while(!OL.empty() && !isOL[t1][t2]){
// Debug();
// DebugOL();
int tmp,tmp1=inf,a1,b1;
node tmp2=OL.top(); OL.pop();
CL[tmp2.a][tmp2.b]=1;//printf("%d,(%d,%d)\n",map[tmp2.a][tmp2.b].f,tmp2.a,tmp2.b);
rep(k,0,k<8,1){
int zb1=tmp2.a+d[k][0],
zb2=tmp2.b+d[k][1];
if(map[zb1][zb2].data && !CL[zb1][zb2])
if(!isOL[zb1][zb2]){
map[zb1][zb2].father1=tmp2.a;
map[zb1][zb2].father2=tmp2.b;
CalcF(zb1,zb2,k+1);//printf("(%d,%d)->(%d,%d)\n",tmp2.a,tmp2.b,zb1,zb2);
isOL[zb1][zb2]=1;// DebugOL();
flag=isOL[t1][t2];
if(flag) break;
OL.push(map[zb1][zb2]);//DeBug();
}else{
node tmp3=CalcNewF(zb1,zb2,k+1,tmp2.a,tmp2.b);
// Debug();
if(tmp3.f<map[zb1][zb2].f){
tmp3.father1=tmp2.a;
tmp3.father2=tmp2.b;
map[zb1][zb2]=tmp3;
ChangeOL(map[zb1][zb2],zb1,zb2);
}
}
if(flag) break;
}
if(flag) break;
//showf();
}
Print(t1,t2,!isOL[t1][t2]);
return 0;
}