寻路算法A-Star(A*)的C++实现

前言

  在游戏中，一个单位从一个地点移动到另一个地点总是复杂的，需要考虑的因素有很多，不过今天只实现最简单的寻路算法。
  地图的组织往往是一个图的数据结构，例如地形贴图，单通道的值会直接储存地形的高度信息。
  我们希望通过寻路算法(Pathfinding Algorithm)，找到两点之间的最短路径。
  作为一个地图，两点之间的路径必然是有很多的，最笨的方法是将所有路径都检测出来，取其长度最小的路径。
  更好的算法还有迪杰斯特拉(Dijkstra)最短路径算法，以及BFS(Breadth First Search)广度优先算法，都能找到最短路径。
  A-Star算法采用了一种“启发式”的方法，会比这些寻路算法更快。

基本原理

  首先，这个算法包含一张基础的地图，包含以下的元素：
  • 一张二维网格组成的地图
  • 绿色的起始点
  • 红色的终结点
  • 灰色的障碍
      我们需要找到一个从绿色起始点到达红色终结点的路径。寻路算法根据需求不同，走法并不统一，例如行走方向是8方向还是4方向（“米”字还是“十”字），会不会像象棋那样被“别住”马腿（例如2行4列的方格是否能直接斜走到1行5列的方格中）。
    在此声明：文章默认实现8方向，无阻塞的寻路。如果有需求，也很容易改变。
      在A*算法中，我们不会记录路径；实现方法是：每个节点（方格）都会记住走到自己方格的上一个方格是谁；假如我们的算法走到终点，就代表我们成功了，我们只需要不断找上一个方格，就能还原出这一个最短路径。

  每一个方格还有其他属性。首先，是当前节点的行列坐标。其次，是三个变量，我们命名为F、G、H。
  其中G的含义为：从原点出发，沿着路径走到当前节点所需要的“代价”。
  从一个格子到另一个格子的“花销”往往是不同的，比如我们做了一个2D RPG游戏，同样是一格子的距离，假如路面有“泥泞”属性，就会导致时间花销变大；假如地面有“坑坑洼洼”属性，就会导致耐久变低等等，这些“花销”可以通过一些评价方法（会直接规定）统一为一种“花销”指标，而这个指标可看做路径的权值。
  我们的实现不用那么复杂，先简单定义一种开销：长度开销，从一个格子水平或垂直到达另一个格子花销为10，而斜向到达花销为14（近似√(10+10)=14.14）。
  H的含义为：从当前节点出发，大致到达终点的“开销”。
  这个变量H就是体现A*算法“启发式”的地方，这个变量H的“开销”并非是精准的，我们可以用一些合理的方法来测算它，最简单的方法是比较直接距离，例如用沟谷定理直接算欧式距离，不过这样会出小数，而我们的“开销”暂时定义为整形，因此采用“曼哈顿距离”（也称街道距离，就是算两者横纵坐标绝对值之和）。
  F是用来评估路径的变量，能帮助我们最快接近最短路径，最简单的方式：F=G+H，这样一来，F的含义就是：从起始点出发走到当前的花费，加上预计到达终点的花费，也就是预计的总花费。或许F变量可以采用一些加权平均的计算方式，有兴趣可以试试。
  现在举个例子，坐标(4, 3)（非数组方式，而是坐标系方式，3行4列或x=4,y=3），起始点右侧的格子。从起始点只需要一步，因此G=10，曼哈顿距离到达终点H=30，因此F=40。
  根据上方的定义，我们写出了格子的结构体：

struct Node {
    int x;
    int y;

    int G;
    int H;
    int F;
    Node* prev = nullptr;//路径的前一个格子

    bool visiable;//可访问的
};

  接下来再用到两个列表，openlist与closelist；openlist代表已知但未探索过的节点，closelist代表探索完成的节点。
  可以想象为：我们站在一个格子中，我们的视线能看透周围8个格子，每看到新的格子，便加入到openlist中，每从openlist中探索（走过）完一个格子，便将其从openlist中去除，并加入到closelist中。
  如下图，走过的格子在closelist中(淡蓝色)，未走过，但已知的格子在openlist中(绿色)：

