AStar.h 6.24 KB
Newer Older
oscar's avatar
oscar committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149
#ifndef _JFXMAP_ASTAR_H_
#define _JFXMAP_ASTAR_H_

// 参考:原文链接:https://blog.csdn.net/u012234115/article/details/47152137
#include <algorithm>
#include <list>
#include <memory>
#include <math.h>

#include "HdLib.h"

namespace jf {

    struct LinkNode;
    typedef std::shared_ptr<LinkNode> ShPtr_LinkNode;

    struct LinkNode {
        LinkNode(LinkEndpoint _stnode): leNode(_stnode),G(0),H(0),F(0),splnParent(nullptr){}
        LinkNode(): leNode(LinkEndpoint()),G(0),H(0),F(0),splnParent(nullptr) {}

        LinkEndpoint leNode;        // 节点(取车道的终点)
        double G;                   // G 表示从上一点移动到路网上指定点的移动耗费
        double H;                   // H 表示从指定点移动到目的点的预计耗费
        double F;                   // F=G+H
        ShPtr_LinkNode splnParent;  // parent的指针
    };

    struct LaneNode;
    typedef std::shared_ptr<LaneNode> ShPtr_LaneNode;

    struct LaneNode {
        LaneEndpoint leNode;        // 节点(取车道的终点)
        double G;                   // G 表示从上一点移动到路网上指定点的移动耗费
        double H;                   // H 表示从指定点移动到目的点的预计耗费
        double F;                   // F=G+H
        ShPtr_LaneNode splnParent;  // parent的指针
        LaneNode(LaneEndpoint _stnode): leNode(_stnode),G(0),H(0),F(0),splnParent(nullptr){}
        LaneNode():leNode(LaneEndpoint()),G(0),H(0),F(0),splnParent(nullptr){}
    };


    class AStar {
    public:
        AStar();

    public:
        /**
         * @brief 道路Link级别寻路
         * @param leStartNode:起点
         * @param vctleEndNode:终点集
         * @param vctleOutLinkPath:寻路结果
         * @param leOutEndNode:该次寻路使用的终点
         * @param nMaxSteps:最大寻路步骤
         * @param bHighPrecision:是否将终点集中的终点按照距离优先使用
         * @return 寻路是否成功
         */
        bool GetPath(const LinkEndpoint &leStartNode, const std::vector<LinkEndpoint> &vctleEndNode,
                     std::vector<LinkEndpoint> &vctleOutLinkPath, LinkEndpoint &leOutEndNode, int nMaxSteps = 65535,
                     bool bHighPrecision = false);

        /**
         * @brief 车道Lane级别寻路
         * @param leStartNode:起点
         * @param vctleEndNode:终点集
         * @param vctleOutLanePath:寻路结果
         * @param leOutEndNode:该次寻路使用的终点
         * @param nMaxSteps:最大寻路步骤
         * @param bHighPrecision:是否将终点集中的终点按照距离优先使用
         * @param bLastLinkChangeLane:是否允许终点的link变道
         * @return 寻路是否成功
         */
        bool GetPath(const LaneEndpoint &leStartNode, const std::vector<LaneEndpoint> &vctleEndNode,
                     std::vector<LaneEndpoint> &vctleOutLanePath, LaneEndpoint &leOutEndNode, int nMaxSteps = 65535,
                     bool bHighPrecision = false, bool bLastLinkChangeLane = false);

        /**
         * @brief 道路Link级别寻路
         * @param leStartNode:起点
         * @param leEndNode:终点
         * @param vctleOutLinkPath:寻路结果
         * @param nMaxSteps:最大寻路步骤
         * @return 寻路是否成功
         */
        bool GetPath(const LinkEndpoint &leStartNode, const LinkEndpoint &leEndNode,
                     std::vector<LinkEndpoint> &vctleOutLinkPath, int nMaxSteps = 65535);

        /**
         * @brief 车道Lane级别寻路
         * @param leStartNode:起点
         * @param leEndNode:终点
         * @param vctleOutLanePath:寻路结果
         * @param nMaxSteps:最大寻路步骤
         * @return 寻路是否成功
         */
        bool GetPath(const LaneEndpoint &leStartNode, const LaneEndpoint &leEndNode,
                     std::vector<LaneEndpoint> &vctleOutLanePath, int nMaxSteps = 65535);

    private:
        // 道路级别寻路
        bool FindLinkPath(const ShPtr_LinkNode &splnStartNode, const std::vector<ShPtr_LinkNode> &vctslEndNode,
                          ShPtr_LinkNode &splnResult, LinkEndpoint &leOutEndNode, int nMaxSteps);

        // 车道级别寻路
        bool FindLanePath(const ShPtr_LaneNode &splnStartNode, const std::vector<ShPtr_LaneNode> &vctslEndNode,
                          const std::vector<LinkEndpoint> &vctleLinkPath, ShPtr_LaneNode &splnResult,
                          LaneEndpoint &leOutEndNode, int nMaxSteps);

        //从开启列表中返回F值最小的节点
        bool GetLeastFNode(ShPtr_LinkNode &splnNode);

        //从开启列表中返回F值最小的节点
        bool GetLeastFNode(ShPtr_LaneNode &splnNode);

        // 根据当前节点,获取下一个节点
        bool GetNextNodes(const ShPtr_LinkNode &splnCurrentNode, std::vector<ShPtr_LinkNode> &vctslNextNodes);

        // 根据当前节点,获取下一个节点
        bool GetNextNodes(const ShPtr_LaneNode &splnCurrentNode, const std::vector<LinkEndpoint> &vctleLinkPath,
                          std::vector<ShPtr_LaneNode> &vctslNextNodes);

        //判断开启/关闭列表中是否包含某点
        ShPtr_LinkNode IsInList(const std::list<ShPtr_LinkNode> &lstspNodes, const ShPtr_LinkNode &splnNode);

        //判断开启/关闭列表中是否包含某点
        ShPtr_LaneNode IsInList(const std::list<ShPtr_LaneNode> &lstspNodes, const ShPtr_LaneNode &splnNode);

        //计算FGH值
        double CalcG(const ShPtr_LinkNode &spStart, const ShPtr_LinkNode &splnNode);

        double CalcG(const ShPtr_LaneNode &spStart, const ShPtr_LaneNode &splnNode);

        double CalcH(const ShPtr_LinkNode &splnNode, const ShPtr_LinkNode &splnEnd);

        double CalcH(const ShPtr_LaneNode &splnNode, const ShPtr_LaneNode &splnEnd);

        double CalcF(const ShPtr_LinkNode &splnNode);

        double CalcF(const ShPtr_LaneNode &splnNode);

    private:
        std::list<ShPtr_LinkNode> m_lstslOpenLinkNodes;   //开放列表
        std::list<ShPtr_LaneNode> m_lstslOpenLaneNodes;   //开放列表
        std::list<ShPtr_LinkNode> m_lstslCloseLinkNodes;  //关闭列表
        std::list<ShPtr_LaneNode> m_lstslCloseLaneNodes;  //关闭列表

    };
}  // namespace jf

#endif  // _JFXMAP_ASTAR_H_