手撕A*（效果不太好）

最新推荐文章于 2024-11-15 20:16:11 发布

爱学习的fzx

最新推荐文章于 2024-11-15 20:16:11 发布

阅读量311

点赞数 3

文章标签：算法

本文链接：https://blog.csdn.net/fzx12138/article/details/136994794

版权

本文介绍了在C++中使用A*算法进行路径搜索的实现，尝试了使用优先级队列和unordered_set替代链表，但发现性能下降，时间增加，作者正在研究原因。

摘要由CSDN通过智能技术生成

根据这篇博客基于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+H
    std::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;
                        }
                    }
                    else
                        open_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) << " ";
                 else
                     std::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;
}