C++如何实现一个简单的A*寻路算法_C++游戏AI开发中的路径规划实战

#include <vector>
#include <queue>
#include <cmath>
#include <iostream>
<p>struct Node {
int x, y;
float g, h; // g: 实际代价，h: 启发值
float f() const { return g + h; }
Node* parent;</p><pre class='brush:php;toolbar:false;'>Node(int x, int y) : x(x), y(y), g(0), h(0), parent(nullptr) {}

bool operator<(const Node& other) const {
    return f() > other.f(); // 优先队列需要最小堆
}

float heuristic(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2); // 曼哈顿距离
}
<p>std::vector<std::pair<int, int>> getNeighbors(int x, int y) {
return {{x+1,y}, {x-1,y}, {x,y+1}, {x,y-1}};
// 若允许斜向移动，可加入 {x+1,y+1}, {x+1,y-1} 等
}</p>

std::vector<Node*> aStar(const std::vector<std::vector<int>>& grid,
                           Node* start, Node* end) {
    int rows = grid.size();
    int cols = grid[0].size();
<pre class='brush:php;toolbar:false;'>auto isValid = [&](int x, int y) {
    return x >= 0 && x < rows && y >= 0 && y < cols && grid[x][y] == 0;
};

std::priority_queue<Node> openList;
std::vector<std::vector<bool>> closedList(rows, std::vector<bool>(cols, false));
std::vector<std::vector<Node*>> nodeMap(rows, std::vector<Node*>(cols, nullptr));

for (int i = 0; i < rows; ++i)
    for (int j = 0; j < cols; ++j)
        if (grid[i][j] == 0)
            nodeMap[i][j] = new Node(i, j);

start = nodeMap[start->x][start->y];
end = nodeMap[end->x][end->y];
start->h = heuristic(start->x, start->y, end->x, end->y);

openList.push(*start);

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

    if (closedList[current.x][current.y]) continue;
    closedList[current.x][current.y] = true;

    if (current.x == end->x && current.y == end->y) {
        // 回溯路径
        std::vector<Node*> path;
        Node* p = end;
        while (p != nullptr) {
            path.push_back(p);
            p = p->parent;
        }
        return path;
    }

    for (auto& [nx, ny] : getNeighbors(current.x, current.y)) {
        if (!isValid(nx, ny) || closedList[nx][ny]) continue;

        Node* neighbor = nodeMap[nx][ny];
        float tentativeG = current.g + 1; // 假设每步代价为1

        if (tentativeG < neighbor->g || !closedList[nx][ny]) {
            neighbor->parent = &const_cast<Node&>(current);
            neighbor->g = tentativeG;
            neighbor->h = heuristic(nx, ny, end->x, end->y);
            openList.push(*neighbor);
        }
    }
}

return {}; // 无路径

int main() {
    std::vector<std::vector<int>> grid = {
        {0, 0, 0, 1, 0},
        {0, 1, 0, 1, 0},
        {0, 1, 0, 0, 0},
        {0, 0, 0, 1, 0},
        {0, 0, 0, 0, 0}
    };
<pre class='brush:php;toolbar:false;'>Node* start = new Node(0, 0);
Node* end = new Node(4, 4);

auto path = aStar(grid, start, end);

if (!path.empty()) {
    std::cout << "Found path:\n";
    for (auto it = path.rbegin(); it != path.rend(); ++it) {
        std::cout << "(" << (*it)->x << "," << (*it)->y << ") ";
    }
    std::cout << "\n";
} else {
    std::cout << "No path found.\n";
}

// 简单释放（实际应遍历所有创建的节点）
for (auto& row : grid)
    for (auto val : row)
        if (val == 0) /* 伪代码 */ delete /* 对应节点 */;
delete start; delete end;

return 0;