标签:oid end include astar public using cond ast 优先
#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_; };
使用优先队列完成了一个Astar搜索算法的c++实现,有时间可以完整的完成一遍
标签:oid end include astar public using cond ast 优先
原文地址:https://www.cnblogs.com/rulin/p/14028157.html