A*寻路算法

于 2024-04-06 21:09:31 发布
阅读量439 收藏
点赞数 5
文章标签： 算法 c++ 图论
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/alianjack/article/details/137438332
版权 
#include <iostream>
#include <vector>
#include <cmath>
#include <queue>
#include <algorithm>

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) {}
    int f() const { return g + h; }  // 计算F值
};

// 判断两个节点是否相等
bool operator==(const Node& lhs, const Node& rhs) {
    return lhs.x == rhs.x && lhs.y == rhs.y;
}

// 判断F值大小
struct CompareF {
    bool operator()(const Node* lhs, const Node* rhs) {
        return lhs->f() > rhs->f();
    }
};

// 检查点是否在地图范围内
bool isValid(int x, int y, int rows, int cols) {
    return x >= 0 && x < rows && y >= 0 && y < cols;
}

// 检查点是否为障碍物
bool isBlocked(int x, int y, const std::vector<std::vector<int>>& grid) {
    return grid[x][y] == 1;
}

// 计算两个节点之间的曼哈顿距离（H值）
int manhattanDistance(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

// A星寻路算法
std::vector<Node*> aStar(const std::vector<std::vector<int>>& grid, const Node& start, const Node& goal) {
    int rows = grid.size();
    int cols = grid[0].size();

    std::vector<Node*> openList;
    std::vector<Node*> closedList;

    openList.push_back(new Node(start.x, start.y));

    while (!openList.empty()) {
        std::sort(openList.begin(), openList.end(), CompareF());  // 按F值从小到大排序

        Node* current = openList.front();
        openList.erase(openList.begin());
        closedList.push_back(current);

        if (*current == goal) {
            std::vector<Node*> path;
            while (current != nullptr) {
                path.push_back(current);
                current = current->parent;
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        // 生成邻居节点
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};
        for (int i = 0; i < 4; ++i) {
            int newX = current->x + dx[i];
            int newY = current->y + dy[i];

            if (!isValid(newX, newY, rows, cols) || isBlocked(newX, newY, grid)) {
                continue;
            }

            Node* neighbor = new Node(newX, newY);
            neighbor->g = current->g + 1;
            neighbor->h = manhattanDistance(*neighbor, goal);
            neighbor->parent = current;

            auto itOpen = std::find_if(openList.begin(), openList.end(), [&](const Node* node) { return *node == *neighbor; });
            auto itClosed = std::find_if(closedList.begin(), closedList.end(), [&](const Node* node) { return *node == *neighbor; });

            if (itOpen != openList.end() && neighbor->g >= (*itOpen)->g) {
                continue;
            }
            if (itClosed != closedList.end() && neighbor->g >= (*itClosed)->g) {
                continue;
            }

            openList.push_back(neighbor);
        }
    }

    return {};  // 无可行路径
}

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

    Node start(0, 0);
    Node goal(4, 4);

    std::vector<Node*> path = aStar(grid, start, goal);

    if (path.empty()) {
        std::cout << "No feasible path found." << std::endl;
    } else {
        std::cout << "The path from start to goal:" << std::endl;
        for (const auto node : path) {
            std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
        }
    }

    // 释放动态分配的节点内存
    for (const auto node : path) {
        delete node;
    }

    return 0;
}

 

这个示例代码展示了如何使用C++实现A星寻路算法，其中使用了优先队列来管理开放列表，并用一个向量来存储关闭列表。算法会在给定的地图上寻找起点到目标点的最短路径并返回。如果不存在可行路径，则返回一个空向量。请注意，在使用动态分配的节点内存后，应在程序结束时释放这些内存以避免内存泄漏。

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

// 定义节点结构体
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) {}
    int f() const { return g + h; }  // 计算F值
};

// 判断两个节点是否相等
bool operator==(const Node& lhs, const Node& rhs) {
    return lhs.x == rhs.x && lhs.y == rhs.y;
}

// 判断F值大小
struct CompareF {
    bool operator()(const Node* lhs, const Node* rhs) {
        return lhs->f() > rhs->f();
    }
};

// 检查点是否在地图范围内
bool isValid(int x, int y, int rows, int cols) {
    return x >= 0 && x < rows && y >= 0 && y < cols;
}

// 检查点是否为障碍物
bool isBlocked(int x, int y, const std::vector<std::vector<int>>& grid) {
    return grid[x][y] == 1;
}

// 计算两个节点之间的曼哈顿距离（H值）
int manhattanDistance(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

// A星寻路算法
std::vector<Node*> aStar(const std::vector<std::vector<int>>& grid, const Node& start, const Node& goal) {
    int rows = grid.size();
    int cols = grid[0].size();

    std::vector<Node*> openList;
    std::vector<Node*> closedList;

    openList.push_back(new Node(start.x, start.y));

    while (!openList.empty()) {
        Node* current = openList[0];
        int currentIndex = 0;
        for (int i = 1; i < openList.size(); ++i) {
            if (openList[i]->f() < current->f()) {
                current = openList[i];
                currentIndex = i;
            }
        }

        openList.erase(openList.begin() + currentIndex);
        closedList.push_back(current);

        if (*current == goal) {
            std::vector<Node*> path;
            while (current != nullptr) {
                path.push_back(current);
                current = current->parent;
            }
            std::reverse(path.begin(), path.end());
            return path;
        }

        // 生成邻居节点
        int dx[] = {-1, 1, 0, 0};
        int dy[] = {0, 0, -1, 1};
        for (int i = 0; i < 4; ++i) {
            int newX = current->x + dx[i];
            int newY = current->y + dy[i];

            if (!isValid(newX, newY, rows, cols) || isBlocked(newX, newY, grid)) {
                continue;
            }

            Node* neighbor = new Node(newX, newY);
            neighbor->g = current->g + 1;
            neighbor->h = manhattanDistance(*neighbor, goal);
            neighbor->parent = current;

            bool inOpenList = false;
            bool inClosedList = false;
            for (const auto node : openList) {
                if (*node == *neighbor) {
                    inOpenList = true;
                    if (neighbor->g < node->g) {
                        node->g = neighbor->g;
                        node->parent = neighbor->parent;
                    }
                }
            }
            for (const auto node : closedList) {
                if (*node == *neighbor) {
                    inClosedList = true;
                    break;
                }
            }

            if (!inOpenList && !inClosedList) {
                openList.push_back(neighbor);
            } else {
                delete neighbor;
            }
        }
    }

    return {};  // 无可行路径
}

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

    Node start(0, 0);
    Node goal(4, 4);

    std::vector<Node*> path = aStar(grid, start, goal);

    if (path.empty()) {
        std::cout << "No feasible path found." << std::endl;
    } else {
        std::cout << "The path from start to goal:" << std::endl;
        for (const auto node : path) {
            std::cout << "(" << node->x << ", " << node->y << ")" << std::endl;
        }
    }

    // 释放动态分配的节点内存
    for (const auto node : path) {
        delete node;
    }

    return 0;
}

 这段代码与之前的实现方式相比，主要区别在于选择最小F值节点的方法。在这个示例代码中，我们通过比较F值的大小来找到最小F值节点，而不是使用优先队列。其他部分的实现基本相同，都是按照A星寻路算法的思想进行的。同样，需要注意在程序结束时释放动态分配的节点内存。

评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值