A星算法-自動尋路-c++

由於本人太笨，看原理看了老半天才看懂，雖然有現成的寫好的版本，但還是想自己寫一個。我寫的這個基本上沒怎麼優化，只是實現了原理。

 

一共分三個文件，一個頭文件兩個CPP文件

 

A星原理可以去網上搜。

 

那我就直接上代碼啦！

 

A星的頭文件

 

/************************************************************************/ /* author: 小魯 /* version: 1.0 /* update: 06/26/10 /************************************************************************/ #include "stdafx.h" #include <list> #include <math.h> #include "iostream" #include <assert.h> using namespace std; // 座標點 typedef struct _point { int x; int y; int isblock; _point() { x = 0; y = 0; } _point(int _x, int _y) { x = _x; y = _y; } }point; //結點 typedef struct _Node { point pt; int f, g, h; struct _Node *preNode;//父節點 }Node; /************************************************************************/ /* A 星算法 */ /************************************************************************/ class AStar{ public: AStar(void); ~AStar(void); public: static const int SPEND_H = 10;//H值耗費 static const int SPEND_G_FAR = 14;//遠的G值耗費 static const int SPEND_G_NEAR = 10;//近的G值耗費 list<Node*> psOpenList; //開放列表 list<Node*> psCloseList;//封閉列表 Node *sStartNode; //起始節點 Node *sEndNode; //終止節點 Node *pcurrNode;//是否是終點? NOT NULL : NULL public: void SetStartNode(point st){sStartNode->pt.x = st.x;sStartNode->pt.y = st.y;};//設定起始節點 void SetEndNode(point et){sEndNode->pt.x = et.x;sEndNode->pt.y = et.y;};//設定結束節點 void Search();//主搜索 void AddToOpenList(Node *src);//添加到開放列表 void GetNodeByPoint(Node *pareNode, int _x, int _y);//通過座標添加到開放列表 bool IsClosed(Node *src);//座標點是否在封閉列表中 bool IsInOpenList(Node *src);//座標點是否在開放列表中 bool IsBlock(Node *src);//座標點是否是阻擋點 void SetH(Node *node);//設定H的耗費 void SetG(Node *node);//設定G的耗費 void SetF(Node *node);//設定F的耗費 Node* GetBestNode(); //得到最優的節點(F值最低的節點) bool isPath(list<point> *path, int i, int j);//測試方法-用於輸出 };

 

A星的實現文件

 

