c++怎么实现A寻路算法_c++ A寻路算法实现方法-C++-PHP中文网

c++怎么实现A寻路算法_c++ A寻路算法实现方法

尼克

发布： 2025-10-01 15:07:02

原创

517人浏览过

A*寻路算法通过f(n)=g(n)+h(n)评估节点，使用优先队列管理开放列表，结合曼哈顿距离启发函数与网格邻居扩展，最终找到从起点到终点的最优路径。

c++怎么实现a寻路算法_c++ a寻路算法实现方法

在C++中实现A*（A星）寻路算法，核心是结合Dijkstra最短路径思想与启发式搜索。它通过评估每个节点的f(n) = g(n) + h(n)来选择最优路径，其中g(n)是从起点到当前点的实际代价，h(n)是当前点到终点的预估代价（启发函数），通常使用曼哈顿距离或欧几里得距离。

1. 定义节点结构

每个网格点需要记录坐标、代价值以及父节点信息，用于回溯路径：

struct Node {
    int x, y;
    double g, h, f;
    Node* parent;
<pre class='brush:php;toolbar:false;'>Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}

bool operator==(const Node& other) const {
    return x == other.x && y == other.y;
}

登录后复制

};

2. 启发函数设计

常用曼哈顿距离作为h值，在四方向移动场景下更合适：

立即学习“C++免费学习笔记（深入）”；

double heuristic(Node& a, Node& b) {
    return abs(a.x - b.x) + abs(a.y - b.y); // 曼哈顿距离
}

登录后复制

3. 开放列表和关闭列表管理

用优先队列维护开放列表（按f值排序），用set或vector管理已访问节点：

法语写作助手

法语助手旗下的AI智能写作平台，支持语法、拼写自动纠错，一键改写、润色你的法语作文。

查看详情

#include <queue>
#include <set>
#include <vector>
<p>struct CompareNode {
bool operator()(Node<em> a, Node</em> b) {
return a->f > b->f; // 小顶堆
}
};</p><p>std::priority_queue<Node<em>, std::vector<Node</em>>, CompareNode> openList;
std::set<std::pair<int, int>> closedSet;</p>

登录后复制

4. 主搜索循环实现

从起点开始扩展邻居，更新代价值并加入开放列表，直到找到终点：

std::vector<Node*> findPath(int grid[][COL], int rows, int cols, Node& start, Node& end) {
    openList.push(&start);
<pre class='brush:php;toolbar:false;'>while (!openList.empty()) {
    Node* current = openList.top(); openList.pop();

    if (current->x == end.x && current->y == end.y) {
        // 构建路径
        std::vector<Node*> path;
        while (current) {
            path.push_back(current);
            current = current->parent;
        }
        reverse(path.begin(), path.end());
        return path;
    }

    closedSet.insert({current->x, current->y});

    // 遍历上下左右四个方向
    int dx[] = {0, 0, -1, 1};
    int dy[] = {-1, 1, 0, 0};

    for (int i = 0; i < 4; ++i) {
        int nx = current->x + dx[i];
        int ny = current->y + dy[i];

        if (nx < 0 || nx >= rows || ny < 0 || ny >= cols) continue;
        if (grid[nx][ny] == 1) continue; // 1表示障碍物
        if (closedSet.find({nx, ny}) != closedSet.end()) continue;

        Node* neighbor = new Node(nx, ny);
        double tentative_g = current->g + 1; // 假设每步代价为1

        bool isNew = true;
        for (auto& n : openListContainer) { // 注意：priority_queue不支持遍历，需额外容器辅助
            if (*n == *neighbor) {
                isNew = false;
                if (tentative_g < n->g) {
                    n->g = tentative_g;
                    n->f = n->g + n->h;
                    n->parent = current;
                }
                break;
            }
        }

        if (isNew) {
            neighbor->g = tentative_g;
            neighbor->h = heuristic(*neighbor, end);
            neighbor->f = neighbor->g + neighbor->h;
            neighbor->parent = current;
            openList.push(neighbor);
            openListContainer.push_back(neighbor); // 辅助查找
        }
    }
}
return {}; // 无路径

登录后复制

}

注意：标准priority_queue无法遍历，实际项目中可用multiset或自定义可更新堆结构优化性能。