根据这篇博客基于C++实现的A*算法（链表和二叉堆实现）_a*算法是不是必须用到链表?-CSDN博客修改了A*，用优先级队列和unordered_set，但是效果不太好，时间反而增加了，正在探索原因。
#include<vector>
#include<algorithm>
#include<iostream>
#include<queue>
#include<unordered_set>struct Node {Node(int X, int Y, std::shared_ptr<Node> p = nullptr) : x(X), y(Y), prev(p) {}int x; //点的x坐标int y; //点的y坐标int G = 0; //起点到该点的欧拉距离int H = 0; //该点到终点的曼哈顿距离int F = 0; //G+Hstd::shared_ptr<Node> prev; //指向的前一个节点bool operator<(const Node& other_node) const { return this->F > other_node.F; }
};class AStar {
public:AStar(std::vector<std::vector<int> > m): maps_(m) {}std::shared_ptr<Node> FindPath(std::shared_ptr<Node> begin, std::shared_ptr<Node> end);void PrintAStarPath(const std::pair<int, int> &, const std::pair<int, int> &);~AStar() {open_set_.clear();close_set_.clear();}private:void RefreshOpenList(std::shared_ptr<Node>, std::shared_ptr<Node> end);int CalculateH(std::shared_ptr<Node>, std::shared_ptr<Node>) const;int CalculateF(std::shared_ptr<Node>,std::shared_ptr<Node>) const;private:std::vector<std::vector<int> > maps_; //地图std::unordered_set<std::shared_ptr<Node>> open_set_; //保存还未遍历过的节点std::priority_queue<Node> open_queue_; //使用优先级队列std::unordered_set<std::shared_ptr<Node> > close_set_; //保存已经找到最短路径的节点const static int cost_low_; //上下位移的距离const static int cost_high_; //斜向位移的距离
};const int AStar::cost_low_ = 10;
const int AStar::cost_high_ = 14;
int AStar::CalculateH(std::shared_ptr<Node> point, std::shared_ptr<Node> end) const {return cost_low_ * (std::abs(point->x - end->x) + std::abs(point->y - end->y));
}int AStar::CalculateF(std::shared_ptr<Node> point,std::shared_ptr<Node> end) const {return point->G + CalculateH(point, end);
}std::shared_ptr<Node> AStar::FindPath(std::shared_ptr<Node> begin, std::shared_ptr<Node> end) {open_set_.emplace(begin);open_queue_.emplace(*begin);RefreshOpenList(begin, end);while (!open_set_.empty()) {auto iter = std::make_shared<Node>(open_queue_.top().x, open_queue_.top().y, open_queue_.top().prev);close_set_.emplace(iter);std::shared_ptr<Node> iter_temp = iter;open_set_.erase(iter);open_queue_.pop();RefreshOpenList(iter_temp, end);auto iter2 = std::find_if(open_set_.cbegin(), open_set_.cend(), [end](std::shared_ptr<Node> sp){ return (sp->x == end->x) && (sp->y == end->y); });if (iter2 != open_set_.end())return *iter2;}return nullptr;
}
void AStar::RefreshOpenList(std::shared_ptr<Node> point, std::shared_ptr<Node> end) {bool upIsWall = false; //表示当前点上有障碍物，即对应的斜向没法走bool downIsWall = false;bool leftIsWall = false;bool rightIsWall = false;if (point->x - 1 >= 0 && maps_[point->x - 1][point->y] == 1)upIsWall = true;if (point->x + 1 < int(maps_.size()) && maps_[point->x + 1][point->y] == 1)downIsWall = true;if (point->y - 1 >= 0 && maps_[point->x][point->y - 1] == 1)leftIsWall = true;if (point->y + 1 < int(maps_.front().size()) && maps_[point->x][point->y + 1] == 1)rightIsWall = true;for (int i = point->x - 1; i <= point->x + 1; ++i) {for (int j = point->y - 1; j <= point->y + 1; ++j) {if (i >= 0 && j >= 0 && i < int(maps_.size()) && j < int(maps_.front().size()) && (i != point->x || j != point->y) && !maps_[i][j]) {if (i != point->x && j != point->y) {if (leftIsWall && j < point->y)continue;if (rightIsWall && j > point->y)continue;if (upIsWall && i < point->x)continue;if (downIsWall && i > point->x)continue; }auto cur = std::make_shared<Node>(i, j, point);cur->G = ((i != point->x && j != point->y) ? cost_high_ : cost_low_) + point->G;cur->H = CalculateH(cur, end);cur->F = CalculateF(cur, end);auto iter_close = std::find_if(close_set_.cbegin(), close_set_.cend(), [i,j](std::shared_ptr<Node> sp){ return (sp->x == i) && (sp->y == j); });if (iter_close == close_set_.end()) {auto iter_open = std::find_if(open_set_.cbegin(), open_set_.cend(), [i,j](std::shared_ptr<Node> sp){ return (sp->x == i) && (sp->y == j); });if (iter_open != open_set_.end()) {if((*iter_open)->G > cur->G) {(*iter_open)->G = cur->G;(*iter_open)->F = (*iter_open)->G + (*iter_open)->H;(*iter_open)->prev = point;}}elseopen_set_.emplace(cur); open_queue_.emplace(*cur);}}}}
}void AStar::PrintAStarPath(const std::pair<int, int>& start, const std::pair<int, int>& end) {auto start_sp = std::make_shared<Node>(start.first, start.second), end_sp = std::make_shared<Node>(end.first, end.second);std::shared_ptr<Node> final = FindPath(start_sp, end_sp);if (!final)std::cout << "没有找到起点到终点路径" << std::endl;else {while (final) {maps_[final->x][final->y] = '*';final = final->prev;}for (const auto &i : maps_) {for (const auto &j : i) {if (j > 1)std::cout << char(j) << " ";elsestd::cout << j << " ";}std::cout << std::endl;}}
}int main() {// 记录开始时间 auto start = std::chrono::high_resolution_clock::now(); std::vector<std::vector<int>> map = {{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},{1, 0, 0, 1, 1, 0, 1, 0, 0, 0, 0, 1},{1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},{1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 1, 1},{1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 1},{1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1},{1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1},{1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}};AStar star(map);star.PrintAStarPath({1, 1}, {6, 10});// 记录结束时间 auto end = std::chrono::high_resolution_clock::now(); // 计算运行时间 std::chrono::duration<double> diff = end-start; std::cout << "Elapsed time: " << diff.count() << " s\n"; return 0;
}
