7个寻路算法 AStar.h 

  1 #ifndef __ASTAR__H
  2 #define __ASTAR__H
  3 
  4 #include "AIDefine.h"
  5 #include "../Common/PriorityQueue.h"
  6 
  7 class AStar
  8 {
  9 private:
 10     AStar();
 11 public:
 12     static bool find(const PointI &size, const PointI &start, const PointI &end, AI_VisitFun visitFun, Path **path = NULL, 
 13         EFindType findType = EFIND_TYPE8, AI_DitanceFun funH = AI_Ditance2, AI_DitanceFun funG = AI_Ditance1)
 14     {
 15         if(size.x <= 0 || size.y <= 0 || !AI_CheckRange(start, size) || !AI_CheckRange(end, size) ||
 16             !visitFun(start.x, start.y), !visitFun(end.x, end.y))
 17         {
 18             if(NULL != path){*path = new Path(false, 0.0f);}
 19             return false;
 20         }
 21         if(start.x == end.x && start.y == end.y)
 22         {
 23             if(NULL != path){*path = new Path(true, 0.0f);}
 24             return true;
 25         }
 26 
 27         PriorityQueue<Info*, InfoCmp> visit;
 28         Info *nodeArr = new Info[size.x * size.y];
 29         Info *cur, *next;
 30         PointI pos;
 31 
 32         cur = &nodeArr[size.x * start.y + start.x];
 33         cur->x = start.x; cur->y = start.y; cur->g = 0;
 34         cur->h = funH(cur->x, cur->y, end.x, end.y); cur->f = cur->g + cur->h;
 35         visit.push(cur);
 36         float cost_g, cost_h;
 37         while(!visit.empty())
 38         {
 39             cur = visit.top(); visit.pop(); cur->isopen = false; cur->closed = true;
 40             if(cur->x == end.x && cur->y == end.y)
 41             {
 42                 if(NULL != path)
 43                 {
 44                     *path = new Path(true, cur->f);
 45                     while (NULL != cur)
 46                     {
 47                         (*path)->push_front(PointI(cur->x, cur->y));
 48                         cur = cur->parent;
 49                     }
 50                 }
 51                 if(NULL != nodeArr){delete[] nodeArr;}
 52                 return true;
 53             }
 54             for(int i = 0; i < findType; ++i)
 55             {
 56                 pos.x = cur->x + AI_FindHelpPoint[i].x;pos.y = cur->y + AI_FindHelpPoint[i].y;
 57                 if(AI_CheckRange(pos,size) && !nodeArr[size.x * pos.y + pos.x].closed && AI_CheckPass(cur->x, cur->y, pos.x, pos.y, visitFun))
 58                 {
 59                     next = &nodeArr[size.x * pos.y + pos.x];
 60                     next->x = pos.x; next->y = pos.y;
 61                     cost_g = funG(cur->x, cur->y, next->x, next->y);
 62                     cost_h = funH(next->x, next->y, end.x, end.y);
 63                     if(next->isopen)
 64                     {
 65                         if(next->f > cur->g + cost_g + cost_h)
 66                         {
 67                             visit.remove(next);
 68                         }else
 69                         {
 70                             continue;
 71                         }
 72                     }
 73                     next->g = cur->g + cost_g; next->h = cost_h;
 74                     next->f = next->g + next->h;
 75                     next->isopen = true;next->parent = cur;visit.push(next);
 76                 }
 77             }
 78         }
 79         if(NULL != path){*path = new Path(false, 0.0f);}
 80         if(NULL != nodeArr){delete[] nodeArr;}
 81         return false;
 82     }
 83 private:
 84     struct Info
 85     {
 86         Info()
 87         {
 88             x = y = 0;
 89             g = h = f = 0.0f;
 90             isopen = closed = false;
 91             parent = NULL;
 92         }
 93         int x,y;
 94         float g,h,f;
 95         bool isopen,closed;
 96         Info *parent;
 97     };
 98     struct InfoCmp
 99     {
100         bool operator () (const Info *p1, const Info *p2)
101         {
102             return p1->f < p2->f;
103         }
104     };
105 };
106 
107 #endif

 

posted @ 2013-11-23 01:26  liusijian  阅读(431)  评论(0)    收藏  举报