astar

1. 问题

最短路径可以用djikstra做，那中间有障碍物怎么办，astar就是解决这个问题的，例如下面的图片,绿点是起点，红点是终点，蓝色是障碍物。

2. 方法

和djkstra很类似，对于图中的任意一个非障碍物的点而言，有两个属性：一个是起点到这个点的距离，一个是这个点到终点的距离。起点到这个点的距离是已知的，因为外面是从起点附近开始遍历的。这个点到终点的距离就不好办了。并不能直接用两个点之间的距离来衡量，因为这两个点之间可能有墙啊，有的人用曼哈顿距离（两点的横纵坐标差之和），但是这个也不准，因为这个值很有可能比实际值要大。难点就在于如何评价这个点到终点的距离，有三个指标，前两个没记住。。。第三个是这距离不能大于实际可能走的距离。曼哈顿距离肯定就不对了，那用两点之间的距离呢？答案是可以做出来的，比如下面的代码用的就是这个。这样肯定不会错，但是可能有问题，就是计算速度，这个先不说。

对于每个点而言，all_cost就是这个点到两边点的距离和，用这个值在优先队列中排序，贪心找到每次消耗最小的点。

下面代码有点改动，我之前没用优先队列，因为当到了一个点后如果更新了已经存在的点，优先队列是不会自动重排的，所以之前djkstra用的list，每次改了节点就修改改掉的，取出的时候sort，这样复杂度会很高。现在不管是什么，全部push进去，哪怕这个点已经优先队列中存过了，我也继续push，因为pop的时候，消耗大的值肯定后弹出，为了避免两个点重复利用，弹出的时候用哈希记录一下，就是说原来是选择性插入，弹出不限制，改成了插入无限制，弹出有过滤。

这么看这个方法是很完美的啊，然而并不是，优化点也在每个点到终点的消耗预估上。这样做速度会变慢，以图中为例，左上角是(0,0)点，起点是(2，2)，在找的时候，找完(1.3)后还会去找(3,3)，可是我们知道，这两个点距离肯定是相同的，这种重复的点会找很多次，这就是问题，有的算法可以在这个问题上进行优化。

3. 代码

#include<iostream>
#include<vector>
#include<algorithm>
#include<cmath>
#include<list>
#include<unordered_map>
#include<unordered_set>
#include<functional>
#include<queue>
void install_map(std::vector<std::vector<bool> >& a)
{
    a.at(1).at(4) = false;
    a.at(2).at(4) = false;
    a.at(3).at(4) = false;
}

class pos
{
    public:    
        size_t line_;
        size_t row_;
        pos(int line, int row):line_(line), row_(row){};
        bool operator == (const pos pos1)const
        {
            return line_==pos1.line_ && row_==pos1.row_;
        }
};

class node
{
    public:
        node* p_parent_;
        double costed_;
        double to_cost_;
        double all_cost_;
        pos pos_;
        static pos start_pos_;
        static pos end_pos_;
        
        node(node* p_parent, pos pos1, int cost_from_parent):p_parent_(p_parent),pos_(pos1)
        {
            costed_ = nullptr==p_parent ? 0 : p_parent->costed_ + cost_from_parent;
            to_cost_ = get_distance(pos_, end_pos_);
            all_cost_ = costed_ + to_cost_;
        }
        bool operator == (const node & node1) const
        {
            return pos_ == node1.pos_;
        }
        static void pos_init(const int x1, const int y1, const int x2, const int y2);
    private:
        double get_distance(const pos & pos1, const pos & pos2)
        {
            auto delta_y = pos1.row_ - pos2.row_;
            auto delta_x = pos1.line_ - pos2.line_;
            return 10.0*sqrt((double)(delta_x*delta_x) + (double)(delta_y*delta_y));
        }
};

void node::pos_init(const int line1, const int row1, const int line2, const int row2)
{
    start_pos_ = pos(line1, row1);
    end_pos_ = pos(line2, row2);
}
pos node::start_pos_ = pos(0, 0);
pos node::end_pos_ = pos(0, 0);


auto cmp = [](node* node1, node* node2){return node1->costed_ > node2->costed_;};

void change_status(std::vector<std::vector<bool> >&block_map, std::priority_queue<node*, std::vector<node*>, decltype(cmp)>&open, std::list<node*>&all_node, size_t x, size_t y, double add_value, node* p_parent)
{
    if(x<block_map.size() && x>=0 && y<block_map[0].size() && y>=0 && block_map.at(x).at(y))
    {
        node* p_node_tmp = new node(p_parent, pos(x, y), add_value);
        open.push(p_node_tmp);
        all_node.push_back(p_node_tmp);
    }
}

int main()
{
    node::pos_init(2, 2, 2, 5);
    std::vector<std::vector<bool> >block_map(6, std::vector<bool>(8, true));
    install_map(block_map);

    std::priority_queue<node*, std::vector<node*>, decltype(cmp)>open(cmp);
    std::list<node*>all_node;
    auto hash_func = [](const pos & pos1){return std::hash<int>()(pos1.line_) ^ std::hash<int>()(pos1.row_);};
    std::unordered_set<pos, decltype(hash_func)>used_pos(1, hash_func);
    node* p_out_node = nullptr;

    open.push(new node(nullptr, node::start_pos_, 0));///<加入起点
    all_node.push_back(open.top());
    while(!open.empty())
    {
        auto node_ele = open.top();
        open.pop();
        if(used_pos.find(node_ele->pos_) != used_pos.end())
            continue;
        used_pos.insert(node_ele->pos_);
    
        if(node_ele->pos_ == node::end_pos_)///<找完了
        {
            p_out_node = node_ele;
            break;
        }
        change_status(block_map, open, all_node, node_ele->pos_.line_  , node_ele->pos_.row_+1, 10.0, node_ele); ///<down
        change_status(block_map, open, all_node, node_ele->pos_.line_+1, node_ele->pos_.row_+1, 14.0, node_ele); ///<right down
        change_status(block_map, open, all_node, node_ele->pos_.line_+1, node_ele->pos_.row_  , 10.0, node_ele); ///<right
        change_status(block_map, open, all_node, node_ele->pos_.line_+1, node_ele->pos_.row_-1, 10.0, node_ele); ///<right up
        change_status(block_map, open, all_node, node_ele->pos_.line_  , node_ele->pos_.row_-1, 10.0, node_ele); ///<up
        change_status(block_map, open, all_node, node_ele->pos_.line_-1, node_ele->pos_.row_-1, 10.0, node_ele); ///<left up
        change_status(block_map, open, all_node, node_ele->pos_.line_-1, node_ele->pos_.row_  , 10.0, node_ele); ///<left
        change_status(block_map, open, all_node, node_ele->pos_.line_-1, node_ele->pos_.row_+1, 10.0, node_ele); ///<left down
    }
    std::cout<<"path line:"<<std::endl;
    while(nullptr != p_out_node)
    {
        std::cout<<"line:"<<p_out_node->pos_.line_<<",row:"<<p_out_node->pos_.row_<<std::endl;
        p_out_node = p_out_node->p_parent_;
    }
    for(auto list_node : all_node)
        delete(list_node);
    return 0;
}