C++实现A寻路算法详解

struct Node {
    int x, y;
    double g, h, f;
    Node* parent;
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;
}
};

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

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

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

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

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

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

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

        if (nx &lt; 0 || nx &gt;= rows || ny &lt; 0 || ny &gt;= 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-&gt;g + 1; // 假设每步代价为1

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

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