dl_astar.h

浏览该文件的文档.

 
#pragma once
 
#include <array>
#include <vector>
#include <map>
#include <list>
#include <algorithm>
#include <cassert>
 
#include "io/dl_log.h"
#include "base/dl_string.h"
 
namespace dl
{
 
 
    constexpr bool ASTAR_SHOW_LOG = false;

template<typename T, typename COST>
class AStar
{
public:
    // 点类型
    using AStarPoint = T;
    // 代价类型（float、int等）
    using AStarCost = COST;
    using FuncCost = std::function<AStarCost(const AStarPoint&, const AStarPoint&)>;
    using FuncLink = std::function<void(AStar*, const AStarPoint&)>;
 
    //节点
    class AStarNode
    {
    public:
        AStarPoint _xy;
        AStarNode* _parent; //父节点（为空代表起点）
        AStarCost _cost;  //前往这个节点 的代价值
        AStarCost _total;    //总代价 （cost + 估计代价）
        bool _open;            //是否在 Open表
        bool _closed;        //是否在 Close表
    };
 
    inline static bool AStarNodeCompare(AStarNode* n1, AStarNode* n2)
    {
        return n1->_total > n2->_total;
    }
 
    //优先队列
    class AStarPriorityQueue
    {
    public:
        AStarNode* Pop()
        {
            AStarNode* node = _heap.front();
            std::pop_heap(_heap.begin(), _heap.end(), AStarNodeCompare);
            _heap.pop_back();
            return node;
        }
        void Push(AStarNode* node)
        {
            _heap.push_back(node);
            std::push_heap(_heap.begin(), _heap.end(), AStarNodeCompare);
        }
        void UpdateNode(AStarNode* node)
        {
            for (auto iter = _heap.begin(); iter != _heap.end(); ++iter)
            {
                if ((*iter)->_xy == node->_xy)
                {
                    std::push_heap(_heap.begin(), iter + 1, AStarNodeCompare);
                    return;
                }
            }
        }
        bool IsEmpty()
        {
            return _heap.empty();
        }
        void Clear()
        {
            _heap.clear();
        }
    private:
        std::vector<AStarNode*> _heap;
    };
 
    inline AStarPoint AbsPoint(const AStarPoint& a, const AStarPoint& b)
    {
        AStarPoint ret{
            a.x < b.x ? (b.x - a.x) : (a.x - b.x),
            a.y < b.y ? (b.y - a.y) : (a.y - b.y)
        };
        return ret;
    }

    inline AStarCost AStarGetH(const AStarPoint& p0, const AStarPoint& p1)
    {
        constexpr AStarCost ASTAR_K0 = 10;
        constexpr AStarCost ASTAR_K1 = 14;
 
        AStarPoint d = AbsPoint(p0, p1);
        return ASTAR_K0 * (d.x + d.y) - (std::min)(d.x, d.y) * (2 * ASTAR_K0 - ASTAR_K1);
    }
 

    AStar(
        const AStarPoint& size,
        FuncCost func_g,
        FuncCost func_h = nullptr,
        FuncLink func_link = nullptr,
        size_t max_node = 10000) :
        _funcG{ func_g },
        _funcH{ func_h },
        _funcLink{ func_link },
        _maxNode{ max_node },
        _bDiagonal{ true },
        _size{}
    {
        _arrNodeBank = new AStarNode[_maxNode];
        _masterNode.reserve(_maxNode);
        assert(_funcG);
        if (_funcH == nullptr)
        {
            _funcH = std::bind(&AStar::AStarGetH, this, std::placeholders::_1, std::placeholders::_2);
        }
    }
 
    void SetSize(const AStarPoint& size)
    {
        _size = size;
    }

