A*算法实现

A*算法实现

最近实现了一下A*算法，感觉蛮好的，尤其是修改地图然后看电脑正确寻路后的那种成就感，有点像小时候蹲在地上，看着一堆蚂蚁搬家，然后故意在他们的路上设置障碍物，然后看蚂蚁不停的探索，然后重新找到新的路线的感觉，真是很有意思。
好！把代码记录在此，便于以后参考。

  1 #include  < iostream >
  2 #include  < string >
  3 #include  < vector >
  4 #include  < list >
  5 #include  < map >
  6 #include  < set >
  7
  8 using   namespace  std;
  9
 10 /**/ /************************************************************************/
 11 /**/ /* A*算法实现，测试A*算法地图      
 1240X10的地图，B代表起始点，E代表终点；                                              
 13空格代表能通过；|代表是墙，不能通过；
 14xx0123456789012345678901234567890123456789xx
 15xx————————————————————xx
 160|                                        |0
 171|                                        |1
 182|                 |                      |2
 193|                   |                      |3
 204|                 |        E             |4
 215|    B            |                      |5
 226|                 |                      |6
 237|                                        |7
 248|                                        |8
 259|                                        |9
 26xx————————————————————xx
 27xx0123456789012345678901234567890123456789xx
 28/************************************************************************/
 29 const   static   int  X  =   40 ;
 30 const   static   int  Y  =   10 ;
 31 // 地图上的情况
 32 enum  E_Map
 33 {
 34    E_River=-2,            //有河流，无法通过，在地图上用 ~ 标出
 35    E_Wall=-1,            //有墙，无法通过，在地图上用 | 标出    
 36    E_Road = 0,            //没有障碍物，能最快的顺利通行，代价为0，在地图上用空格标出    
 37    E_Sand=1,            //是沙地，能通过，但是相对难一些，代价为1，在地图上用 * 标出
 38} ;
 39
 40 struct  Point
 41 {
 42    int x;
 43    int y;
 44    Point(int i=0,int j=0):x(i),y(j){}
 45    bool operator==(const Point & r)
 46    {
 47        return (x==r.x)&&(y==r.y);
 48    }
 49} ;
 50 bool   operator == ( const  Point &  l, const  Point  &  r)
 51 {
 52    return (l.x==r.x)&&(l.y==r.y);
 53}
 54 bool   operator < ( const  Point &  l, const  Point  &  r)
 55 {
 56    if(l.y<r.y)    return true;
 57    else if(l.y>r.y)    return false;
 58    else    return (l.x < r.x);
 59}
 60
 61 // const static int GameMap[Y][X] = {
 62 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 63 //     0,0,-2,-2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 64 //     0,0,0,-2,-2,-2,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
 65 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
 66 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
 67 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
 68 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 69 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 70 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 71 //     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 72 // };
 73
 74 // const static int GameMap[Y][X] = {
 75 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 76 //     0,0,-2,-2,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 77 //     0,0,0,-2,-2,-2,0,0,-1,0,-1,0,0,0,0,0,0,-1,-1,0,0,0,-1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
 78 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,-1,0,0,0,0,0,-1,-1,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
 79 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
 80 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
 81 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 82 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,
 83 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 84 //     0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 85 // };
 86
 87 const   static   int  GameMap[Y][X]  =   {
 88    0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 89    0,0,-2,-2,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 90    0,0,0,-2,-2,-2,0,-1,-1,0,0,0,0,0,0,0,0,-1,-1,0,0,0,-1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,
 91    0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,-1,-1,0,0,0,0,0,-2,-2,-2,0,0,0,0,0,0,
 92    0,0,0,0,0,-1,0,0,-1,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-2,-2,0,0,0,0,0,
 93    0,0,0,0,0,-1,0,0,-1,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,-2,0,0,0,0,0,
 94    0,0,0,0,0,-1,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 95    0,0,0,0,0,-1,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,
 96    0,0,0,0,0,0,-1,0,0,-1,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,-1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
 97    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0
 98} ;
 99