算法步骤

伪代码：

起点G=0，H=曼哈顿距离，F=G+H
其他格子暂时将F和G设为无限大
起点加入openlist
loop(openlist不为空){
    找到openlist中F值最小的的格子，将其设为当前处理的格子
    if 这个格子就是end格子:
        return true

    计算当前格子周围8个格子（如果是超出边界，或者是障碍物，或者已经探索过（处于closelist中），则不予处理，）：
        if (当前格子的G+当前格子到达周围格子的开销(10或14)) < 周围格子本身的G，则更新周围格子的G和F(因为路径更短，H不会变)
        if 这个周围格子不在openlist，也不再closelist中，则加入openlist

    将当前格子取出openlist并加入closelist中
}
return false(能找到的都找遍了，说明找不到路径)

思路大致清晰，接下来是代码的编写

代码实现

  由于涉及可视化比较麻烦，因此采用打印到控制台输出。
  在这个程序中，我希望将地图信息以字符数组的方式传入，程序应该告诉我们是否能找到路径，并将找到的最短路径打印出来。

//以这种方式
int main() {
//地图可视化参照上方图片
//O代表可行路径
//X代表障碍物
//S代表起点
//E代表终点
    std::vector<std::vector<char>> map = {
        {'O', 'O','O','O','O','O','O','O'},
        {'O', 'O','O','O','X','O','O','O'},
        {'O', 'O','S','O','X','O','E','O'},
        {'O', 'O','O','O','X','O','O','O'},
        {'O', 'O','O','O','O','O','O','O'},
        {'O', 'O','O','O','O','O','O','O'}
    };

    A_Star problem(map);

    if (problem.solve()) {
        problem.print_map();
    }
}

  我们需要实现的就是A_Star类，针对它的功能，我们对其做如下声明：

class A_Star {
public:
    A_Star(const std::vector<std::vector<char>>& map);
    ~A_Star();
    bool solve();//解决方案
    void print_path();//追溯路径，打印在控制台上
    void print_map();//直接打印寻找的地图

    Node* m_map = nullptr;//地图用一维数组数组存储
    int m_map_height;//高和宽
    int m_map_weight;
    Node* m_start = nullptr;//开始和结束格子
    Node* m_end = nullptr;
}

  对于如果有拷贝需求，拷贝构造函数还是很需要的，按需写即可。
  对于构造方法，希望它能将字符串数组转化为格子结构，并初始化属性。

A_Star::A_Star(const std::vector<std::vector<char>>& map) {
    m_map_height = map.size();
    m_map_weight = map[0].size();

    if (m_map_height == 0 || m_map_weight == 0) {
        throw std::exception("map format error");
    }

    m_map = new Node[m_map_height * m_map_weight];
    for (int i = 0; i < m_map_height; ++i) {
        for (int j = 0; j < m_map_weight; ++j) {
            Node& curr_node = m_map[i*m_map_weight + j];

            curr_node.x = j;
            curr_node.y = i;

            curr_node.G = std::numeric_limits<int>::max();//先全部设置为无限大
            curr_node.F = std::numeric_limits<int>::max();

            switch (map[i][j]) {
            case 'O':
                curr_node.visiable = true;
                break;
            case 'X':
                curr_node.visiable = false;
                break;
            case 'S':
                curr_node.visiable = true;
                m_start = &curr_node;
                break;
            case 'E':
                curr_node.visiable = true;
                m_end = &curr_node;
                break;
            default:
                throw std::exception("illegal identifier exist");
            }
        }
    }

    if (m_start == nullptr) {
        throw std::exception("start point is not exist");
    }
    if (m_end == nullptr) {
        throw std::exception("end point is not exist");
    }

    for (int i = 0; i < m_map_height; ++i) {
        for (int j = 0; j < m_map_weight; ++j) {
            Node& curr_node = m_map[i*m_map_weight + j];
            curr_node.H = (std::abs(curr_node.x - m_end->x) + std::abs(curr_node.y - m_end->y)) * 10;//计算曼哈顿距离
        }
    }
}

  析构方法很简单，我们只动态申请了地图，释放掉就好了。

