A*算法的实现

  1 #include<cstdio>
  2 #include<cstdlib>
  3 #include<cstring>
  4 #include<iostream>
  5 #include<list>
  6 #include<string>
  7 #include<ctime>
  8 #include <algorithm>
  9 #include"jsoncpp/json.h"
 10 using namespace std;
 11 const int n=5;
 12 const int dx[4]={-1,0,1,0};     //up,right,down,left
 13 const int dy[4]={0,1,0,-1};
 14 int invalid[n][n]={
 15 0,1,0,1,0,
 16 0,0,0,1,0,
 17 0,0,0,1,0,
 18 0,1,0,0,0,
 19 0,1,0,1,0
 20 };
 21 int tx=4,ty=0,tx1=4,ty1=4;
 22 
 23 struct point
 24 {
 25     int f,g,h;
 26     int x,y;
 27     point(int _x=0,int _y=0)
 28     {
 29         x=_x;
 30         y=_y;
 31     }
 32 };
 33 
 34 list<point> CloseList;
 35 list<point> OpenList;
 36 point father[100][100];    //记录路径 
 37 
 38 
 39 bool My_validDirection1(int x,int y)  //判断当前移动方向的下一格是否合法
 40 {
 41     if (x>=n || y>=n || x<0 || y<0) return false;
 42     if (invalid[x][y]) return false;
 43     return true;
 44 }
 45 
 46 
 47 int As()
 48 {
 49     point start(tx,ty);
 50     point end(tx1,ty1);
 51     point tempStart(0,0);    //当前点 
 52     
 53     OpenList.push_front(start);
 54     while(OpenList.size()!=0)
 55     {
 56         //选出f最小点作为当前点 
 57         list<point>::iterator it_close=OpenList.begin();
 58         list<point>::iterator it=OpenList.begin();
 59         tempStart=*it;
 60         ++it;
 61         for(;it!=OpenList.end();++it)
 62         {
 63             if((*it).f<tempStart.f)
 64             {
 65                 tempStart=*it;
 66                 it_close=it;
 67             }
 68         }
 69         //将当前点加入关闭列表 
 70         OpenList.erase(it_close);
 71         CloseList.push_front(tempStart);
 72         //周围的点进行扩展 
 73         for(int i=0;i<4;++i)
 74         {
 75             int exist=0,close=0;
 76             point p(tempStart.x+dx[i],tempStart.y+dy[i]);
 77             //如果已经存在关闭列表，则不进行操作 
 78             for(it=CloseList.begin();it!=CloseList.end();++it)
 79             {
 80                 if((*it).x==p.x&&(*it).y==p.y)
 81                 {
 82                     close=1;
 83                     break;
 84                 }
 85             }                    
 86             //如果是非法点或者存在于关闭列表，则不操作 
 87             if(My_validDirection1(p.x,p.y)&&!close)
 88             {
 89                 for(it=OpenList.begin();it!=OpenList.end();++it)
 90                 {
 91                     if((*it).x==p.x&&(*it).y==p.y)
 92                     {
 93                         exist=1;
 94                         break;
 95                     }
 96                 }        
 97                 //如果存在于开启列表，则更新fg        
 98                 if(exist)
 99                 {
100                     int g=tempStart.g+1;
101                     if(g>p.g)
102                     {
103                         tempStart=point(father[tempStart.x][tempStart.y].x,father[tempStart.x][tempStart.y].y);
104                         p.g=abs(p.x-tempStart.x)+abs(p.y-tempStart.y);
105                         p.f=p.g+p.h;
106                     }
107                 }
108                 //否则加入开启列表，计算fgh 
109                 else
110                 {
111                     p.g=abs(p.x-tempStart.x)+abs(p.y-tempStart.y);
112                     p.h=abs(p.x-tx1)+abs(p.y-ty1);
113                     p.f=p.g+p.h;
114                     OpenList.push_front(p);
115                     father[p.x][p.y]=tempStart;
116                 }
117             }
118         }
119         //查询目标点是否在开启列表内 
120         for(it=OpenList.begin();it!=OpenList.end();++it)
121         {
122             if((*it).x==tx1&&(*it).y==ty1)
123             {
124                 int a=tx1,b=ty1,xx=tx1,yy=ty1;
125                 while(father[xx][yy].x!=tx||father[xx][yy].y!=ty)
126                 {
127                     cout<<xx<<","<<yy<<"<-"    ;            
128                     a=father[xx][yy].x;
129                     b=father[xx][yy].y;
130                     xx=a;yy=b;
131                 }
132                 cout<<xx<<","<<yy<<"<-start";
133                 if(xx==tx)
134                 {
135                     if(yy>ty)
136                     {//r2
137                         return 1;
138                     }
139                     else
140                     {//l0
141                         return 3;
142                     }
143                 }
144                 else
145                 {
146                     if(xx>tx)
147                     {//d1
148                         return 2;
149                     }
150                     else
151                     {//u3
152                         return 0;
153                     }                    
154                 }
155             }
156         }            
157         
158     }
159     return -1;
160 }
161 //0:left,1:down,2:right,3:up
162 
163 int main()
164 {
165 
166     int ans=As();
167 
168 //    cout<<ans<<endl;
169  
170     return 0;
171 }