双向Astar算法C++代码实现

以下是双向Astar算法的C++代码实现，供您参考：

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

using namespace std;

#define INF 0x3f3f3f3f

struct Node {
    int x, y, f, g, h; // 行、列、估价函数、起点到当前点的距离、当前点到终点的估价距离
    bool operator <(const Node &b) const { // 重载小于运算符
        return f > b.f; // 按f值从小到大排序
    }
};

const int MAXN = 1005;
int n, m, ans;
char map[MAXN][MAXN];
vector<Node> vec[MAXN][MAXN]; // 存储每个点可以到达的点
int dis[MAXN][MAXN], vis[MAXN][MAXN]; // dis数组记录起点到每个点的距离，vis数组记录是否访问过该点

int dx[]{0, 0, 1, -1};
int dy[]{1, -1, 0, 0};

// 从起点开始搜索
void Astar(Node start, Node end) {
    priority_queue<Node> q1, q2; // 分别存储正反两个方向的搜索队列
    memset(dis, INF, sizeof(dis)); // 初始化dis数组
    memset(vis, 0, sizeof(vis)); // 初始化vis数组
    start.f = start.g + start.h; // 计算起点的f值
    end.f = end.g + end.h; // 计算终点的f值
    q1.push(start); // 将起点加入正向搜索队列
    q2.push(end); // 将终点加入反向搜索队列
    dis[start.x][start.y] = 0; // 起点到自己的距离为0
    dis[end.x][end.y] = 0; // 终点到自己的距离为0
    while (!q1.empty() && !q2.empty()) { // 当两个队列都不为空时，继续搜索
        Node u;
        if (q1.top().f <= q2.top().f) { // 选择f值小的队列进行搜索
            u = q1.top(); // 取出正向搜索队列中的队首元素
            q1.pop(); // 弹出队首元素
            if (vis[u.x][u.y]) { // 如果这个点已经被访问过了，就跳过
                continue;
            }
            vis[u.x][u.y] = 1; // 标记这个点已经被访问过了
            for (int i = 0; i < vec[u.x][u.y].size(); i++) { // 遍历这个点可以到达的所有点
                Node v = vec[u.x][u.y][i];
                if (!vis[v.x][v.y]) { // 如果这个点没有被访问过
                    int d = u.g + v.g; // 计算起点到当前点的距离
                    if (d < dis[v.x][v.y]) { // 如果当前点到起点的距离比原来计算的距离要小
                        dis[v.x][v.y] = d; // 更新起点到当前点的距离
                        v.g = d; // 更新当前点到起点的距离
                        v.f = v.g + v.h; // 更新当前点的f值
                        q1.push(v); // 将当前点加入正向搜索队列
                    }
                } else { // 如果这个点已经被访问过了
                    ans = min(ans, dis[u.x][u.y] + dis[v.x][v.y] + v.g); // 更新最短路
                }
            }
        } else { // 选择f值小的队列进行搜索
            u = q2.top(); // 取出反向搜索队列中的队首元素
            q2.pop(); // 弹出队首元素
            if (vis[u.x][u.y]) { // 如果这个点已经被访问过了，就跳过
                continue;
            }
            vis[u.x][u.y] = 1; // 标记这个点已经被访问过了
            for (int i = 0; i < vec[u.x][u.y].size(); i++) { // 遍历这个点可以到达的所有点
                Node v = vec[u.x][u.y][i];
                if (!vis[v.x][v.y]) { // 如果这个点没有被访问过
                    int d = u.g + v.g; // 计算终点到当前点的距离
                    if (d < dis[v.x][v.y]) { // 如果当前点到终点的距离比原来计算的距离要小
                        dis[v.x][v.y] = d; // 更新终点到当前点的距离
                        v.g = d; // 更新当前点到终点的距离
                        v.f = v.g + v.h; // 更新当前点的f值
                        q2.push(v); // 将当前点加入反向搜索队列
                    }
                } else { // 如果这个点已经被访问过了
                    ans = min(ans, dis[u.x][u.y] + dis[v.x][v.y] + v.g); // 更新最短路
                }
            }
        }
    }
}

int main() {
    cin >> n >> m;
    Node start, end;
    for (int i = 1; i <= n; i++) {
        for (int j = 1; j <= m; j++) {
            cin >> map[i][j];
            if (map[i][j] == 'S') { // 找到起点
                start.x = i;
                start.y = j;
                start.g = 0;
                start.h = abs(i - n) + abs(j - m); // 计算起点到终点的曼哈顿距离
                start.f = start.g + start.h;
            }
            if (map[i][j] == 'E') { // 找到终点
                end.x = i;
                end.y = j;
                end.g = 0;
                end.h = abs(i - start.x) + abs(j - start.y); // 计算终点到起点的曼哈顿距离
                end.f = end.g + end.h;
            }
        }
    }
    for (int i = 1; i <= n; i++) { // 预处理每个点可以到达的点
        for (int j = 1; j <= m; j++) {
            if (map[i][j] != '#') { // 如果当前点不是障碍物
                for (int k = 0; k < 4; k++) { // 向四个方向搜索
                    int nx = i + dx[k];
                    int ny = j + dy[k];
                    if (nx >= 1 && nx <= n && ny >= 1 && ny <= m && map[nx][ny] != '#') { // 如果可以到达
                        Node node;
                        node.x = nx;
                        node.y = ny;
                        node.g = 1; // 计算当前点到邻居点的距离为1
                        node.h = abs(nx - end.x) + abs(ny - end.y); // 计算邻居点到终点的曼哈顿距离
                        node.f = node.g + node.h; // 计算邻居点的f值
                        vec[i][j].push_back(node); // 将邻居点加入到当前点可以到达的点的集合中
                    }
                }
            }
        }
    }
    ans = INF; // 初始化最短路为无穷大
    Astar(start, end); // 开始双向Astar搜索
    cout << ans << endl; // 输出最短路
    return 0;
}

注：以上代码仅供参考，实际应用中可能需要根据具体情况进行修改。