[原创]南水之源A*（A-Star)算法

开发导航之前我看了一些A*（A-Star)算法的例子和讲解。没有求得甚解！不过也从A*（A-Star)算法中得到启发，写了一套自己的A*（A-Star)算法。当然，这不是真正（我也不知道）的A*（A-Star)算法，大家也只当这是一个傻瓜化的A*（A-Star)吧。

来点废话：首先，我得到一个要导航的任务，没有路网数据结构，没有算法要求，就是要自己完成一套路网数据(自己采集gps信息)，并进行导航。

考虑到要采集数据所以，先要有采集出来数据的数据结构，数据结构又是根据算法需要来的，所以，我先打算把导航算法搞出来（可能我小时候被狗追过，改不了的我这思路）。所以，在没有数据支持和数据结构以及我连A*（A-Star)都看懂了一般的情况下，我准备开始写代码了，下面我把我A*（A-Star)的思路小结一下给大家看看，并且自己也整理下我这不堪的思路。

整体想法：

导航是从一个点到另一个点所要经过的路径，所以将所有的路都变成点信息，一个点需要包含它所能连接到的其他点的信息以及到其他点的长度····

·····

一个星期后···

╮(╯▽╰)╭代码写完了，想法过程难得写了···

直接上代码吧

/*
Point.h
*/

#pragma once

#include <map>
#include <vector>

typedef unsigned int uint;

class Point
{
public:
    double    x;            //列
    double    y;            //行
public:

    //构造函数，接受行和列的初始化
    Point(double x = 0.0, double y = 0.0): x(x), y(y)
    {
        return;
    }

    //赋值操作
    Point& operator=(const Point& pt)
    {
        x = pt.x;
        y = pt.y;
        return *this;
    }

    //比较操作
    bool operator==(const Point& pt)
    {
        return x == pt.x && y == pt.y;
    }

    double operator-(const Point& pt)
    {
        return sqrt((x - pt.x)*(x - pt.x) + (y - pt.y)*(y - pt.y));
    }

};

/*
RoadPoint.h
*/

#pragma once
#include "Point.h"
#include <queue>
#include <stack>
#include <algorithm>


typedef std::stack<uint> ROADSTACK;
//结合成路的点
/*
    两个RoadPoint  结合  RoadLine  就是一条路，以及路两边的点。
    每个点可以连接到多条RoadLine，一条RoadLine两边只有两个点。
    每个点都有自己特别的Identify标识
*/
// stack::empty  

class RoadLine
{
    //两个点的标识
    uint m_identify1;
    uint m_identify2;
    double m_length;
public:
    RoadLine(uint r1, uint r2, double length):m_identify1(r1),m_identify2(r2),m_length(length)
    {

    }

    uint getOther(uint identify)
    {
        if (identify == m_identify1)
        {
            return m_identify2;
        }
        return m_identify1;
    }

    double getLength()
    {
        return m_length;
    }
};


class RoadPoint
{
    uint m_identify;
    Point m_point;
    double m_lengthFromDest;
    int m_numOfRoads;
    std::vector<RoadLine> m_vecRL;
public:

    RoadPoint(uint id, double x, double y, int num):m_identify(id),m_numOfRoads(num)
    {
        m_point.x = x;
        m_point.y = y;
        m_lengthFromDest = 0;
    }

    RoadPoint(uint id, Point point, int num):m_identify(id),m_point(point),m_numOfRoads(num)
    {
        m_lengthFromDest = 0;
    }

    RoadPoint(uint id, Point point, int num, Point dest):m_identify(id),m_point(point),m_numOfRoads(num)
    {
        m_lengthFromDest = point - dest;//重载过运算符
    }

    uint id()
    {
        return m_identify;
    }
    
    //返回当前第num个路径连接的点的 id ，没有则返回0;
    uint idFromRoadLine(int num, double &length)
    {
        if (num < m_vecRL.size())
        {
            length = m_vecRL.at(num).getLength();
            return m_vecRL.at(num).getOther(id());
        }
        return 0;
    }

    //返回当前第num个路径连接road长度
    double lengthFromRoadLine(int num)
    {
        if (num < m_vecRL.size())
        {
            return m_vecRL.at(num).getOther(id());
        }
        return 0;
    }

    void setDest(double x, double y)
    {
        Point dp(x,y);
        m_lengthFromDest = m_point - dp;
    }


    void connect(RoadLine rl, bool isNew)
    {
        if(isNew)
        {
            m_numOfRoads++;
        }

        m_vecRL.push_back(rl);
    }

    int getNumOfRoads()
    {
        return m_numOfRoads;
    }