A_Star::~A_Star() {
    delete[] m_map;
}

  对于solve方法，我希望能根据初始化的数据来找到一条最短路径，如果找到就返回true，否则返回false，并将寻找的路径信息存在Node结构中。
  代码相对较长，但处理周围8个节点中有很多重复代码

bool A_Star::solve() {

    std::vector<Node*> openlist;
    std::vector<Node*> closelist;

    openlist.push_back(m_start);//开始节点加入openlist
    m_start->G = 0;//开始节点的G置零并计算F
    m_start->F = m_start->H;

    while (openlist.size() != 0) {//如果openlist不为空

        Node* curr_node = nullptr;//找到F值最小的节点作为当前处理节点
        for (std::vector<Node*>::iterator node = openlist.begin(); node != openlist.end(); node++) {
            if (curr_node == nullptr) {
                curr_node = *node;
                continue;
            }
            else if ((*node)->F <= curr_node->F) {
                curr_node = *node;
            }
        }


        if (curr_node == m_end) {//如果这个节点刚好是终点，直接返回true退出
            return true;
        }

        bool has_prev_x = curr_node->x - 1 >= 0;
        bool has_next_x = curr_node->x + 1 < m_map_weight;
        bool has_prev_y = curr_node->y - 1 >= 0;
        bool has_next_y = curr_node->y + 1 < m_map_height;

        if (has_prev_x) {//存在左侧
            Node* left = &m_map[curr_node->y * m_map_weight + curr_node->x - 1];
            if (left->visiable && std::find(closelist.begin(), closelist.end(), left) == closelist.end()) {//如果此路径可用并且不在closelist中
                if (curr_node->G + 10 < left->G) {//自身G走10到达left节点比left原来的G小，则给left_up换条路径，并重新计算G和F
                    left->prev = curr_node;
                    left->G = curr_node->G + 10;
                    left->F = left->G + left->H;
                    if (std::find(openlist.begin(), openlist.end(), left) == openlist.end()) {//如果不在openlist中，加入openlist
                        openlist.push_back(left);
                    }
                }
            }
            if (has_prev_y) {//存在左上
                Node* left_up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x - 1];
                if (left_up->visiable && std::find(closelist.begin(), closelist.end(), left_up) == closelist.end()) {
                    if (curr_node->G + 14 < left_up->G) {//自身G走14到达left_up节点比left_up原来的G小，则换条路径
                        left_up->prev = curr_node;
                        left_up->G = curr_node->G + 14;
                        left_up->F = left_up->G + left_up->H;
                        if (std::find(openlist.begin(), openlist.end(), left_up) == openlist.end()) {//如果左侧节点不再openlist中，则添加进去
                            openlist.push_back(left_up);
                        }
                    }
                }
            }
            if (has_next_y) {//存在左下
                Node* left_down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x - 1];
                if (left_down->visiable && std::find(closelist.begin(), closelist.end(), left_down) == closelist.end()) {
                    if (curr_node->G + 14 < left_down->G) {//自身G走14到达left_down节点比left_down原来的G小，则换条路径
                        left_down->prev = curr_node;
                        left_down->G = curr_node->G + 14;
                        left_down->F = left_down->G + left_down->H;
                        if (std::find(openlist.begin(), openlist.end(), left_down) == openlist.end()) {
                            openlist.push_back(left_down);
                        }
                    }
                }
            }
        }
        if (has_next_x) {//存在右侧
            Node* right = &m_map[curr_node->y * m_map_weight + curr_node->x + 1];
            if (right->visiable && std::find(closelist.begin(), closelist.end(), right) == closelist.end()) {
                if (curr_node->G + 10 < right->G) {
                    right->prev = curr_node;
                    right->G = curr_node->G + 10;
                    right->F = right->G + right->H;
                    if (std::find(openlist.begin(), openlist.end(), right) == openlist.end()) {
                        openlist.push_back(right);
                    }
                }
            }
            if (has_prev_y) {//存在右上
                Node* right_up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x + 1];
                if (right_up->visiable && std::find(closelist.begin(), closelist.end(), right_up) == closelist.end()) {
                    if (curr_node->G + 14 < right_up->G) {
                        right_up->prev = curr_node;
                        right_up->G = curr_node->G + 14;
                        right_up->F = right_up->G + right_up->H;
                        if (std::find(openlist.begin(), openlist.end(), right_up) == openlist.end()) {
                            openlist.push_back(right_up);
                        }
                    }
                }
            }
            if (has_next_y) {//存在右下
                Node* right_down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x + 1];
                if (right_down->visiable && std::find(closelist.begin(), closelist.end(), right_down) == closelist.end()) {
                    if (curr_node->G + 14 < right_down->G) {
                        right_down->prev = curr_node;
                        right_down->G = curr_node->G + 14;
                        right_down->F = right_down->G + right_down->H;
                        if (std::find(openlist.begin(), openlist.end(), right_down) == openlist.end()) {
                            openlist.push_back(right_down);
                        }
                    }
                }
            }
        }
        if (has_prev_y) {//存在上侧
            Node* up = &m_map[(curr_node->y - 1)*m_map_weight + curr_node->x];
            if (up->visiable && std::find(closelist.begin(), closelist.end(), up) == closelist.end()) {
                if (curr_node->G + 10 < up->G) {
                    up->prev = curr_node;
                    up->G = curr_node->G + 10;
                    up->F = up->G + up->H;
                    if (std::find(openlist.begin(), openlist.end(), up) == openlist.end()) {
                        openlist.push_back(up);
                    }
                }
            }
        }
        if (has_next_y) {//存在下侧
            Node* down = &m_map[(curr_node->y + 1)*m_map_weight + curr_node->x];
            if (down->visiable && std::find(closelist.begin(), closelist.end(), down) == closelist.end()) {
                if (curr_node->G + 10 < down->G) {
                    down->prev = curr_node;
                    down->G = curr_node->G + 10;
                    down->F = down->G + down->H;
                    if (std::find(openlist.begin(), openlist.end(), down) == openlist.end()) {
                        openlist.push_back(down);
                    }
                }
            }
        }
        openlist.erase(std::find(openlist.begin(), openlist.end(), curr_node));//把当前节点从openlist中拿出来

        closelist.push_back(curr_node);//放到closelist中
    }

    return false;//没找到路径，直接返回
}

  剩下的代码就是用来可视化的：

