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