    bool operator ==(const RoadPoint &p)
    {
        return m_identify == p.m_identify;
    }
    
    double getLengthFromDest()
    {
        return m_lengthFromDest;
    }


    //当前点到目的地点的距离
    double getLengthFromDest(RoadPoint p)
    {
        return m_lengthFromDest = m_point - p.m_point;
    }


};






class RoadPointManager
{
private:
    RoadPointManager();

    static RoadPointManager *singleIntance;

    typedef std::map<uint,RoadPoint> ROADMAP;
    //储存所有路点的map
    ROADMAP m_roadNet;

    //储存路径的stack
    ROADSTACK m_roadStack;

    typedef std::map<double ,RoadPoint> ROADFLAG;
    typedef std::pair<double ,RoadPoint> ROADFLAG_P;

    //不可访问点数组（方便查找，添加，删除）
    std::vector<uint> m_UnuseRoadPoints;  

    double m_legnthOfRoad;
    double g_totalLegnth;

public:
    static RoadPointManager* SigngleInt();

    /*
    uint id,    点的标识
    float x,    点的x位置
    float y,    点的y位置
    int num,    点连接其他点的数量
    bool isGetDest = false,        是否附加上目标点的信息，一般不加，只是留个可选项
    float destX = 0.0,            由isGetDest控制是否使用，代表目标点的x位置。一般不加，只是留个可选项
    float destY = 0.0            由isGetDest控制是否使用，代表目标点的y位置。一般不加，只是留个可选项
    */
    void addRoadPoint(uint id, double x, double y, int num = 0,bool isGetDest = false, float destX = 0.0, float destY = 0.0)
    {
        RoadPoint p(id,x,y,num);
        if(isGetDest)
        {
            p.setDest(destX, destY);
        }
        m_roadNet.insert(std::pair<uint,RoadPoint>(id, p));
    }

    /*
    uint r1,    需要连接的两个点的r1点标识
    uint r2,    需要连接的两个点的r2点标识
    bool isNewAdd = true    是不是之前没有的连接，即r1和r2都不知道自己会连上对方，所以自己的num需要+1的情况
    */
    bool addRoadLine(uint r1, uint r2, bool isNewAdd = true)//如果没有r1 或者r2就返回false
    {
        ROADMAP::iterator it1 = m_roadNet.find(r1);
        ROADMAP::iterator it2 = m_roadNet.find(r2);
        if(it1 == m_roadNet.end()|| it2 == m_roadNet.end())
        {
            //找不到其中一个的 RoadPoint
            return false;
        }
        
        RoadPoint p1 = it1->second;
        RoadPoint p2 = it2->second;

        RoadLine rl(r1, r2, p1.getLengthFromDest(p2));

        //p1.connect(rl,isNewAdd);
        //p2.connect(rl,isNewAdd);

        it1->second.connect(rl,isNewAdd);
        it2->second.connect(rl,isNewAdd);
        return true;
    }

    //一直要找到dest
    ROADSTACK findWay(uint start, uint dest)
    {
        ROADMAP::iterator itstart = m_roadNet.find(start);
        ROADMAP::iterator itdest = m_roadNet.find(dest);
        if(itstart == m_roadNet.end()|| itdest == m_roadNet.end())
        {
            //找不到其中一个的 RoadPoint
            printf("找不到其中的 RoadPoint, 返回无效值!");
            return m_roadStack;
        }

        RoadPoint p1 = itstart->second;
        RoadPoint p2 = itdest->second;

        while (!m_roadStack.empty())
        {
            m_roadStack.pop();
        }
        
        m_UnuseRoadPoints.clear();

        //凡是访问过的点都加入不可访问点
        m_UnuseRoadPoints.push_back(p1.id());
        g_totalLegnth = 0.0;
        //findroad(p1, p2);
        findroad2(p1, p2,m_UnuseRoadPoints,0);
        return m_roadStack;
    }