void A_Star::print_path() {//从终点往回追溯路径并打印
    Node* curr_node = m_end;
    while (curr_node != nullptr) {
        std::cout << "(" << curr_node->x << ", " << curr_node->y << ")" << std::endl;
        curr_node = curr_node->prev;
    }
}

void A_Star::print_map() {
    char* map = new char[m_map_height * m_map_weight];//还原输入地图的标记点
    for (int i = 0; i < m_map_height; ++i) {
        for (int j = 0; j < m_map_weight; ++j) {
            if (m_map[i*m_map_weight + j].visiable)
                map[i*m_map_weight + j] = 'O';
            else
                map[i*m_map_weight + j] = 'X';
        }
    }

    map[m_start->y*m_map_weight + m_start->x] = 'S';
    map[m_end->y*m_map_weight + m_end->x] = 'E';

    Node* curr_node = m_end->prev;//将除了起点和终点的路径标为'*'
    while (curr_node->prev != nullptr) {
        map[curr_node->y * m_map_weight + curr_node->x] = '*';
        curr_node = curr_node->prev;
    }

    for (int i = 0; i < m_map_height; ++i) {//打印输出到控制台
        for (int j = 0; j < m_map_weight; ++j) {
            std::cout << map[i*m_map_weight + j] << " ";
        }
        std::cout << std::endl;
    }

    delete[] map;
}

测试

  按照上面一开始放出的代码，输出：

