开发导航之前我看了一些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 

 

 

 

/*RoadPoint.h*/#pragma once#include "Point.h"#include 
     
      #include 
      
       #include 
       
        typedef std::stack
        
          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
         
           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
          
            ROADMAP; //储存所有路点的map ROADMAP m_roadNet; //储存路径的stack ROADSTACK m_roadStack; typedef std::map
           
             ROADFLAG; typedef std::pair
            
              ROADFLAG_P; //不可访问点数组（方便查找，添加，删除） std::vector
             
               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
              
               (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
               
                 UnusePoints, double hasPassby) { //遍历p1底下所有的点，如果有p2直接命中跳出递归，其余点做简单的判断，命中并继续，如果已经命中则加入不可被调用连接 //如果此点下无其他子节点，或者所有节点都已经被加入到不可调用连接，则将此点放入不可被调用连接 //1.先将所有节点都遍历 //bl是所有当前start点能连接到的点和此点到dest点的距离 printf("findroad2：start:%d, dest:%d, startnum:%d\n",start.id(),dest.id(),start.getNumOfRoads()); ROADFLAG roadlist; std::vector
                
                  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("\n此路径长度为：%f\n",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,不知道自己是否在忽视列表中?\n",start.id(),st_id); //查看pa是否在不可访问的UnuseRoadPoints中，如果不在则加入roadlist列表 if (find(UnuseRoadPoints.begin(),UnuseRoadPoints.end(),st_id) == UnuseRoadPoints.end()) { printf("我是start：%d 下的： %d,我不在忽视列表中！\n",start.id(),st_id); //加入不可回头访问 UnuseRoadPoints.push_back(st_id); //如果不在UnuseRoadPoints中，则可加入roadlist，方便之后检查 double estimateLength = rp.getLengthFromDest(dest) + lengthOfRoad; printf("\n将第%d条路加入尝试，此路径预估为：%f,总长为：%f\n",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("\n尝试：%d路，此路径预估为：%f,总长为：%f\n",it->second.id(),estimateLength,g_totalLegnth); if(g_totalLegnth < 0.1 /*还没有发现一条路径*/ ||/*或者，如果已经发现路径，但是目前预估的路径比已发现路径更优*/ estimateLength < g_totalLegnth) { //如果找到dest，则返回上一步继续 printf("进入1：\n"); 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("\n最终：");    while (!roadStack.empty())    {        uint flag = roadStack.top();        printf("路径%d--->",flag);        roadStack.pop();    }        printf("\n最终路径长度为：%f\n",RoadPointManager::SigngleInt()->totalLegnth());    printf("\n\n");    //第二次查找    ROADSTACK roadStack2 = RoadPointManager::SigngleInt()->findWay(12,2);    printf("最终：");    while (!roadStack2.empty())    {        uint flag = roadStack2.top();        printf("路径%d--->",flag);        roadStack2.pop();    }        printf("\n最终路径长度为：%f\n",RoadPointManager::SigngleInt()->totalLegnth());        getchar();    return 0;}