c++怎么实现一个A寻路算法_c++实现A寻路算法的原理与代码示例

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
<p>using namespace std;</p>
                    <div class="aritcle_card">
                        <a class="aritcle_card_img" href="/ai/2128">
                            <img src="https://img.php.cn/upload/ai_manual/000/000/000/175679969239968.png" alt="算家云">
                        </a>
                        <div class="aritcle_card_info">
                            <a href="/ai/2128">算家云</a>
                            <p>高效、便捷的人工智能算力服务平台</p>
                            <div class="">
                                <img src="/static/images/card_xiazai.png" alt="算家云">
                                <span>37</span>
                            </div>
                        </div>
                        <a href="/ai/2128" class="aritcle_card_btn">
                            <span>查看详情</span>
                            <img src="/static/images/cardxiayige-3.png" alt="算家云">
                        </a>
                    </div>
                <p>// 网格大小
const int ROW = 5;
const int COL = 5;</p><p>// 节点结构
struct Node {
int x, y;
double g, h, f;
Node* parent;</p><pre class='brush:php;toolbar:false;'>Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}

// 方向数组：上下左右
int dx[] = {-1, 1, 0, 0};
int dy[] = {0, 0, -1, 1};

vector<vector<bool>> closed(ROW, vector<bool>(COL, false));
vector<vector<Node*>> nodeMap(ROW, vector<Node*>(COL, nullptr));

// 初始化起点
Node* startNode = new Node(start.first, start.second);
startNode->g = 0;
startNode->h = heuristic(start.first, start.second, end.first, end.second);
startNode->f = startNode->g + startNode->h;

priority_queue<Node*, vector<Node*>, CompareNode> openList;
openList.push(startNode);
nodeMap[start.first][start.second] = startNode;

while (!openList.empty()) {
    Node* current = openList.top();
    openList.pop();

    int x = current->x;
    int y = current->y;

    // 标记为已处理
    closed[x][y] = true;

    // 到达终点
    if (x == end.first && y == end.second) {
        cout << "找到路径：";
        printPath(current);
        cout << endl;
        return;
    }

    // 探索邻居
    for (int i = 0; i < 4; i++) {
        int nx = x + dx[i];
        int ny = y + dy[i];

        if (isValid(nx, ny, grid) && !closed[nx][ny]) {
            double tentativeG = current->g + 1; // 假设移动代价为1

            Node* neighbor = nodeMap[nx][ny];
            if (neighbor == nullptr) {
                neighbor = new Node(nx, ny);
                neighbor->g = tentativeG;
                neighbor->h = heuristic(nx, ny, end.first, end.second);
                neighbor->f = neighbor->g + neighbor->h;
                neighbor->parent = current;
                nodeMap[nx][ny] = neighbor;
                openList.push(neighbor);
            }
            else if (tentativeG < neighbor->g) {
                // 发现更短路径，更新
                neighbor->g = tentativeG;
                neighbor->f = neighbor->g + neighbor->h;
                neighbor->parent = current;
                // 实际中可能需要重新排序openList，这里简化处理
            }
        }
    }
}

cout << "未找到路径！\n";