A* 寻路算法学习

代码小记

 #include <iostream>
 #include <list>

 struct POINT {
     int X;
     int Y;
 };

 // G: 起点到当前点的成本
 // H: 当前点到终点的估算成本
 // F: G,H之和
 struct MapNode {
     int G;
     int H;
     int F;
     bool can_pass;
     POINT pt;
     MapNode* pParent;

     MapNode() : G(), H(), F(), can_pass(false), pParent(NULL) {}

     bool operator==(const MapNode& other) {
         return (other.pt.X == pt.X && other.pt.Y == pt.Y);
     }

     bool operator!=(const MapNode& other) {
         return !(*this == other);
     }
 };

 bool nodeCompare(MapNode* p1, MapNode* p2) {
     return (p1->F <  p2->F);
 }

 int maps[][] = {
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
     ,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
 };

 MapNode* findNode(std::list<MapNode*>& nodes, const POINT& pt) {
     for(std::list<MapNode*>::iterator it = nodes.begin();
             it != nodes.end();
             ++it) {
         if((*it)->pt.X == pt.X && (*it)->pt.Y == pt.Y) {
             return *it;
         }
     }
     return NULL;
 }

 void pathFinding() {
     std::list<MapNode*> all;
     std::list<MapNode*> open;
     std::list<MapNode*> close;

     for(int row=; row<; ++row) {
         for(int col=; col<; ++col) {
             MapNode* pNode = new MapNode;
             pNode->pt.X = row;
             pNode->pt.Y = col;
             pNode->can_pass = (maps[row][col] ==  ? true : false);
             all.push_back(pNode);
         }
     }

     POINT pt_start = {, };
     POINT pt_target = {, };
     MapNode* pStart = findNode(all, pt_start);
     MapNode* pTarget = findNode(all, pt_target);

     open.push_back(pStart);
     while(!open.empty()) {
         open.sort(nodeCompare);
         MapNode *pNode = open.front();
         if (*pNode == *pTarget) {
             break;
         }
         open.pop_front();
         close.push_back(pNode);
         // 查找周围节点并将合适的加入open
         int ptMask[][] = { // x坐标相对位置，y坐标相对位置，移动耗费
             {-, -, }, // 左上
             { , -, }, // 正上
             { , -, }, // 右上
             {-,  , }, // 正左
             { ,  , }, // 正右
             {-,  , }, // 左下
             { ,  , }, // 正下
             { ,  , }  // 右下
         };
         for(int i=; i<; ++i) {
             POINT pt;
             pt.X = pNode->pt.X + ptMask[i][];
             pt.Y = pNode->pt.Y + ptMask[i][];

             if(pt.X >  && pt.X <  && pt.Y >  && pt.Y < ) {
                 MapNode* pChildNode = findNode(all, pt);
                 if(!pChildNode->can_pass || NULL != findNode(close, pt)) {
                     // 如果pNodeLT不可抵达的或者它在 close list 中，忽略它
                 } else {
                     if(NULL == findNode(open, pt)) {
                         // 如果pNodeLT不在 open list 中，把它加入 open list，
                         // 并且把当前方格设置为它的父亲，记录该方格的F,G和H值
                         pChildNode->pParent = pNode;
                         pChildNode->G = pChildNode->pParent->G + ptMask[i][];
                         pChildNode->H =  * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
                         pChildNode->F = pChildNode->G + pChildNode->H;
                         open.push_back(pChildNode);
                     } else {
                         // 如果pNodeLT已经在 open list 中，检查这条路径 ( 即经由
                         // 当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G
                         // 值表示这是更好的路径
                         int tG = pNode->G + ptMask[i][];
                         if(tG < pChildNode->G) {
                             pChildNode->pParent = pNode;
                             pChildNode->G = tG;
                             pChildNode->H =  * (abs(pTarget->pt.X - pt.X) + abs(pTarget->pt.Y - pt.Y));
                             pChildNode->F = pChildNode->G + pChildNode->H;
                         }
                     }
                 }
             }
         }
     }
     if(NULL != pTarget->pParent) {
         MapNode* pNode = pTarget;
         std::cout << "path is : " << std::endl;
         do {
             std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "] ->" << std::endl;
             pNode = pNode->pParent;
         } while(*pNode != *pStart);
         std::cout << "pt[" << pNode->pt.X << ", " << pNode->pt.Y << "]" << std::endl;
     }
 }

 int main() {

     pathFinding();

     system("pause");

     return ;
 }