    bool Search(const AStarPoint& source, const AStarPoint& target, bool target_near,
        std::list<AStarPoint>& list_path,
        std::list<AStarPoint>* fail_path = nullptr)
    {
        if (ASTAR_SHOW_LOG)
            log_msg("{} -> {}", source, target);
        //清空
        _bankCur = 0;
        _masterNode.clear();
        _queueOpen.Clear();
        //起点插入Open表
        AStarNode* node_start = _get_node(source);
        node_start->_open = true;
        node_start->_closed = false;
        node_start->_cost = 0;
        node_start->_total = _funcH(source, target);
        node_start->_parent = NULL;
        _queueOpen.Push(node_start);
 
        _nodeBest = nullptr;
        _xyTarget = target;
        while (!_queueOpen.IsEmpty())
        {
            //取出 消耗值 最小的节点
            _nodeBest = _queueOpen.Pop();
            if (ASTAR_SHOW_LOG)
                log_info("当前最佳节点为：{}", _nodeBest->_xy);
            //是目标节点
            if (_nodeBest->_xy == target
                || (target_near && _is_link_node(_nodeBest->_xy, target)))
            {
                //构造路径（包含首尾节点）
                list_path.clear();
                while (_nodeBest)
                {
                    list_path.push_front(_nodeBest->_xy);
                    _nodeBest = _nodeBest->_parent;
                }
                return true;
            }
 
            //由于_check_connecting 会增加节点，所以提前判断，并预留一些
            if (_bankCur + 32 >= _maxNode)
            {
                if (fail_path)
                {
                    //构造路径（包含首尾节点）  
                    fail_path->clear();
                    while (_nodeBest)
                    {
                        fail_path->push_front(_nodeBest->_xy);
                        _nodeBest = _nodeBest->_parent;
                    }
                }
                return false;
            }
            //图的连接
            if (_funcLink)
                _funcLink(this, _nodeBest->_xy);
            else
                _link_node(_nodeBest->_xy);
 
            //
            _nodeBest->_closed = true;
        }
 
        if (fail_path)
        {
            //构造路径（包含首尾节点）  
            while (_nodeBest)
            {
                fail_path->push_front(_nodeBest->_xy);
                _nodeBest = _nodeBest->_parent;
            }
        }
        return false;
    }
 

    void AddNode(const AStarPoint& xy, AStarCost k_cost)
    {
        if (_nodeBest->_parent == nullptr ||
            _nodeBest->_parent->_xy != xy)
        {
            // 代价
            AStarCost g = _funcG(_nodeBest->_xy, xy);
            // 如果代价 等于0，说明此节点不可通行
            if (g == 0)
                return;
 
            AStarNode* actual_node = _get_node(xy);
 
            // 不在close表
            if (actual_node->_closed == false)
            {
                if (actual_node->_open)
                {
                    // 上一个点代价 + 移到自己的代价 
                    AStarCost new_cost = _nodeBest->_cost + g * k_cost;
                    // 总代价，自己代价 + 估计代价
                    AStarCost new_total = new_cost + _funcH(xy, _xyTarget);
 
                    //新的 代价更小 才刷新
                    if (new_total < actual_node->_total)
                    {
                        if (ASTAR_SHOW_LOG)
                            log_info("更新节点：{}，{}->{}", actual_node->_xy, actual_node->_cost, new_cost);
 
                        actual_node->_parent = _nodeBest;
                        actual_node->_cost = new_cost;
                        actual_node->_total = new_total;
                        _queueOpen.UpdateNode(actual_node);
                    }
                }
                else
                {//不在open表，则加入
                    actual_node->_parent = _nodeBest;
                    actual_node->_cost = _nodeBest->_cost + g * k_cost;
                    actual_node->_total = actual_node->_cost + _funcH(xy, _xyTarget);
 
                    _queueOpen.Push(actual_node);
                    actual_node->_open = true;
 
                    if (ASTAR_SHOW_LOG)
                        log_info("增加节点：{}，{}", actual_node->_xy, actual_node->_cost);
                }
            }
        }
    }

    void SetLinkDiagonal(bool diagonal)
    {
        assert(_funcLink == nullptr);
        _bDiagonal = diagonal;
    }