    double totalLegnth()
    {
        return g_totalLegnth;
    }
private:
    //
    bool findroad(RoadPoint start, RoadPoint dest)
    {
        //遍历p1底下所有的点，如果有p2直接命中跳出递归，其余点做简单的判断，命中并继续，如果已经命中则加入不可被调用连接
        //如果此点下无其他子节点，或者所有节点都已经被加入到不可调用连接，则将此点放入不可被调用连接
        //1.先将所有节点都遍历
        //bl是所有当前start点能连接到的点和此点到dest点的距离
        ROADFLAG roadlist;

        for(int i = 0;i < start.getNumOfRoads();i++)
        {
            //获取当前点能连接到的路点
            double lengthOfRoad;
            uint st_id =  start.idFromRoadLine(i, lengthOfRoad);
            RoadPoint rp = m_roadNet.at(st_id);

            if (rp == dest)
            {
                //如果找到此点，塞入栈中，结束递归
                m_roadStack.push(dest.id());
                m_roadStack.push(start.id());
                return true;
            }
            //查看pa是否在不可访问的UnuseRoadPoints中，如果不在则加入roadlist列表
            if (find(m_UnuseRoadPoints.begin(),m_UnuseRoadPoints.end(),st_id) == m_UnuseRoadPoints.end())
            {
                //加入不可回头访问
                m_UnuseRoadPoints.push_back(st_id);
                //如果不在UnuseRoadPoints中，则可加入roadlist，方便之后检查
                ROADFLAG_P pa(rp.getLengthFromDest(dest) + lengthOfRoad, rp);
                roadlist.insert(pa);
            }
            
        }
        
        //将roadlist中的点，按预测距离长短进行查找，直到找到dest
        //因为map根据key值排序的，所以从begin到end就是距离从短到长的调用
        for(ROADFLAG::iterator it = roadlist.begin(); it != roadlist.end(); it++)
        {
            //如果找到dest
            if(findroad(it->second,dest))
            {
                //将父节点加入路径上
                m_roadStack.push(start.id());
                return true;
            }
        }

        //这个节点下的所有点都不能连接到dest
        return false;

    }

