关于A*寻路算法的实现

关于A*寻路算法的总结

具体的原理简介可以参考以下帖子，该帖子讲的十分清楚
原文连接
由于原文的代码已经失效，所以自己动手写了cpp代码，带有详细注释
代码结构如下

具体的程序代码如下：
star.h头文件
简要说明：star是一个表示某个结点的类

#ifndef STAR_H
#define STAR_H

class star//每个结点
{
public:
    star(int x, int y, int G, int H);
    int x;//坐标点范围0 - m-1
    int y;//坐标点范围0 - n-1
    star* parent;//指向parent的指针
    int F, G, H;//F = G + H;
    void update();//更新F的值
};

#endif // STAR_H

star.cpp实现文件

#include "star.h"

star::star(int _x, int _y, int _G, int _H)
{
    x = _x;
    y = _y;
    G = _G;
    H = _H;
    F = G + H;
    parent = nullptr;
}

void star::update()
{
    F = G + H;
}

maze.h头文件
maze是一个具体的迷宫类，其slove成员函数用到了A*算法，是本文的重点

#ifndef MAZE_H
#define MAZE_H

#include <iostream>
#include <vector>
#include <set>
#include <unordered_map>
#include <algorithm>
#include "star.h"

using namespace std;

struct cmp
{
    bool operator()(star* s1, star* s2)
    {
        //比较时候整体上采用按照star结点F值进行比较，如果相同，则按照x和y比较
        if (s1->F == s2->F)
            return true;
        return s1->F < s2->F;
    }
};

class maze
{
public:
    maze(vector<vector<int>> _map, int _begin_x, int _begin_y, int _end_x, int _end_y);
    ~maze();
    void slove();
private:
    vector<vector<int>> map;
    //m*n矩阵，为1代表有障碍，不可走，不可跨越，为0代表可以自由通行

    //起点和终点的x、y坐标
    int begin_x;
    int begin_y;
    int end_x;
    int end_y;

    int n;
    //表示一行有多少个元素,主要是用于find_set中把x和y两
    //个坐标统一为一个坐标 x * n + y ,其中n= map[0].size();

    //用set来存放
    set<star*, cmp> open_set;
    set<star*, cmp> close_set;
    //find_set用来查询某个坐标点是否在open_set或者close_set中，
    //如果在open_set中为1，在close_set中为0，迭代器则是相应map中的迭代器
    unordered_map<int, pair<set<star*>::iterator, int>> find_set;

};


#endif // MAZE_H

maze.cpp
本文的重点在于maze的solve()成员函数

#include "maze.h"

maze::maze(vector<vector<int>> _map, int _begin_x, int _begin_y, int _end_x, int _end_y)
{
    map = _map;
    begin_x = _begin_x;
    begin_y = _begin_y;
    end_x = _end_x;
    end_y = _end_y;
    n = map[0].size();
}

maze::~maze()
{
    for (auto i : open_set)
    {
        delete(i);//释放动态生成的 star 结点
    }
    for (auto i : close_set)
    {
        delete(i);
    }
}

