A* 寻路算法学习

  1 #include <iostream>
  2 #include <list>
  3 
  4 struct POINT {
  5     int X;
  6     int Y;
  7 };
  8 
  9 // G: 起点到当前点的成本
 10 // H: 当前点到终点的估算成本
 11 // F: G,H之和
 12 struct MapNode {
 13     int G;
 14     int H;
 15     int F;
 16     bool can_pass;
 17     POINT pt;
 18     MapNode* pParent;
 19 
 20     MapNode() : G(0), H(0), F(0), can_pass(false), pParent(NULL) {}
 21 
 22     bool operator==(const MapNode& other) {
 23         return (other.pt.X == pt.X && other.pt.Y == pt.Y);
 24     }
 25 
 26     bool operator!=(const MapNode& other) {
 27         return !(*this == other);
 28     }
 29 };
 30 
 31 bool nodeCompare(MapNode* p1, MapNode* p2) {
 32     return (p1->F <  p2->F);
 33 }
 34 
 35 int maps[20][30] = {
 36     1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,
 37     1,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,1,
 38     1,0,1,0,0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,1,1,1,1,1,0,1,
 39     1,0,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 40     1,0,0,0,1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,1,
 41     1,0,1,1,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 42     1,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1,
 43     1,0,1,0,1,0,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 44     1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,0,1,0,1,
 45     1,0,1,0,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 46     1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1,
 47     1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 48     1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,0,1,0,1,
 49     1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,0,0,0,0,0,0,0,0,0,1,0,1,
 50     1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,0,1,1,1,1,1,1,1,1,1,0,1,
 51     1,0,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,1,0,1,
 52     1,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,1,0,1,
 53     1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,0,1,1,1,1,0,1,1,1,1,1,1,0,1,
 54     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,1,
 55     1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
 56 };
 57 
 58 MapNode* findNode(std::list<MapNode*>& nodes, const POINT& pt) {
 59     for(std::list<MapNode*>::iterator it = nodes.begin();
 60             it != nodes.end();
 61             ++it) {
 62         if((*it)->pt.X == pt.X && (*it)->pt.Y == pt.Y) {
 63             return *it;
 64         }
 65     }
 66     return NULL;
 67 }
 68 
 69 void pathFinding() {
 70     std::list<MapNode*> all;
 71     std::list<MapNode*> open;
 72     std::list<MapNode*> close;
 73 
 74     for(int row=0; row<20; ++row) {
 75         for(int col=0; col<30; ++col) {
 76             MapNode* pNode = new MapNode;
 77             pNode->pt.X = row;
 78             pNode->pt.Y = col;
 79             pNode->can_pass = (maps[row][col] == 0 ? true : false);
 80             all.push_back(pNode);
 81         }
 82     }
 83 
 84     POINT pt_start = {1, 1};
 85     POINT pt_target = {10, 16};
 86     MapNode* pStart = findNode(all, pt_start);
 87     MapNode* pTarget = findNode(all, pt_target);
 88 
 89     open.push_back(pStart);
 90     while(!open.empty()) {
 91         open.sort(nodeCompare);
 92         MapNode *pNode = open.front();
 93         if (*pNode == *pTarget) {
 94             break;
 95         }
 96         open.pop_front();
 97         close.push_back(pNode);
 98         // 查找周围节点并将合适的加入open
 99         int ptMask[8][3] = { // x坐标相对位置，y坐标相对位置，移动耗费
100             {-1, -1, 14}, // 左上
101             {0 , -1, 10}, // 正上
102             {1 , -1, 14}, // 右上
103             {-1, 0 , 10}, // 正左
104             {1 , 0 , 10}, // 正右
105             {-1, 1 , 14}, // 左下
106             {0 , 1 , 10}, // 正下
107             {1 , 1 , 14}  // 右下
108         };
109         for(int i=0; i<8; ++i) {
110             POINT pt;
111             pt.X = pNode->pt.X + ptMask[i][0];
112             pt.Y = pNode->pt.Y + ptMask[i][1];
113 
114             if(pt.X > 0 && pt.X < 30 && pt.Y >0  && pt.Y < 20) {
115                 MapNode* pChildNode = findNode(all, pt);
116                 if(!pChildNode->can_pass || NULL != findNode(close, pt)) {
117                     // 如果pNodeLT不可抵达的或者它在 close list 中，忽略它
118                 } else {
119                     if(NULL == findNode(open, pt)) {
120                         // 如果pNodeLT不在 open list 中，把它加入 open list，
121                         // 并且把当前方格设置为它的父亲，记录该方格的F,G和H值
122                         pChildNode->pParent = pNode;
123                         pChildNode->G = pChildNode->pParent->G + ptMask[i][2];
124                         pChildNode->H = 10 * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
125                         pChildNode->F = pChildNode->G + pChildNode->H;
126                         open.push_back(pChildNode);
127                     } else {
128                         // 如果pNodeLT已经在 open list 中，检查这条路径 ( 即经由
129                         // 当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G
130                         // 值表示这是更好的路径
131                         int tG = pNode->G + ptMask[i][2];
132                         if(tG < pChildNode->G) {
133                             pChildNode->pParent = pNode;
134                             pChildNode->G = tG;
135                             pChildNode->H = 10 * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
136                             pChildNode->F = pChildNode->G + pChildNode->H;
137                         }
138                     }
139                 }
140             }
141         }
142     }
143     if(NULL != pTarget->pParent) {
144         MapNode* pNode = pTarget;
145         std::cout << "path is : " << std::endl;
146         do {
147             std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "] ->" << std::endl;
148             pNode = pNode->pParent;
149         } while(*pNode != *pStart);
150         std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "]" << std::endl;
151     }
152 }
153 
154 int main() {
155 
156     pathFinding();
157 
158     system("pause");
159 
160     return 0;
161 }