怎样在C++中实现A*寻路算法_路径规划实战指南

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

using namespace std;

// 定义节点结构
struct Node {
    int x, y;
    int g, h, f; // g: 起点到当前节点的代价, h: 启发式函数, f: g+h
    Node* parent;

    Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}
};

// 定义比较函数，用于优先队列
struct CompareNodes {
    bool operator()(Node* a, Node* b) {
        return a->f > b->f; // f值小的优先级高
    }
};

// 启发式函数 (曼哈顿距离)
int heuristic(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// A* 寻路算法
vector<pair<int, int>> aStar(vector<vector<int>>& grid, int startX, int startY, int endX, int endY) {
    int rows = grid.size();
    int cols = grid[0].size();

    // 检查起点和终点是否有效
    if (startX < 0 || startX >= cols || startY < 0 || startY >= rows ||
        endX < 0 || endX >= cols || endY < 0 || endY >= rows ||
        grid[startY][startX] == 1 || grid[endY][endX] == 1) {
        return {}; // 无效路径
    }

    // 创建起点和终点节点
    Node* startNode = new Node(startX, startY);
    Node* endNode = new Node(endX, endY);

    // 开放列表和关闭列表
    priority_queue<Node*, vector<Node*>, CompareNodes> openList;
    vector<vector<bool>> closedList(rows, vector<bool>(cols, false));

    openList.push(startNode);

    // 八个方向的移动
    int dx[] = {0, 0, 1, -1, 1, -1, 1, -1};
    int dy[] = {1, -1, 0, 0, 1, -1, -1, 1};

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

        // 如果当前节点是终点，则找到路径
        if (current->x == endNode->x && current->y == endNode->y) {
            vector<pair<int, int>> path;
            while (current != nullptr) {
                path.push_back({current->x, current->y});
                current = current->parent;
            }
            reverse(path.begin(), path.end());

            // 清理内存
            while (!openList.empty()) {
                Node* node = openList.top();
                openList.pop();
                delete node;
            }

            delete startNode;
            delete endNode;
            return path;
        }

        closedList[current->y][current->x] = true;

        // 遍历相邻节点
        for (int i = 0; i < 8; ++i) { // 8 directions
            int newX = current->x + dx[i];
            int newY = current->y + dy[i];

            // 检查是否越界或障碍物
            if (newX < 0 || newX >= cols || newY < 0 || newY >= rows || grid[newY][newX] == 1 || closedList[newY][newX]) {
                continue;
            }

            Node* neighbor = new Node(newX, newY);
            neighbor->g = current->g + 1; // 假设移动代价为1
            neighbor->h = heuristic(newX, newY, endX, endY);
            neighbor->f = neighbor->g + neighbor->h;
            neighbor->parent = current;

            // 检查邻居是否在开放列表中，如果存在，是否需要更新
            bool inOpenList = false;
            priority_queue<Node*, vector<Node*>, CompareNodes> tempOpenList = openList;
            while (!tempOpenList.empty()) {
                Node* node = tempOpenList.top();
                tempOpenList.pop();
                if (node->x == neighbor->x && node->y == neighbor->y) {
                    inOpenList = true;
                    if (neighbor->g < node->g) {
                        node->g = neighbor->g;
                        node->f = neighbor->f;
                        node->parent = current;
                    }
                    break;
                }
            }

            if (!inOpenList) {
                openList.push(neighbor);
            } else {
                delete neighbor; // 避免内存泄漏
            }
        }
    }

    // 清理内存
    delete startNode;
    delete endNode;
    return {}; // 没有找到路径
}

int main() {
    vector<vector<int>> grid = {
        {0, 0, 0, 0, 0},
        {0, 1, 1, 1, 0},
        {0, 0, 0, 0, 0},
        {0, 1, 1, 1, 0},
        {0, 0, 0, 0, 0}
    };

    vector<pair<int, int>> path = aStar(grid, 0, 0, 4, 4);

    if (!path.empty()) {
        cout << "Path found:" << endl;
        for (auto& p : path) {
            cout << "(" << p.first << ", " << p.second << ") ";
        }
        cout << endl;
    } else {
        cout << "No path found." << endl;
    }

    return 0;
}