A*算法C++代码

以下是 A*算法的 C++ 代码示例：

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

using namespace std;

struct Node {
    int x, y;  // 节点坐标
    int f, g, h;  // f(x) = g(x) + h(x)
    Node* parent;  // 父节点指针
    Node(int x, int y) : x(x), y(y), f(0), g(0), h(0), parent(nullptr) {};
};

// 比较函数
struct cmp {
    bool operator()(Node* n1, Node* n2) {
        return n1->f > n2->f;
    }
};

class AStar {
public:
    AStar(int rows, int cols, int startX, int startY, int endX, int endY, vector<vector<int>>& grids) {
        this->rows = rows;
        this->cols = cols;
        this->startX = startX;
        this->startY = startY;
        this->endX = endX;
        this->endY = endY;
        this->grids = grids;
    }

    vector<Node*> findPath() {
        vector<Node*> path;
        Node* startNode = new Node(startX, startY);
        Node* endNode = new Node(endX, endY);
        priority_queue<Node*, vector<Node*>, cmp> openList;  // 开启列表
        vector<Node*> closeList;  // 关闭列表
        openList.push(startNode);
        while (!openList.empty()) {
            Node* curNode = openList.top();
            openList.pop();
            closeList.push_back(curNode);
            if (curNode->x == endX && curNode->y == endY) {
                // 找到路径
                Node* node = curNode;
                while (node) {
                    path.push_back(node);
                    node = node->parent;
                }
                reverse(path.begin(), path.end());
                break;
            }
            for (int i = -1; i <= 1; i++) {
                for (int j = -1; j <= 1; j++) {
                    if (i == 0 && j == 0) {
                        continue;
                    }
                    int xPos = curNode->x + i;
                    int yPos = curNode->y + j;
                    if (xPos < 0 || xPos >= rows || yPos < 0 || yPos >= cols) {
                        continue;
                    }
                    if (grids[xPos][yPos] == 1) {
                        continue;
                    }
                    bool inCloseList = false;
                    for (auto node : closeList) {
                        if (node->x == xPos && node->y == yPos) {
                            inCloseList = true;
                            break;
                        }
                    }
                    if (inCloseList) {
                        continue;
                    }
                    Node* childNode = new Node(xPos, yPos);
                    childNode->parent = curNode;
                    childNode->g = curNode->g + 1;
                    childNode->h = heuristic(childNode, endNode);
                    childNode->f = childNode->g + childNode->h;
                    bool inOpenList = false;
                    for (auto node : openList) {
                        if (node->x == xPos && node->y == yPos) {
                            inOpenList = true;
                            if (childNode->f < node->f) {
                                node->f = childNode->f;
                                node->g = childNode->g;
                                node->h = childNode->h;
                                node->parent = childNode->parent;
                            }
                            break;
                        }
                    }
                    if (!inOpenList) {
                        openList.push(childNode);
                    }
                }
            }
        }
        // 释放内存
        for (auto node : closeList) {
            delete node;
        }
        return path;
    }

private:
    int rows, cols;  // 地图大小
    int startX, startY, endX, endY;  // 起点和终点坐标
    vector<vector<int>> grids;  // 地图数组

    // 计算启发函数
    int heuristic(Node* a, Node* b) {
        return abs(a->x - b->x) + abs(a->y - b->y);
    }
};

int main() {
    int rows = 5, cols = 5;
    int startX = 0, startY = 0;
    int endX = 4, endY = 4;
    vector<vector<int>> grids(rows, vector<int>(cols, 0));
    grids[1][1] = 1;
    grids[2][1] = 1;
    grids[3][1] = 1;
    AStar astar(rows, cols, startX, startY, endX, endY, grids);
    vector<Node*> path = astar.findPath();
    for (auto node : path) {
        cout << "(" << node->x << "," << node->y << ")" << endl;
    }
    return 0;
}