O O O O * O O O
O O O * X * O O
O O S O X O E O
O O O O X O O O
O O O O O O O O
O O O O O O O O

  和图中基本一致，还可以试试别的：

std::vector<std::vector<char>> map = {
    {'O', 'X','X','X','X','O','O','O'},
    {'O', 'X','O','O','X','O','O','O'},
    {'O', 'O','S','O','X','O','E','X'},
    {'O', 'O','O','O','X','X','X','O'},
    {'O', 'O','O','X','O','X','O','O'},
    {'O', 'O','O','O','O','O','O','O'}
};

outout:
O X X X X O O O
O X O O X O O O
O O S O X O E X
O O O * X X X *
O O O X * X * O
O O O O O * O O

  这个略有不同，我们代码实现的是斜向完全无阻塞，可视化网站实现的是单边遮挡无阻塞，双边遮挡阻塞，因此略有不同，不过研究明白算法，更改还是很容易的。

脑洞

  最基础的A*寻路算法就这样实现了，可以看出算法的灵活性，在很多地方都可以加以定制。
  例如，我们需要移动的角色有“闪烁技能”，能瞬移一面墙的距离，那么我们的已知范围就可以从周围8格扩大到24格，同时为了防止人物肆意妄为的“闪烁”，可以针对H变量动手，使得闪烁的代价增大，致使F变量增大，在openlist寻找最小F的过程中，就不会优先选择闪烁。
  除此之外，前面提到的不同路径的不同花销也是可以做一些小动作，使得AI能根据地形选择不同的移动方式。
  期待早日能应用到实践当中！

引用

A*算法理解及代码实现 思路参考自这里，python代码以及pygame可视化
PathFinding.js一个JS库的可视化演示，里面还包括很多其他寻路算法的展示

©著作权归作者所有,转载或内容合作请联系作者
  • 人面猴
    序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
    沈念sama阅读 202,607评论 5赞 476
  • 死咒
    序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
    沈念sama阅读 85,047评论 2赞 379
  • 救了他两次的神仙让他今天三更去死
    文/潘晓璐 我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
    开封第一讲书人阅读 149,496评论 0赞 335
  • 道士缉凶录：失踪的卖姜人
    文/不坏的土叔 我叫张陵，是天一观的道长。 经常有香客问我，道长，这世上最难降的妖魔是什么？ 我笑而不...
    开封第一讲书人阅读 54,405评论 1赞 273
  • 港岛之恋（遗憾婚礼）
    正文 为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
    茶点故事阅读 63,400评论 5赞 364
  • 恶毒庶女顶嫁案：这布局不是一般人想出来的
    文/花漫 我一把揭开白布。 她就那样静静地躺着，像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上，一...
    开封第一讲书人阅读 48,479评论 1赞 281
  • 城市分裂传说
    那天，我揣着相机与录音，去河边找鬼。 笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。 我是一名探鬼主播，决...
    沈念sama阅读 37,883评论 3赞 395
  • 双鸳鸯连环套：你想象不到人心有多黑
    文/苍兰香墨 我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
    开封第一讲书人阅读 36,535评论 0赞 256
  • 万荣杀人案实录
    序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
    沈念sama阅读 40,743评论 1赞 295
  • 护林员之死
    正文 独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 35,544评论 2赞 319
  • 白月光启示录
    正文 我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 37,612评论 1赞 329
  • 活死人
    序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
    沈念sama阅读 33,309评论 4赞 318
  • 日本核电站爆炸内幕
    正文 年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
    茶点故事阅读 38,881评论 3赞 306
  • 男人毒药：我在死后第九天来索命
    文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 29,891评论 0赞 19
  • 一桩弑父案，背后竟有这般阴谋
    文/苍兰香墨 我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 31,136评论 1赞 259
  • 情欲美人皮
    我被黑心中介骗来泰国打工， 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。 一个月前我还...
    沈念sama阅读 42,783评论 2赞 349
  • 代替公主和亲
    正文 我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
    茶点故事阅读 42,316评论 2赞 342

推荐阅读更多精彩内容