zoukankan      html  css  js  c++  java
  • 使用优先队列完成了一个Astar搜索算法的c++实现,有时间可以完整的完成一遍

    #include<queue>
    #include<vector>
    #include<unordered_map>
    using namespace std;
    struct Node {
        int x;
        int y;
        double cost;
        int parent;
    
        Node(int ix, int iy, int icost, int iparent)
            : x(ix), y(iy), cost(icost), parent(iparent) {
            }
        
        bool operator < (const Node& b) {
            return cost < b.cost;
        }
    };
    
    
    
    class AstarPlanner {
        public:
            void generateObstacleMap();
            vector<tuple<int, int, double>> getMotions();
            void verifyGrid(int x, int y);
            vector<pair<int, int>> aStarPlan(int sx, int sy, int tx, int ty) {
                auto start_node = Node(0, 0, 0, -1);
                auto target_node = Node(10, 10, 0, -1);
    
                priority_queue<tuple<double, int>> priority_cost_queue;
                unordered_map<int, Node> open_list;
                unordered_map<int, Node> closed_list;
    
                priority_cost_queue.push({0.0, 0});
    
                while (!priority_cost_queue.empty()) {
                     
                    auto c_index = get<1>(priority_cost_queue.top());
                    auto c_cost = get<0>(priority_cost_queue.top());
                    auto cur_node = open_list[c_index];
                    if (cur_node.x == target_node.x && cur_node.y == target_node.y) {
                        target_node.parent = cur_node.parent;
                        target_node.cost = cur_node.cost;
                        break;
                    }
    
    
                    priority_cost_queue.pop();
                    open_list.erase(c_index);
                    closed_list.insert(pair<int, Node>(c_index, cur_node));
    
                    // expansion cur_node
                    auto motions = getMotions();
                    for (auto motion : motions_) {
                        auto node = Node(cur_node.x + get<0>(motion), cur_node.y + 
                                        get<1>(motion), cur_node.cost + get<2>(motion), c_index);
                        int node_index = getNodeIndex(node);
    
                        auto ptr_close = closed_list.find(node_index);
                        if (ptr_close != closed_list.end()) {
                            continue;
                        }
    
                        auto ptr_open = open_list.find(node_index);
                        if (ptr_open != open_list.end()) {
                            ptr_open->second.cost = node.cost;
                            priority_cost_queue.push({node.cost, node_index});
                        }
                        else if (ptr_open->second.cost > node.cost) {
                            open_list[node_index] = node;
                            priority_cost_queue.push({node.cost, node_index});
                        }
                        
                    }
                }
    
                vector<pair<int, int>> path;
                path.emplace_back(target_node.x, target_node.y);
                auto parent_id = target_node.parent;
                while (1) {
                    path.emplace_back(closed_list[parent_id].x, closed_list[parent_id].y);
                    if (closed_list[parent_id].x == start_node.x && closed_list[parent_id].y == start_node.y) {
                        break;
                    }
                    parent_id = closed_list[parent_id].parent;
                } 
    
                reverse(path.begin(), path.end());
                return path;
            }
    
            int getNodeIndex(Node node);
        private:
            vector<vector<bool>> obstacle_map_;
            int x_wideth_;
            int y_wideth_;
            vector<tuple<int, int, double>> motions_;
    
    };
    

      

  • 相关阅读:
    转载:史上最全|阿里那些牛逼带闪电的开源工具,你知道几个?
    互怼、IPO、雷潮、寒冬,2018 互联网圈的那些事儿
    微信迎来又一次重大改版 7.0 版本
    公众号文章目录
    聊几个与赚钱相关的小事情
    使用docker Registry快速搭建私有镜像仓库
    开源组件ELK日志系统配置与管理
    Mysql MHA高可用集群架构
    强大的开源企业级数据监控利器Lepus安装与配置管理
    关于下载gitbash客户端
  • 原文地址:https://www.cnblogs.com/rulin/p/14028157.html
Copyright © 2011-2022 走看看