100 // 打印地图
101 void  PrintMap( const  Point  & B, const  Point  & E, const  vector < Point >&  path = vector < Point > ());
102 void  PrintMap( const  Point  & B, const  Point  & E, const  vector < Point >&  path)
103 {
104    int LastMap[Y][X] = {0};
105    for (int y=0;y<Y;++y)
106    {
107        for (int x=0;x<X;++x)
108        {
109            LastMap[y][x] = GameMap[y][x];
110        }
111    }
112    //路径
113    vector<Point>::const_iterator itr = path.begin();
114    for (;itr != path.end();++itr)
115    {
116        LastMap[(*itr).y][(*itr).x] = '&';
117    }
118    //起始和目标
119    LastMap[B.y][B.x] = 'B';
120    LastMap[E.y][E.x] = 'E';
121
122    cout<<"A*寻路路径为："<<endl;
123    cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
124    cout<<"xx————————————————————xx"<<endl;
125    for (int y=0;y<Y;++y)
126    {
127        cout<<y<<"[";
128        for (int x=0;x<X;++x)
129        {
130            if (LastMap[y][x] == E_Road)
131            {
132                cout<<" ";
133            }
134            else if (LastMap[y][x] == E_Wall)
135            {
136                cout<<"|";
137            }
138            else if (LastMap[y][x] == E_River)
139            {
140                cout<<"~";
141            }
142            else if (LastMap[y][x] == E_Sand)
143            {
144                cout<<"*";
145            }
146            else if (LastMap[y][x] == 'B')
147            {
148                cout<<"B";
149            }
150            else if (LastMap[y][x] == 'E')
151            {
152                cout<<"E";
153            }
154            else if (LastMap[y][x] == '&')
155            {
156                cout<<"&";
157            }
158        }
159        cout<<"]"<<y<<endl;
160    }
161    cout<<"xx————————————————————xx"<<endl;
162    cout<<"xx0123456789012345678901234567890123456789xx"<<endl;
163}
164
165 // 计算当前位置与终点位置的Hn
166 double  Hn( const  Point  &  E, const  Point & p)
167 {
168    return abs(E.y-p.y) + abs(E.x-p.x);
169}
170
171 // 计算相邻两个位置(即包括对象线在内的周围8个坐标)的相对Gn
172 double  Gg( const  Point  &  p1, const  Point &  p2)
173 {
174    double d = GameMap[p2.y][p2.x];
175    return ((p1.x-p2.x)!=0&&(p1.y-p2.y)!=0)?(1.5+d):(1.0+d);
176}
177
178 // 探测位置p的下一步(p.x + addx,p.y + addy)的Gn,Hn
179 void  testNext( const  Point  &  E, const  Point & p, const   set < Point >&  closeTbl,
180     map < Point, double >   & mapGn,map < Point, double >   & mapGnTemp,
181     multimap < double ,Point >   & HnPoint, int  addx, int  addy)
182 {
183    int x = p.x + addx;
184    int y = p.y + addy;    
185    if (x>=0 && y>=0 && x<X && y<Y && GameMap[y][x]>=0)
186    {
187        Point t = Point(x,y);
188        if (closeTbl.find(t) != closeTbl.end())
189        {
190            return;
191        }
192        //得到对应本次遍历的Gn
193        double dgn = mapGn[p] + Gg(p,t);
194        mapGnTemp[t] = dgn;
195
196        map<Point,double>::iterator itr = mapGn.find(t);
197        if (itr == mapGn.end() || itr->second > dgn)
198        {
199            mapGn[t] = dgn;
200        }
201        HnPoint.insert(make_pair(Hn(E,t),t));
202    }
203}
204
205 bool  APath( const  Point  &  B, const  Point  &  E,vector < Point >& path)
206 {    
207    //A*算法：Fn = Gn + Hn
208    path.clear();
209    vector<Point> openTbl;
210    openTbl.push_back(B);
211    set<Point>    closeTbl;
212    closeTbl.insert(B);
213    map<Point,double> mapGn;
214    mapGn[B] = 0;
215    while(!openTbl.empty())
216    {
217        Point p = openTbl.back();
218        openTbl.pop_back();
219        if (p == E)
220        {
221            path.push_back(E);
222            break;
223        }
224        //multimap<double,Point> FnPoint;
225        multimap<double,Point> HnPoint;    //当前位置周围所有可选择的位置到终点的Hn
226        map<Point,double> mapGnTemp;    //当前位置周围所有可选择的位置到起点的Gn
227
228        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,-1);        //左上位置
229        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,0);        //左边位置
230        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,-1,1);        //左下位置
231        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,0,-1);        //上面位置
232        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,0,1);            //下面位置
233        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,-1);        //右上位置
234        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,0);            //右边位置
235        testNext(E,p,closeTbl,mapGn,mapGnTemp,HnPoint,1,1);            //右下位置
236        if (HnPoint.empty())
237        {
238            //无路可走了，只能一步一步回退。。。
239
240            //PrintMap(B,E,path);
241            //标记该点已经被探测，使得下次不再被选择
242            closeTbl.insert(p);
243            //回退一步，重新探测之前走过的一个点
244            if (path.empty())
245            {
246                return false;
247            }
248            p = path.back();
249            path.pop_back();
250            closeTbl.erase(p);
251            openTbl.push_back(p);
252            continue;
253        }
254
255        //HnPoint的第一维就是从p点开始到达终点（Hn）最高效的周围坐标点pt
256        multimap<double,Point>::iterator  itr = HnPoint.begin();
257        double hn = itr->first;
258        Point pt = itr->second;
259
260        map<Point,double>::iterator itrFind = mapGn.find(pt);
261        while (itrFind != mapGn.end() && (itrFind->second) < mapGnTemp[pt])
262        {
263            //如果这个不是最优，则优先试探其他的相同Hn的路径
264            ++itr;
265            if (itr != HnPoint.end() )
266            {
267                if (hn < itr->first)
268                {
269                    break;
270                }
271                pt = itr->second;
272                itrFind = mapGn.find(pt);
273            }
274            else
275                break;
276        }
277        //判断pt是否被探测过
278        if (itrFind != mapGn.end() && (itrFind->second) < mapGnTemp[pt])
279        {
280            /**//**************************************************************************************
281            pt已经被探测过，并且之前的Gn更小，说明可以用更小的代价到达pt。
282            所以说明我们之前选择的p点不是最佳选择，首先应该标记p无效，然后回退到之前的坐标重新选择！
283            ****************************************************************************************/
284            //PrintMap(B,E,path);
285            //标记该点已经被探测，使得下次不再被选择
286             closeTbl.insert(p);
287            //回退一步，重新探测之前走过的一个点
288            p = path.back();
289            path.pop_back();
290            closeTbl.erase(p);
291            openTbl.push_back(p);
292            continue;
293        }    
294
295        //pt没有被探测过，或者是最优选项，所以将p加入路径，然后下一步探测pt 
296        openTbl.push_back(pt);
297
298        closeTbl.insert(p);
299        path.push_back(p);
300    }    
301    return !path.empty();
302}
303
304 int  main( int  argc,  char   *  argv[])
305 {
306    const Point B(4,5),E(36,4);
307    vector<Point> path;
308    bool bFind = APath(B,E,path);
309    
310    PrintMap(B,E,path);
311    if (!bFind)
312    {
313        cout<<"++++++找不到可达路径!+++++++++"<<endl;
314    }
315
316    return 0;
317}

 

最上面注释里面画的地图是为了简单演示地图最终的显示效果，其实代码二维数组给出的地图有趣多了。