A_Star算法的C++实现（一）

A_Star算法代码初始版：

运行环境：vs2022

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
using namespace std;

struct Point
{
    int x, y;
    Point(int x, int y) :x(x), y(y) {}
    double distance(Point& p) {
        return sqrt((x - p.x) ^ 2 + (y - p.y) ^ 2);
    }
};

//定义节点
struct Node
{
    Point point;
    double g, h, f;
    Node* parent;
    Node(Point point, double g, double h, Node* parent = nullptr) :point(point), g(g),  h(h),f(g+h), parent(parent) {}
};


//自定义Node*的排序规则
struct NodeCompare {
    bool operator()(Node* n1, Node* n2){
        return (n1->f) < (n2->f);//升序排列
    }
};


//A-star
vector<Point>AstarPathPlanning(vector<vector<int>>& gridmap,Point &start,Point&goal) {
    int row = gridmap.size();//地图行,表示地图的宽
    int col = gridmap[0].size();//地图列，表示地图的长
    //定义openlist和closelist
    vector<Node*> openlist;//表示待搜索的节点
    vector<Node*> closelist;//表示已搜索的节点
    openlist.push_back(new Node(start,start.distance(start),start.distance(goal)));
    int count1 = 1;//统计new的次数
    vector<Point> path;
    while (!openlist.empty()) {
    //获取当前搜索节点current，即openlist中f最小的节点
        sort(openlist.begin(), openlist.end(),NodeCompare{});//先对openlist排序，这里要自定义排序规则
        Node* current = *openlist.begin();//openlist.begin()在排序后记为f最小的元素的迭代器位置
        openlist.erase(openlist.begin());//将current对应的元素从openlist中剔除
        closelist.push_back(current);//将current加入到closelist中
        //对当前搜索节点current进行分类讨论
        //1.current是重点，则返回路径，表示找到路径
        if (current->point.x == goal.x && current->point.y == goal.y)
        {
            while (current != nullptr)//利用父节点，从终点向起点回溯最短路径
            {
                path.push_back(current->point);
                current = current->parent;
            }
            reverse(path.begin(), path.end());//路径是反的，反转路径
            //仍然存在一定的问题，需要解决，因为openlist和closelist存储的元素都是new动态分配的内存，需要delete，否则会造成内存的泄露
            int count2 = 0;
            for (auto o : openlist) {
                delete o;
                count2++;
            }
            for (auto c : closelist)
            {
                delete c;
                count2++;
            }
            cout << "new的次数：" << count1 << endl;
            cout << "delete的次数：" << count2 << endl;
            return path;
        }
        //2.current不是重点，需要讨论其邻近的节点。
        int x = current->point.x;
        int y = current->point.y;
        vector<Point> neighbors = {//由current得到8个邻近点的坐标
            {x - 1,y - 1} ,{ x - 1,y } ,{ x - 1,y + 1 },
            { x,y - 1 }, { x,y + 1 },
            { x + 1,y - 1 } ,{ x + 1,y }, { x + 1,y + 1 }
        };
            //遍历所有邻近节点,每一个临近节点必须满足在地图内同时不是障碍物
        for (auto n : neighbors) {
            if ((n.x >= 0 && n.x < row) && (n.y >= 0 && n.y < col) && gridmap[n.x][n.y] == 0) {
            //2.1n在closelist中，表示已经探索过了，此时直接跳过
                bool incloselist = false;
                for (auto c : closelist) {
                    if (c->point.x == n.x && c->point.y == n.y) {
                        incloselist = true;
                        break;//此处的break是指，已经确认了外面for循环的n点是属于closelist中的，所以就没必要继续该遍历了，直接跳出该循环
                    }
                }
                if (incloselist) {
                    continue;//如果incloselist为true，则说明该点在closelist里面，则执行该if里面的continue语句，意味着结束该次外循环，也就是对该n点不进行操作，直接开始查找下一个n点的情况。
                }
                //2.2n是否在openlist中进行讨论
                bool inopenlist = false;
                for (auto o : openlist)
                {
                    if (o->point.x == n.x && o->point.y == n.y)
                    {
                        inopenlist = true;//如果在openlist当中，则对比值，更新代价值和父节点parent
                        double g = current->g + n.distance(current->point);//邻近节点n到起点的距离=当前搜索节点current到起点的距离+当前搜索节点current到邻近节点n的距离
                        double h = n.distance(goal);//邻近节点n到重点的估计距离代价
                        double f = g + h;
                        if (f < (o->f)) {
                            o->f = f;
                            o->parent = current;
                        }
                        break;//结束该循环，因为已经找到openlist中某一点就是当前neighbor中的该点，再继续循环已经没有意义了
                    }
                }
                if (!inopenlist) {//2.2.2 n不在openlist中，对比f值，计算代价值，添加到openlist中，下次备选
                    double g = current->g + n.distance(current->point);//邻近节点n到起点的距离=当前搜索节点current到起点的距离+当前搜索节点current到邻近节点n的距离
                    double h = n.distance(goal);//邻近节点n到重点的估计距离代价
                    double f = g + h;
                    openlist.push_back(new Node(n, g, h, current));
                    count1++;
                }
            }; 
        }
    }
    //搜索完成没有路径，表示路径规划失败，此时返回空路径
    int count2 = 0;
    for (auto o : openlist) {
        delete o;
        count2++;
    }
    for (auto c : closelist)
    {
        delete c;
        count2++;
    }
    cout << "new的次数：" << count1 << endl;
    cout << "delete的次数：" << count2 << endl;
    return path;
}


int main()
{
    vector<vector<int>> gridmap = {
        {0,0,1,0,0},
        {0,1,0,0,0},
        {0,0,0,1,0},
        {0,0,0,0,1},
        {1,0,0,0,0},
    };
    Point start(0, 0);
    Point goal(4, 4);

    //vector<vector<int>> gridmap{
    //{0,0,1,1,0,1,0,0},
    //{0,0,0,1,0,1,0,0},
    //{0,0,1,0,0,1,0,0},
    //{0,0,1,0,0,1,0,0},
    //{0,0,1,1,0,0,1,1},
    //{0,0,0,1,0,0,0,0},
    //{0,0,0,1,1,0,0,0},
    //};
    //Point start(0, 0);
    //Point goal(5, 4);

    vector<Point> path = AstarPathPlanning(gridmap, start, goal);
    cout << "最终的路径点数" << path.size() << endl;
    for (auto p : path)
    {
        if (p.x == goal.x && p.y == goal.y)
        {
            cout << "(" << p.x << "," << p.y << ")" << endl ;
        }
        else {
            cout << "(" << p.x << "," << p.y << ")"<<"->";
        }
    }
    return 0;
}