void maze::slove()
{
    //求解迷宫的主要方法:A*算法
    vector<int> dx{-1, 0, 1, 0};
    vector<int> dy{0, 1, 0, -1};

	//是否找到的标志位
    bool has_found_end = false;

    //begin_star是 起点 对应的 star
    star* begin_star = new star(begin_x, begin_y, 0, abs(end_x - begin_x) + abs(end_y - begin_y));

    auto p = open_set.insert(begin_star);
    //返回值为pair<set<int>::iterator, bool>
    //迭代器表示该元素的位置
    find_set[begin_x * n + begin_y] = {p.first, 1};
    //1 表示在 open_set中
    while(open_set.size() && (!has_found_end))
    {

        auto cur = open_set.begin();
        //获取open_set中最小的元素，cur是其迭代器
        
        int cur_x = (*cur)->x, cur_y = (*cur)->y;
        for (int i = 0; i < 4; i ++)
        {
            //轮流遍历周围点
            int next_x = cur_x + dx[i], next_y = cur_y + dy[i];
            if (next_x >= 0 && next_x < map.size() && next_y >= 0 && next_y < map[0].size() && map[next_x][next_y] == 0)
            {
                //进入此处说明周围点(next_x, next_y)有效
                if (find_set.count(next_x * n + next_y))
                {
                    //如果在open_set或者close_set中
                    if (find_set[next_x * n + next_y].second == 1)//说明在open_set里面
                    {
                        //如果在open_set里面，需要做比较判断原来的F和当前的F
                        auto it = find_set[next_x * n + next_y].first;//获取其迭代器
                        if ((*it)->G > (*cur)->G + 1)
                        {
                            //如果改点的G值相比于当前路径还要大的话，就将改变其路径为当前的
                            //具体需要改变其parent,和G,(需要删除在插入),以及在find_set中重新更新
                            star* change = *it;//把需要改变的节点的star的指针提取出来
                            open_set.erase(it);//从open_set中删除
                            change->G = (*cur)->G + 1;
                            change->update();//由于其G发生改变，需要进行update更新其F值
                            change->parent = *cur;//改变其parent指向为当前
                            auto val = open_set.insert(change);//返回一个pair<iterator, bool>
                            find_set[change->x * n + change->y].first = val.first;//更新find_set
                        }
                        continue;
                    }
                    else continue;//如果在close_set里面，就直接忽略
                }

                //进入此处表明（next_x, next_y）还没有被遍历过,首先判断是否是终点
                if (next_x == end_x && next_y == end_y)
                {
                    //插入到open_set中，然后跳出循环
                    star* end = new star(end_x, end_y, (*cur)->G + 1, 0);
                    end->parent = (*cur);
                    auto val = open_set.insert(end);
                    find_set[end_x*n + end_y] = {val.first, 1};//将其插入到find_set中
                    has_found_end = true;
                    break;//此时跳出for循环
                }

                //此时表明不是终点，需要将其插入到open_set中,同时要在find_set中也要添加这一项
                //创建新项startmp
                star* startmp = new star(next_x, next_y, (*cur)->G + 1, abs(next_x - end_x) + abs(next_y - end_y));
                startmp->parent = *cur;
                auto val = open_set.insert(startmp);
                if (!val.second) cout << "插入失败" << endl;
                find_set[next_x*n + next_y] = {val.first, 1};

            }

        }
        //此时for循环已经循环结束，需要将cur指向的star数据移动到close_set中，并更新find_set
        star* close_star = *cur;
        open_set.erase(cur);//将其从open_set中拿走
        auto val = close_set.insert(close_star);
        find_set[close_star->x * n + close_star->y] = {val.first, 0};

    }

    //此时表明已经找到数据或者是全部遍历但是仍找不到数据
    //如果找不到对应路径
    if (!has_found_end)
    {
        cout << "找不到对应的路径" << endl;
        return;
    }
    //这里表明找到了对应路径
    vector<vector<int>> path;
    cout << "找到了相应的路径" << endl;
    star* ans = *(find_set[end_x * n + end_y].first);
    while (ans)
    {
        path.push_back({ans->x, ans->y});
        ans = ans->parent;
    }
    for (int i = path.size() - 1; i >= 0; i --)
        cout << "(" << path[i][0] << "," << path[i][1] << ")" << endl;

    return;

}

测试文件如下main.cpp

#include "maze.h"

int main()
{
    vector<vector<int>> maps{
        {0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,1,1,1,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,1,1,0,0,0,0,1,0,0,1,1,1,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,1,0,1,0,0,0,0,0,0,1,0},
        {0,1,0,0,1,0,0,0,1,0,1,0,1,1,0,0,1,0,1,0,0,0,0,0,1,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,1,1,0,1,0,0,0,0,1,0,0,0},
        {0,1,1,1,1,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,0,1,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,1,0,0,1,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,0,1,0,1,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,1,0,0,1,0,0,0,1,1,1,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,0,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,0,0,0,0}
    };

	//构建一个迷宫类对象
    maze m(maps, 0, 0, 9, 17);
    m.slove();
    return 0;
}