C++实现A*算法的示例代码

以下是使用C++实现A*算法的示例代码：

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

using namespace std;

// 定义一个节点结构体
struct Node {
    int x; // 节点的横坐标
    int y; // 节点的纵坐标
    int g; // 距离起点的距离
    int h; // 距离终点的估计距离
    bool operator<(const Node& other) const {
        return g + h > other.g + other.h; // 用于优先队列中节点的比较
    }
};

// 定义地图大小和起点终点坐标
const int N = 5;
const int M = 5;
int sx = 0, sy = 0; // 起点坐标
int tx = N-1, ty = M-1; // 终点坐标

// 定义地图
int map[N][M] = {
    {0, 0, 0, 0, 0},
    {0, 1, 1, 0, 0},
    {0, 0, 1, 0, 0},
    {0, 0, 1, 1, 0},
    {0, 0, 0, 0, 0}
};

// 定义一个用于存储路径的二维数组
int path[N][M];

// 定义一个用于存储节点是否被访问过的二维数组
bool vis[N][M];

// 定义一个用于存储起点到每个节点的距离的二维数组
int dist[N][M];

// 定义一个用于存储每个节点的父节点的二维数组
Node parent[N][M];

// 计算两个节点之间的曼哈顿距离
int manhattan(Node a, Node b) {
    return abs(a.x - b.x) + abs(a.y - b.y);
}

// A*算法
void Astar() {
    // 定义一个优先队列用于存储待访问的节点
    priority_queue<Node> q;

    // 初始化起点
    Node start = {sx, sy, 0, manhattan({sx, sy}, {tx, ty})};
    q.push(start);
    dist[sx][sy] = 0;
    vis[sx][sy] = true;

    // 开始搜索
    while (!q.empty()) {
        Node cur = q.top();
        q.pop();

        // 如果当前节点是终点，则搜索结束
        if (cur.x == tx && cur.y == ty) {
            break;
        }

        // 遍历当前节点的四个相邻节点
        for (int i = -1; i <= 1; i++) {
            for (int j = -1; j <= 1; j++) {
                if (i == 0 && j == 0) continue;
                int nx = cur.x + i;
                int ny = cur.y + j;
                // 如果相邻节点在地图内且没有被访问过且不是障碍物
                if (nx >= 0 && nx < N && ny >= 0 && ny < M && !vis[nx][ny] && map[nx][ny] == 0) {
                    // 计算相邻节点到起点的距离
                    int ndist = cur.g + 1;
                    // 如果相邻节点到起点的距离比之前计算的距离更小，则更新距离和父节点
                    if (ndist < dist[nx][ny]) {
                        dist[nx][ny] = ndist;
                        parent[nx][ny] = cur;
                        // 计算相邻节点到终点的估计距离
                        int nh = manhattan({nx, ny}, {tx, ty});
                        // 将相邻节点加入优先队列
                        q.push({nx, ny, ndist, nh});
                        // 标记相邻节点已经被访问过
                        vis[nx][ny] = true;
                    }
                }
            }
        }
    }

    // 回溯路径
    Node cur = {tx, ty, 0, 0};
    while (cur.x != sx || cur.y != sy) {
        path[cur.x][cur.y] = 1;
        cur = parent[cur.x][cur.y];
    }
    path[sx][sy] = 1;
}

// 输出地图和路径
void print() {
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < M; j++) {
            if (map[i][j] == 1) {
                cout << "X ";
            } else if (path[i][j] == 1) {
                cout << "* ";
            } else {
                cout << ". ";
            }
        }
        cout << endl;
    }
}

int main() {
    Astar();
    print();
    return 0;
}

代码解析：

1. 定义了一个节点结构体，包含节点的横纵坐标、距离起点的距离和距离终点的估计距离。重载了小于运算符，用于优先队列中节点的比较。
2. 定义了地图大小和起点终点坐标，以及地图和路径的二维数组、节点是否被访问过的二维数组、起点到每个节点的距离的二维数组和每个节点的父节点的二维数组。
3. 定义了计算两个节点之间的曼哈顿距离的函数。
4. 实现了A*算法的函数，包括初始化起点、优先队列的定义、搜索过程和回溯路径。
5. 实现了输出地图和路径的函数。
6. 在main函数中调用Astar函数和print函数。

A*算法的实现主要是在Astar函数中，首先初始化起点，将起点加入优先队列。然后开始搜索，每次从优先队列中取出距离起点最近的节点，遍历其四个相邻节点，如果相邻节点在地图内且没有被访问过且不是障碍物，则计算相邻节点到起点的距离，如果相邻节点到起点的距离比之前计算的距离更小，则更新距离和父节点，并将相邻节点加入优先队列。搜索结束后，回溯路径，从终点开始，依次找到每个节点的父节点，标记路径。最后输出地图和路径。