a*算法C++代码

以下是一个简单的A*算法的C++代码实现，其中包含了估价函数、开放和封闭列表等概念：

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

using namespace std;

// 表示一个节点的结构体
struct Node {
    int x, y;  // 节点的坐标
    int g, h, f;  // g: 起点到当前节点的实际距离，h: 当前节点到终点的预估距离，f: g+h
    Node* parent;  // 指向父节点的指针

    Node(int x, int y, int g, int h, Node* parent) : x(x), y(y), g(g), h(h), f(g+h), parent(parent) {}
};

// 比较两个节点的f值
struct CompareNode {
    bool operator() (Node* a, Node* b) {
        return a->f > b->f;
    }
};

// 计算两个节点之间的曼哈顿距离
int manhattanDistance(int x1, int y1, int x2, int y2) {
    return abs(x1-x2) + abs(y1-y2);
}

// 判断节点是否在地图内
bool isValid(int x, int y, int n, int m) {
    return x >= 0 && x < n && y >= 0 && y < m;
}

vector<pair<int, int>> findPath(vector<vector<int>>& map, int sx, int sy, int tx, int ty) {
    int n = map.size();
    int m = map[0].size();

    // 起点和终点相同，直接返回
    if (sx == tx && sy == ty) {
        return {{sx, sy}};
    }

    // 用二维数组记录每个节点的状态，0表示未访问，1表示在开放列表，-1表示在封闭列表
    vector<vector<int>> state(n, vector<int>(m, 0));

    // 定义开放列表和封闭列表
    priority_queue<Node*, vector<Node*>, CompareNode> openList;
    vector<Node*> closeList;

    // 将起点加入开放列表
    Node* startNode = new Node(sx, sy, 0, manhattanDistance(sx, sy, tx, ty), nullptr);
    openList.push(startNode);
    state[sx][sy] = 1;

    // 开始搜索
    while (!openList.empty()) {
        // 取出f值最小的节点
        Node* curNode = openList.top();
        openList.pop();

        // 将该节点加入封闭列表
        closeList.push_back(curNode);
        state[curNode->x][curNode->y] = -1;

        // 如果当前节点是终点，返回路径
        if (curNode->x == tx && curNode->y == ty) {
            vector<pair<int, int>> path;
            while (curNode) {
                path.emplace_back(curNode->x, curNode->y);
                curNode = curNode->parent;
            }
            reverse(path.begin(), path.end());
            return path;
        }

        // 遍历当前节点的相邻节点
        vector<pair<int, int>> dirs{{0, 1}, {0, -1}, {1, 0}, {-1, 0}};
        for (auto& dir : dirs) {
            int nx = curNode->x + dir.first;
            int ny = curNode->y + dir.second;

            // 判断相邻节点是否在地图内
            if (!isValid(nx, ny, n, m)) continue;
            // 判断相邻节点是否是障碍物或已在封闭列表中
            if (map[nx][ny] == 1 || state[nx][ny] == -1) continue;

            // 计算相邻节点的g、h、f值
            int ng = curNode->g + 1;
            int nh = manhattanDistance(nx, ny, tx, ty);
            int nf = ng + nh;

            // 如果相邻节点已在开放列表中，更新其g值和父节点
            if (state[nx][ny] == 1) {
                for (auto& node : openList) {
                    if (node->x == nx && node->y == ny) {
                        if (ng < node->g) {
                            node->g = ng;
                            node->f = nf;
                            node->parent = curNode;
                        }
                        break;
                    }
                }
            }
            // 否则将相邻节点加入开放列表
            else {
                Node* node = new Node(nx, ny, ng, nh, curNode);
                openList.push(node);
                state[nx][ny] = 1;
            }
        }
    }

    // 如果没有找到路径，返回空数组
    return {};
}

int main() {
    vector<vector<int>> map = {
        {0, 0, 0, 0, 0},
        {0, 1, 0, 1, 0},
        {0, 1, 0, 0, 0},
        {0, 0, 1, 1, 0},
        {0, 0, 0, 0, 0},
    };
    int sx = 0, sy = 0, tx = 4, ty = 4;
    auto path = findPath(map, sx, sy, tx, ty);
    for (auto& p : path) {
        cout << "(" << p.first << ", " << p.second << ") ";
    }
    cout << endl;
    return 0;
}