A*算法

#include<iostream>#include<cstdio>#include<queue>#include<cstring>#include<stdlib.h>#include<math.h>using namespace std;struct load_list{int x;int y;load_list(int x,int y){this->x=x;this->y=y;}load_list *next;};struct mynode{int x;int  y;double h;double g;double f;int pre_x;int pre_y;mynode *next;mynode(int x,int y){this->x=x;this->y=y;};mynode(){};bool operator==(mynode& other){if(x==other.x&&y==other.y)return 1;else return 0;};mynode& operator=(mynode &other){this->x=other.x;this->y=other.y;this->h=other.h;this->g=other.g;this->f=other.f;this->pre_x=other.pre_x;this->pre_y=other.pre_y;this->next=NULL;return *this;}};//--------------------------//整个图为5*5的矩阵const int x_len=5;const int y_len=5;load_list *load=new load_list(-1,-1);mynode *open=new mynode;mynode *Close=new mynode;mynode *s=new mynode(0,0);mynode *e=new mynode(4,4);int can_visit[5][5];//--------------------------int len;bool isvoild(int x,int y){if(x>=0&&x<x_len&&y>=0&&y<=y_len&&can_visit[x][y]==0)return 1;else return 0;}mynode * IsInOpen(mynode *tmp){mynode *p=open->next;while(p){if(*p==*tmp)return p;else p=p->next;}return NULL;}mynode * IsInClose(mynode *tmp){mynode *p=Close;while(p->next){if(*(p->next)==*tmp)return p;else p=p->next;}return NULL;}void insert_inopen(mynode *t){mynode *tmp=new mynode;*tmp=*t;mynode *p=open;while(p->next){if(p->next->f>tmp->f)break;else p=p->next;}tmp->next=p->next;p->next=tmp;}void insert_inClose(mynode *t){mynode *tmp=new mynode;*tmp=*t;if(Close->next==NULL)Close->next=tmp;else {tmp->next=Close->next;Close->next=tmp;}}int print_load(int e_x,int e_y){//------------------------------------------------------//从后往前找到路径，并利用头插法插入到load_list链表中int x=e_x,y=e_y;load_list *t=new load_list(e->x,e->y);t->next=load->next;load->next=t;mynode *p=Close->next;while(x!=s->x||y!=s->y){while(p->x!=x||p->y!=y)p=p->next;{load_list *t=new load_list(p->x,p->y);t->next=load->next;load->next=t;x=p->pre_x;y=p->pre_y;p=Close->next;}}t=new load_list(s->x,s->y);t->next=load->next;load->next=t;//------------------------------------------------------//输出路径。。。t=load->next;while(t){printf("%d %d\n",t->x,t->y);t=t->next;}return 0;}int main(){open->next=NULL;Close->next=NULL;load->next=NULL;s->h=0;s->g=sqrt((abs(s->x-e->x))*(abs(s->x-e->x))+(abs(s->y-e->y))*(abs(s->y-e->y)));s->f=s->g+s->h;s->next=NULL;insert_inopen(s);can_visit[1][1]=can_visit[1][3]=can_visit[3][1]=can_visit[3][2]=1;    //设置不能走的点while (open->next){if(*(open->next)==*e){print_load(open->next->pre_x,open->next->pre_y);break;}else {//--------------------------------------------------------------------------------//对该点的四个邻点进行匹配//--------------------------------------------------------------------------------int temp_x=open->next->x;int temp_y=open->next->y;double temp_g=open->next->g;double temp_h=open->next->h;mynode *temp=open->next;insert_inClose(temp);open->next=open->next->next;if(isvoild(temp_x-1,temp_y)){mynode *find;mynode *tmp=new mynode;tmp->h=temp_h+1;tmp->g=sqrt((abs(temp_x-1-e->x))*(abs(temp_x-1-e->x))+(abs(temp_y-e->y))*(abs(temp_y-e->y)));tmp->next=NULL;tmp->x=temp_x-1;tmp->y=temp_y;tmp->f=tmp->g+tmp->h;tmp->pre_x=temp_x;tmp->pre_y=temp_y;if((find=IsInOpen(tmp))!=NULL){if(find->f>tmp->f){find->f=temp->f;find->pre_x=tmp->pre_x;find->pre_y=tmp->pre_y;}}if((find=IsInClose(tmp))!=NULL&&find->next->f>tmp->f){mynode *t=find->next;find->next=find->next->next;t->pre_x=tmp->pre_x;t->pre_y=tmp->pre_y;t->f=tmp->f;insert_inopen(t);}if((find=IsInOpen(tmp))==NULL&&(find=IsInClose(tmp))==NULL)insert_inopen(tmp);}if(isvoild(temp_x+1,temp_y)){mynode *find;mynode *tmp=new mynode;tmp->h=temp_h+1;tmp->g=sqrt((abs(temp_x+1-e->x))*(abs(temp_x+1-e->x))+(abs(temp_y-e->y))*(abs(temp_y-e->y)));tmp->next=NULL;tmp->x=temp_x+1;tmp->y=temp_y;tmp->f=tmp->g+tmp->h;tmp->pre_x=temp_x;tmp->pre_y=temp_y;if((find=IsInOpen(tmp))!=NULL){if(find->f>tmp->f){find->f=temp->f;find->pre_x=tmp->pre_x;find->pre_y=tmp->pre_y;}}if((find=IsInClose(tmp))!=NULL&&find->next->f>tmp->f){mynode *t=find->next;find->next=find->next->next;t->pre_x=tmp->pre_x;t->pre_y=tmp->pre_y;t->f=tmp->f;insert_inopen(t);}if((find=IsInOpen(tmp))==NULL&&(find=IsInClose(tmp))==NULL)insert_inopen(tmp);}if(isvoild(temp_x,temp_y-1)){mynode *find;mynode *tmp=new mynode;tmp->h=temp_h+1;tmp->g=sqrt((abs(temp_x-e->x))*(abs(temp_x-e->x))+(abs(temp_y-1-e->y))*(abs(temp_y-1-e->y)));tmp->next=NULL;tmp->x=temp_x;tmp->y=temp_y-1;tmp->f=tmp->g+tmp->h;tmp->pre_x=temp_x;tmp->pre_y=temp_y;if((find=IsInOpen(tmp))!=NULL){if(find->f>tmp->f){find->f=temp->f;find->pre_x=tmp->pre_x;find->pre_y=tmp->pre_y;}}if((find=IsInClose(tmp))!=NULL&&find->next->f>tmp->f){mynode *t=find->next;find->next=find->next->next;t->pre_x=tmp->pre_x;t->pre_y=tmp->pre_y;t->f=tmp->f;insert_inopen(t);}if((find=IsInOpen(tmp))==NULL&&(find=IsInClose(tmp))==NULL)insert_inopen(tmp);}if(isvoild(temp_x,temp_y+1)){mynode *find;mynode *tmp=new mynode;tmp->h=temp_h+1;tmp->g=sqrt((abs(temp_x-e->x))*(abs(temp_x-e->x))+(abs(temp_y+1-e->y))*(abs(temp_y+1-e->y)));tmp->next=NULL;tmp->x=temp_x;tmp->y=temp_y+1;tmp->f=tmp->g+tmp->h;tmp->pre_x=temp_x;tmp->pre_y=temp_y;if((find=IsInOpen(tmp))!=NULL){if(find->f>tmp->f){find->f=temp->f;find->pre_x=tmp->pre_x;find->pre_y=tmp->pre_y;}}if((find=IsInClose(tmp))!=NULL&&find->next->f>tmp->f){mynode *t=find->next;find->next=find->next->next;t->pre_x=tmp->pre_x;t->pre_y=tmp->pre_y;t->f=tmp->f;insert_inopen(t);}if((find=IsInOpen(tmp))==NULL&&(find=IsInClose(tmp))==NULL)insert_inopen(tmp);}//--------------------------------------------------------------------------------//对该点的四个邻点进行匹配//--------------------------------------------------------------------------------}}}