C++ 实现曼哈顿路由算法

曼哈顿路由算法是一种常见的寻路算法，可以用来在网格图中找到两点之间的最短路径。下面是 C++ 实现的示例代码：

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

using namespace std;

// 定义网格节点
struct Node {
    int x, y; // 节点坐标
    int g, h; // g: 起点到该点的实际代价; h: 该点到终点的估计代价
    Node* parent; // 父节点指针

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

// 定义比较函数
struct CompareNode {
    bool operator()(const Node* a, const Node* b) const {
        return a->g + a->h > b->g + b->h;
    }
};

// 曼哈顿距离估价函数
int manhattanDistance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

// 判断节点是否是障碍物
bool isObstacle(int x, int y) {
    // TODO: 根据实际情况修改
    return false;
}

// A*算法
vector<Node*> aStar(Node* startNode, Node* endNode) {
    vector<Node*> path;

    // 定义开放列表和关闭列表
    priority_queue<Node*, vector<Node*>, CompareNode> openList;
    vector<Node*> closedList;

    // 将起点加入开放列表
    startNode->h = manhattanDistance(startNode->x, startNode->y, endNode->x, endNode->y);
    openList.push(startNode);

    while (!openList.empty()) {
        // 取出开放列表中的代价最小节点
        Node* curNode = openList.top();
        openList.pop();

        // 如果当前节点为终点，返回路径
        if (curNode == endNode) {
            while (curNode != nullptr) {
                path.push_back(curNode);
                curNode = curNode->parent;
            }
            reverse(path.begin(), path.end());
            break;
        }

        // 将当前节点加入关闭列表
        closedList.push_back(curNode);

        // 遍历当前节点的邻居
        for (int i = -1; i <= 1; ++i) {
            for (int j = -1; j <= 1; ++j) {
                // 排除自身和斜向节点
                if (i == 0 && j == 0 || abs(i) == 1 && abs(j) == 1) {
                    continue;
                }

                int nx = curNode->x + i;
                int ny = curNode->y + j;

                // 排除越界和障碍物
                if (nx < 0 || ny < 0 || isObstacle(nx, ny)) {
                    continue;
                }

                int ng = curNode->g + manhattanDistance(curNode->x, curNode->y, nx, ny);

                // 判断邻居是否已在关闭列表中
                bool inClosedList = false;
                for (Node* node : closedList) {
                    if (node->x == nx && node->y == ny) {
                        inClosedList = true;
                        break;
                    }
                }

                if (inClosedList) {
                    continue;
                }

                // 判断邻居是否已在开放列表中
                bool inOpenList = false;
                for (Node* node : openList) {
                    if (node->x == nx && node->y == ny) {
                        inOpenList = true;

                        // 如果新路径更优，则更新邻居节点
                        if (ng < node->g) {
                            node->g = ng;
                            node->parent = curNode;
                        }
                        break;
                    }
                }

                if (!inOpenList) {
                    Node* neighbor = new Node(nx, ny);
                    neighbor->g = ng;
                    neighbor->h = manhattanDistance(nx, ny, endNode->x, endNode->y);
                    neighbor->parent = curNode;
                    openList.push(neighbor);
                }
            }
        }
    }

    // 释放内存
    for (Node* node : closedList) {
        delete node;
    }
    for (Node* node : openList) {
        delete node;
    }

    return path;
}

int main() {
    Node* startNode = new Node(0, 0);
    Node* endNode = new Node(5, 5);
    vector<Node*> path = aStar(startNode, endNode);

    for (Node* node : path) {
        cout << "(" << node->x << "," << node->y << ") ";
    }
    cout << endl;

    delete startNode;
    delete endNode;

    return 0;
}

注意，这里的示例代码仅实现了最基本的 A* 算法框架，不包括优化和细节处理。在实际应用中，还需要根据具体情况对算法进行优化和调整。