a*算法c++

以下是一个简单的C++实现A*算法的示例代码：

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

using namespace std;

// 定义一个节点结构体
struct Node {
    int x, y;  // 节点坐标
    int g, h;  // 路径长度g和启发函数估计值h
    bool operator < (const Node &rhs) const {
        return g + h > rhs.g + rhs.h;  // 用于优先队列中的排序
    }
};

// 计算启发函数估计值，这里使用曼哈顿距离
int manhattan(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// A*算法
vector<Node> astar(vector<vector<int>> &grid, Node start, Node end) {
    int n = grid.size(), m = grid[0].size();
    vector<Node> path;

    // 定义状态数组，存储每个节点的状态信息
    vector<vector<int>> state(n, vector<int>(m, 0));
    state[start.x][start.y] = 1;

    // 定义优先队列，用于存储待扩展的节点
    priority_queue<Node> q;
    q.push(start);

    // 定义前驱数组，用于记录每个节点的前驱节点
    vector<vector<Node>> prev(n, vector<Node>(m));

    // A*算法的主循环
    while (!q.empty()) {
        auto curr = q.top();
        q.pop();

        // 如果当前节点为终点，退出循环
        if (curr.x == end.x && curr.y == end.y) {
            path.push_back(curr);
            while (curr.x != start.x || curr.y != start.y) {
                path.push_back(prev[curr.x][curr.y]);
                curr = prev[curr.x][curr.y];
            }
            reverse(path.begin(), path.end());
            break;
        }

        // 扩展当前节点的相邻节点
        vector<pair<int, int>> directions = {{-1, 0}, {0, -1}, {1, 0}, {0, 1}};
        for (auto dir : directions) {
            int x = curr.x + dir.first, y = curr.y + dir.second;
            if (x < 0 || x >= n || y < 0 || y >= m || grid[x][y] == 1 || state[x][y] == 1) continue;
            int g = curr.g + 1;
            int h = manhattan(x, y, end.x, end.y);
            Node node = {x, y, g, h};
            q.push(node);
            state[x][y] = 1;
            prev[x][y] = curr;
        }
    }

    return path;
}

// 测试
int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 0},
        {0, 1, 1, 0},
        {0, 0, 0, 0},
        {0, 0, 0, 0}
    };
    Node start = {0, 0, 0, 0};
    Node end = {3, 3, 0, 0};
    vector<Node> path = astar(grid, start, end);
    for (auto node : path) {
        cout << "(" << node.x << ", " << node.y << ")" << endl;
    }
    return 0;
}

这里使用了一个Node结构体来表示每个节点的状态信息，使用了一个状态数组来记录每个节点是否已经被访问过，使用了一个优先队列来存储待扩展的节点，使用了一个前驱数组来记录每个节点的前驱节点。在A*算法的主循环中，每次从优先队列中取出估价函数值最小的节点进行扩展，如果找到了终点，则通过前驱数组回溯路径。