    void SetFuncLink(FuncLink func_link)
    {
        _funcLink = func_link;
    }
 
 
    ~AStar()
    {
        delete[] _arrNodeBank;
    }
private:
    AStarNode* _get_node(const AStarPoint& xy)
    {
        //如果在 主列表 直接返回，如果没有则从节点银行构造一个
        uint64_t index = ((uint64_t)xy.y << 32) | (uint64_t)xy.x;
 
        AStarNode*& node = _masterNode[index];
 
        if (node)
        {
            return node;
        }
        else
        {
            //此处会越界
            if (_bankCur >= _maxNode)
            {
                //cout << "寻路的节点超过上限！" <<;
                return nullptr;
            }
 
            node = &_arrNodeBank[_bankCur++];
            node->_xy = xy;
            node->_open = false;
            node->_closed = false;
            return node;
        }
    }
    void _link_node(const AStarPoint& p)
    {
        constexpr AStarCost ASTAR_K0 = 10;
        constexpr AStarCost ASTAR_K1 = 14;
 
        if (_size.x == 0)
        {
            AddNode({ p.x + 1, p.y }, ASTAR_K0);
            AddNode({ p.x - 1, p.y }, ASTAR_K0);
            AddNode({ p.x, p.y + 1 }, ASTAR_K0);
            AddNode({ p.x, p.y - 1 }, ASTAR_K0);
            if (_bDiagonal)
            {
                AddNode({ p.x + 1, p.y + 1 }, ASTAR_K1);
                AddNode({ p.x + 1, p.y - 1 }, ASTAR_K1);
                AddNode({ p.x - 1, p.y - 1 }, ASTAR_K1);
                AddNode({ p.x - 1, p.y + 1 }, ASTAR_K1);
            }
        }
        else
        {
            if (p.x + 1 < _size.w)
                AddNode({ p.x + 1, p.y }, ASTAR_K0);
            if (p.x > 0)
                AddNode({ p.x - 1, p.y }, ASTAR_K0);
            if (p.y + 1 < _size.h)
                AddNode({ p.x, p.y + 1 }, ASTAR_K0);
            if (p.y > 0)
                AddNode({ p.x, p.y - 1}, ASTAR_K0);
                
            if (_bDiagonal)
            {
                if (p.x + 1 < _size.w && p.y + 1 < _size.h)
                    AddNode({ p.x + 1, p.y + 1 }, ASTAR_K1);
                if (p.x + 1 < _size.w && p.y > 0)
                    AddNode({ p.x + 1, p.y - 1 }, ASTAR_K1);
                if (p.x > 0 && p.y > 0)
                    AddNode({ p.x - 1, p.y - 1 }, ASTAR_K1);
                if (p.x > 0 && p.y + 1 < _size.h)
                    AddNode({ p.x - 1, p.y + 1 }, ASTAR_K1);
            }
        }
    }
    bool _is_link_node(const AStarPoint& p0, const AStarPoint& p1)
    {
        AStarPoint p = AbsPoint(p0, p1);
        if (_bDiagonal)
        {
            return p.x <= 1 && p.y <= 1;
        }
        else
            return p.x + p.y <= 1;
    }
 
    // 主列表{y, x}
    std::unordered_map<uint64_t, AStarNode*>_masterNode;
    // Open表
    AStarPriorityQueue _queueOpen;
    // 节点银行
    AStarNode* _arrNodeBank;
    // 每次搜索前置0
    size_t _bankCur;
    // 遍历节点上限（也是节点银行大小）
    size_t _maxNode;
 
    // 是否可斜穿
    bool _bDiagonal;
    // 不为0则检查越界
    AStarPoint _size;
 
    //回调函数
    FuncCost _funcG;
    FuncCost _funcH;
    FuncLink _funcLink;
 
    //临时缓存
    AStarNode* _nodeBest;
    AStarPoint _xyTarget;
};
}