/************************************************************************/ /* author: 小魯 /* version: 1.0 /* update: 06/26/10 /************************************************************************/ #include "stdafx.h" #include "AStar.h" AStar::AStar(void) { sStartNode = new Node(); sEndNode = new Node(); pcurrNode = NULL; } AStar::~AStar(void) { delete sStartNode; delete sEndNode; delete pcurrNode; } void AStar::Search() { Node *currentNode;//當前節點 currentNode = sStartNode; currentNode->preNode = NULL;//初始節點的父節點爲空 currentNode->g = 0; SetH(currentNode); SetF(currentNode); psOpenList.push_front(sStartNode);//添加起始點到開放列表 do { currentNode = GetBestNode();//從開放列表得到最優節點 //遍歷當前結點周圍的結點並加入開放列表，當前節點從開放列表中移到封閉列表中 AddToOpenList(currentNode); psCloseList.push_front(currentNode);//添加到關閉列表中 if(psOpenList.size() < 1 || pcurrNode)break; //如果目標節點已經存在於開放列表或開放列表是空則退出 } while (true); //*******************************測試方法Start*******************************// list<point> path; //用於存放尋路後的數據 do { path.push_front(currentNode->pt); cout<<currentNode->pt.x<<"/t"<<currentNode->pt.y<<endl; if(currentNode->pt.x == sStartNode->pt.x && currentNode->pt.y == sStartNode->pt.y)break; currentNode = currentNode->preNode; } while (true); //輸出模擬的地圖和尋路後的路徑 for(int j=0;j<10;++j) { for (int i=0;i<10;++i) { if(isPath(&path,i,j)) cout<<"〇"<<"/t"; else cout<<"×"<<"/t"; } cout<<endl; } //*******************************測試方法End*******************************// } //*******************************測試方法Start*******************************// bool AStar::isPath(list<point> *path, int i, int j) { for(list<point>::iterator openIterator = path->begin(); openIterator != path->end(); ++openIterator) { if((*openIterator).x == i && (*openIterator).y == j){return true;} } return false; } //*******************************測試方法End*******************************// //添加到開放列表 void AStar::AddToOpenList(Node *src){ //添加當前座標的周圍座標(一共8個方向) GetNodeByPoint(src, src->pt.x, src->pt.y + 1);//上 GetNodeByPoint(src, src->pt.x, src->pt.y - 1);//下 GetNodeByPoint(src, src->pt.x - 1, src->pt.y);//左 GetNodeByPoint(src, src->pt.x + 1, src->pt.y);//右 GetNodeByPoint(src, src->pt.x - 1, src->pt.y + 1);//左上 GetNodeByPoint(src, src->pt.x - 1, src->pt.y - 1);//左下 GetNodeByPoint(src, src->pt.x + 1, src->pt.y + 1);//右上 GetNodeByPoint(src, src->pt.x + 1, src->pt.y - 1);//右下 } void AStar::GetNodeByPoint(Node *pareNode, int _x, int _y) { //如果座標已經越界則不添加---具體參數讀配置文件 if(_x < 0 || _y< 0 || _x >10 || _y>10) return ; Node *sNode = new Node(); //設定座標值 sNode->pt.x = _x; sNode->pt.y = _y; //如果此座標存在於封閉列表或是阻擋物的話，不進行添加 if(IsClosed(sNode) || IsBlock(sNode)) { return; } else { //設定父結點 sNode->preNode = pareNode; SetG(sNode);SetH(sNode);SetF(sNode);//設定各種耗費 psOpenList.push_front(sNode);//添加到開放列表 if(sNode->pt.x == sEndNode->pt.x && sNode->pt.y == sEndNode->pt.y) pcurrNode = sNode;//終點座標已經存在於開放列表 } } //是否存在於封閉列表 bool AStar::IsClosed(Node *src) { for(list<Node*>::iterator openIterator = psCloseList.begin(); openIterator != psCloseList.end(); ++openIterator) { if((*openIterator)->pt.x == src->pt.x && (*openIterator)->pt.y == src->pt.y){return true;} } return false; } //是否存在於開放列表 bool AStar::IsInOpenList(Node *src) { for(list<Node*>::iterator openIterator = psOpenList.begin(); openIterator != psOpenList.end(); ++openIterator) { if((*openIterator)->pt.x == src->pt.x && (*openIterator)->pt.y == src->pt.y){return true;} } return false; } //是否是阻擋座標 bool AStar::IsBlock(Node *src) { //下面是測試數據，真實的數據去讀配置文件 if(src->pt.x == 0 && src->pt.y == 4)return true; if(src->pt.x == 1 && src->pt.y == 4)return true; if(src->pt.x == 2 && src->pt.y == 4)return true; if(src->pt.x == 3 && src->pt.y == 4)return true; if(src->pt.x == 4 && src->pt.y == 4)return true; //if(src->pt.x == 5 && src->pt.y == 4)return true; if(src->pt.x == 6 && src->pt.y == 4)return true; if(src->pt.x == 7 && src->pt.y == 4)return true; if(src->pt.x == 8 && src->pt.y == 4)return true; if(src->pt.x == 9 && src->pt.y == 4)return true; if(src->pt.x == 10 && src->pt.y == 4)return true; return false; } //計算從B點(某點)到終點的耗費 void AStar::SetH(Node *node) { node->h = (abs(node->pt.x - sEndNode->pt.x) + abs(node->pt.y - sEndNode->pt.y)) * SPEND_H; } //計算從起點到B點(某點)的耗費 void AStar::SetG(Node *node) { if(node->pt.x != node->preNode->pt.x && node->pt.y != node->preNode->pt.y) node->g = node->preNode->g + SPEND_G_FAR; else node->g = node->preNode->g + SPEND_G_NEAR; } //計算總體耗費 F = G + H void AStar::SetF(Node *node) { node->f = node->g + node->h; } //從開放列表中得到最優值(F值最低) Node* AStar::GetBestNode() { Node* bNode; int bestF = 9999999; list<Node*>::iterator iterT;//記錄最優值的位置方便刪除 for(list<Node*>::iterator openIterator = psOpenList.begin(); openIterator != psOpenList.end(); ++openIterator) { if(bestF > (*openIterator)->f){ bestF = (*openIterator)->f; bNode = *openIterator; iterT = openIterator; } } if(bNode) psOpenList.erase(iterT);//找到最優值後從開放列表中刪除 return bNode; }

 

測試文件

 

/************************************************************************/ /* author: 小魯 /* version: 1.0 /* update: 06/26/10 /************************************************************************/ #include "stdafx.h" #include "iostream" #include <list> #include "AStar.h" using namespace std; /************************************************************************/ /* A 星算法測試 */ /************************************************************************/ int _tmain(int argc, _TCHAR* argv[]) { point sp; sp.x = 1; sp.y = 3; point ep; ep.x = 10; ep.y = 10; AStar astar; astar.SetStartNode(sp); astar.SetEndNode(ep); astar.Search(); getchar(); return 0; }