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(0), H(0), F(0), 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[20][30] = {
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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,
    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
};

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

    POINT pt_start = {1, 1};
    POINT pt_target = {10, 16};
    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[8][3] = { // x坐标相对位置，y坐标相对位置，移动耗费
            {-1, -1, 14}, // 左上
            {0 , -1, 10}, // 正上
            {1 , -1, 14}, // 右上
            {-1, 0 , 10}, // 正左
            {1 , 0 , 10}, // 正右
            {-1, 1 , 14}, // 左下
            {0 , 1 , 10}, // 正下
            {1 , 1 , 14}  // 右下
        };
        for(int i=0; i<8; ++i) {
            POINT pt;
            pt.X = pNode->pt.X + ptMask[i][0];
            pt.Y = pNode->pt.Y + ptMask[i][1];

            if(pt.X > 0 && pt.X < 30 && pt.Y >0  && pt.Y < 20) {
                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][2];
                        pChildNode->H = 10 * (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][2];
                        if(tG < pChildNode->G) {
                            pChildNode->pParent = pNode;
                            pChildNode->G = tG;
                            pChildNode->H = 10 * (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 0;
}