    bool findroad2(RoadPoint start, RoadPoint dest, std::vector<uint> UnusePoints, double hasPassby)
    {
        //遍历p1底下所有的点，如果有p2直接命中跳出递归，其余点做简单的判断，命中并继续，如果已经命中则加入不可被调用连接
        //如果此点下无其他子节点，或者所有节点都已经被加入到不可调用连接，则将此点放入不可被调用连接
        //1.先将所有节点都遍历
        //bl是所有当前start点能连接到的点和此点到dest点的距离
        printf("findroad2：start:%d, dest:%d,  startnum:%d
",start.id(),dest.id(),start.getNumOfRoads());
        ROADFLAG roadlist;
        std::vector<uint> UnuseRoadPoints = UnusePoints;
        for(int i = 0;i < start.getNumOfRoads();i++)
        {
            //获取当前点能连接到的路点
            double lengthOfRoad;
            uint st_id =  start.idFromRoadLine(i, lengthOfRoad);
            RoadPoint rp = m_roadNet.at(st_id);

            if (rp == dest)
            {
                //如果找到更好的stack，则开始清空之前的所有
                while(!m_roadStack.empty())
                {
                    uint flag = m_roadStack.top();
                    printf("路径%d--->",flag);
                    m_roadStack.pop();
                }
                printf("
此路径长度为：%f
",g_totalLegnth);
                g_totalLegnth = 0.0;
                g_totalLegnth += start.getLengthFromDest(dest);
                //如果找到此点，塞入栈中，结束递归
                m_roadStack.push(dest.id());
                m_roadStack.push(start.id());
                return true;
            }

            printf("我是start：%d 下的： %d,不知道自己是否在忽视列表中?
",start.id(),st_id);
            //查看pa是否在不可访问的UnuseRoadPoints中，如果不在则加入roadlist列表
            if (find(UnuseRoadPoints.begin(),UnuseRoadPoints.end(),st_id) == UnuseRoadPoints.end())
            {
                printf("我是start：%d 下的： %d,我不在忽视列表中！
",start.id(),st_id);
                //加入不可回头访问
                UnuseRoadPoints.push_back(st_id);
                //如果不在UnuseRoadPoints中，则可加入roadlist，方便之后检查
                double estimateLength = rp.getLengthFromDest(dest) + lengthOfRoad;

                printf("
将第%d条路加入尝试，此路径预估为：%f,总长为：%f
",st_id,estimateLength,g_totalLegnth);
                if(g_totalLegnth < 0.1 /*还没有发现一条路径*/
                    ||/*或者，如果已经发现路径，但是目前预估的路径比已发现路径更优*/  estimateLength < g_totalLegnth)
                {
                    //因为map的key值用了double，而它做判断的时候很有可能此double离得很近，而map就当做同一个key值了，
                    //所以，要添加此值,需要使key值被识别时不一样！就算这个值不是正确也没事，只是访问时候的排序而已
                    while(roadlist.find(estimateLength) != roadlist.end())
                    {
                        estimateLength -= 1.0;
                    }
                    ROADFLAG_P pa(estimateLength, rp);
                    roadlist.insert(pa);
                }
            }
            
        }
        
        bool isGotTheWay = false;
        //将roadlist中的点，按预测距离长短进行查找，直到找到dest
        //因为map根据key值排序的，所以从begin到end就是距离从短到长的调用
        for(ROADFLAG::iterator it = roadlist.begin(); it != roadlist.end(); it++)
        {
            double estimateLength = hasPassby + it->second.getLengthFromDest(dest) + start.getLengthFromDest(it->second);

            printf("
尝试：%d路，此路径预估为：%f,总长为：%f
",it->second.id(),estimateLength,g_totalLegnth);
            if(g_totalLegnth < 0.1 /*还没有发现一条路径*/
                    ||/*或者，如果已经发现路径，但是目前预估的路径比已发现路径更优*/  estimateLength < g_totalLegnth)
            {
                    //如果找到dest，则返回上一步继续

                printf("进入1：
");
                if(findroad2(it->second,dest,UnuseRoadPoints, hasPassby + start.getLengthFromDest(it->second)))
                {
                        isGotTheWay = true;
                        //将父节点加入路径上
                        m_roadStack.push(start.id());
                        //这里其实应该已经可以跳出了，但是不知道是否还有更优的点所以，之后的还要进行判断
                        //这里不能直接跳出，而应该继续判断下去找出其他出路
                        g_totalLegnth += start.getLengthFromDest(it->second);
                }
            }
        }

        //这个节点下的所有点都不能连接到dest
        return isGotTheWay;

    }
};

/*
RoadPoint.cpp
*/
#include "RoadPoint.h"

RoadPointManager* RoadPointManager::singleIntance = NULL;

RoadPointManager::RoadPointManager()
{}

RoadPointManager* RoadPointManager::SigngleInt()
{
        if (singleIntance == NULL)
        {
            singleIntance  = new RoadPointManager();
        }
        return singleIntance;
}

/*
main.cpp
*/


#include "NaviXML.h"


int main()
{

RoadPointManager::SigngleInt()->addRoadPoint(0,        1,        9,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(1,        2,        3,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(2,        3,        6,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(3,        4,        11,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(4,        4,        3,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(5,        5,        8,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(6,        6,        6,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(7,        7,        11,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(8,        7,        4,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(9,        8,        7,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(10,        8,        5,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(11,        8,        2,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(12,        9,        9,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(13,        10,        6,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(14,        11,        13,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(15,        11,        3,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(16,        13,        10,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(17,        14,        6,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(18,        14,        4,    0);
    RoadPointManager::SigngleInt()->addRoadPoint(19,        16,        8,    0);

    RoadPointManager::SigngleInt()->addRoadLine(    3,        7,        true);
    //RoadPointManager::SigngleInt()->addRoadLine(    7,        16,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    7,        12,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    3,        9,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    7,        9,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    9,        12,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    12,        16,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    16,        13,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    13,        15,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    9,        10,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    3,        5,        true);
    //RoadPointManager::SigngleInt()->addRoadLine(    5,        15,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    6,        10,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    10,        15,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    2,        0,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    0,        3,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    5,        6,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    6,        9,        true);

    RoadPointManager::SigngleInt()->addRoadLine(    2,        8,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    10,        8,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    11,        8,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    4,        8,        true);

    RoadPointManager::SigngleInt()->addRoadLine(    7,        14,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    14,        16,        true);

    RoadPointManager::SigngleInt()->addRoadLine(    1,        2,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    1,        4,        true);

    RoadPointManager::SigngleInt()->addRoadLine(    13,        17,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    17,        19,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    17,        18,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    15,        18,        true);

    //再加一个点
    RoadPointManager::SigngleInt()->addRoadPoint(    20,        4,        8,    0);
    //再加三条线
    RoadPointManager::SigngleInt()->addRoadLine(    0,        20,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    2,        20,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    5,        20,        true);

    RoadPointManager::SigngleInt()->addRoadLine(    11,        13,        true);
    RoadPointManager::SigngleInt()->addRoadLine(    12,        19,        true);

ROADSTACK roadStack = RoadPointManager::SigngleInt()->findWay(9,13);

    printf("
最终：");
    while (!roadStack.empty())
    {
        uint flag = roadStack.top();
        printf("路径%d--->",flag);
        roadStack.pop();
    }
    
    printf("
最终路径长度为：%f
",RoadPointManager::SigngleInt()->totalLegnth());

    printf("

");

    //第二次查找
    ROADSTACK roadStack2 = RoadPointManager::SigngleInt()->findWay(12,2);

    printf("最终：");
    while (!roadStack2.empty())
    {
        uint flag = roadStack2.top();
        printf("路径%d--->",flag);
        roadStack2.pop();
    }
    
    printf("
最终路径长度为：%f
",RoadPointManager::SigngleInt()->totalLegnth());
    
    getchar();
    return 0;


}