A*算法和A*跳步算法的CPP实现
- by Hector
A*算法本身很简单,很早之前就了解过,用c++简单实现了一下,并又跳步算法进行了优化。
A*算法
A*算法不同于深度和广度优先算法,是一种启发式搜索算法
其主要逻辑是不断估算当前点到目的节点的大概距离,选取最优的路径不断前进,直至到达终点。过程如下:
1. 将当前点放如一个未处理的节点列表,叫做openList,另外有个已处理的节点列表叫做closeList.
2. 从openList中找到最优节点n(可用堆算法|优先队列),针对其每一个子节点x:
- a. 如果x在open中,如果x比open中节点的更好,则替换open中的节点,否则跳过x
- b. 如果x在close中,如果x比close中的节点更好,则要删掉close中的节点,并将x放到open中,代表重新处理。否则跳过x.
- c. 如果x不在open和close中,则放入open中,代表需要处理。
3. 重复2,直至到目标节点。到了目标节点后,从目标节点一步步后退,即可找到来的路径。
估价函数 f'(n) = g'(n) + h'(n)
f'(n)是估价函数,g'(n)是起点到终点的最短路径值,h'(n)是n到目标的最短路经的启发值。
跳步优化
跳步优化不同于常规的A*算法寻找当前节点所有的子节点,而是在于“跳过”一些明显不必经过的路,
抽象地说,就是在搜索时大量减掉明显无用的分支,使得朴素的搜索过程变得高效。
参考:
- 1. 代码:https://github.com/qiao/PathFinding.js/blob/master/src/finders/JumpPointFinder.js
- 2. 解说:http://plusplus7.com/lemonbag/jpsstudy
源码放在github: https://github.com/myourys/Algorithm 其中AStar.cpp文件。
/*============================================================================= # FileName: AStar.cpp # Desc: A星寻路算法 # Author: Hector # Email: myourys@gmail.com # HomePage: http://www.yiwuye.com # Version: 0.0.1 # LastChange: 2014-08-21 18:48:40 # History: =============================================================================*/ /* * A*算法不同于深度和广度优先算法,是一种启发式搜索算法 * 其主要逻辑是不断估算当前点到目的节点的大概距离,选取最优的路径不断前进,直至到达终点。过程如下: * 1. 将当前点放如一个未处理的节点列表,叫做openList,另外有个已处理的节点列表叫做closeList. * 2. 从openList中找到最优节点n(可用堆算法|优先队列),针对其每一个子节点x: * a. 如果x在open中,如果x比open中节点的更好,则替换open中的节点,否则跳过x * b. 如果x在close中,如果x比close中的节点更好,则要删掉close中的节点,并将x放到open中,代表重新处理。否则跳过x. * c. 如果x不在open和close中,则放入open中,代表需要处理。 * 3. 重复2,直至到目标节点。到了目标节点后,从目标节点一步步后退,即可找到来的路径。 * * 估价函数 f'(n) = g'(n) + h'(n) * f'(n)是估价函数,g'(n)是起点到终点的最短路径值,h'(n)是n到目标的最短路经的启发值。 */ /* * 跳步优化 * 跳步优化不同于常规的A*算法寻找当前节点所有的子节点,而是在于“跳过”一些明显不必经过的路, * 抽象地说,就是在搜索时大量减掉明显无用的分支,使得朴素的搜索过程变得高效。 * 参考: * 1. 代码:https://github.com/qiao/PathFinding.js/blob/master/src/finders/JumpPointFinder.js * 2. 解说:http://plusplus7.com/lemonbag/jpsstudy */ #include <iostream> #include <queue> /* priority_queue不支持遍历,利用vector容器和push_heap(堆算法)模拟一个优先容器 */ #include <vector> #include <sstream> #define kUseJumpNode 0 #define STR(A) #A using namespace std; /* 地图 * 第一行 行列数 * 第二行 起点,终端坐标 * 地图 0:可通过 1:不可通过 坐标起点为左下角 * 这里我们设定一个节点到相邻节点消耗为10,如果是对接线则为14. */ const char* MapStr = STR( 9 10 \n 6 3 6 6 \n 0 0 0 0 1 0 0 0 0 0 \n 0 0 0 0 1 0 0 0 0 0 \n 0 0 0 0 1 0 0 0 0 0 \n 0 0 1 1 1 0 0 0 0 0 \n 0 0 1 1 0 0 0 0 0 0 \n 0 0 1 0 0 0 0 0 0 0 \n 0 0 1 0 1 0 0 0 0 0 \n 0 0 0 0 1 0 0 0 0 0 \n 0 0 0 0 1 0 0 0 0 0 \n ); class Point { public: int x,y; Point(int _x,int _y):x(_x),y(_y){}; Point(){x =-1;y=-1;}; Point& operator=(const Point & other){this->x = other.x;this->y=other.y;return *this;}; bool operator==(const Point & other){return this->x == other.x && this->y== other.y;}; }; typedef struct _Node{ Point pos;//当前点在迷宫中的位置坐标 int g;//起始点到当前点实际代价 int h;//当前节点到目标节点最佳路径的估计代价 int f;//估计函数:f = g + h。 _Node *father;//指向其父节点的指针 }Node; //比较函数,用于堆操作 struct NodeCompare { bool operator ()(Node *&n1, Node *&n2) const { return n1->f > n2->f; //小值优先 } }; class AStar{ public: AStar():_map(nullptr){}; ~AStar(){clear();}; int readMap(const char* str); void printMap(); bool findRoad(); void printRoad(); void clear(); private: int estimate(const Point& pos); //估计点到目的地的值 void initNode(Node &node,const Point& pos,int g,Node* father); void expandNode(Node* n); void getNabors(vector<Point> *nabors,Node* n); void getJumpNodes(vector<Point> *jumps,Node* n); bool validPoint(const Point& p); private: Point _mapSize; Point _startPos; Point _endPos; int **_map; vector<Node*> _closeList; vector<Node*> _openList; }; void AStar::clear() { for(auto iter:_closeList) delete iter; for(auto iter:_openList) delete iter; if(_map) { for(int i =0;i<_mapSize.x;i++) delete []_map[i]; delete []_map; } } int AStar::estimate(const Point& pos) { return 10* (abs(_endPos.x - pos.x) + abs(_endPos.y-pos.y)); } int AStar::readMap(const char* str) { istringstream istr(str); istr>>_mapSize.x>>_mapSize.y; istr>>_startPos.x>>_startPos.y; istr>>_endPos.x>>_endPos.y; _map = new int *[_mapSize.x]; for(int i = _mapSize.x-1;i>=0;i--) { _map[i] = new int[_mapSize.y]; for(int j = 0;j< _mapSize.y;j++) { istr>>_map[i][j]; } } return 0; } void AStar::printMap() { for(int i = _mapSize.x-1;i>=0;i--) { for(int j = 0;j< _mapSize.y;j++) { cout<<_map[i][j]<<" "; } cout<<endl; } } void AStar::initNode(Node &node,const Point& pos,int g,Node* father) { node.pos= pos; node.g = g; node.h = estimate(pos); node.f = node.g +node.h; node.father = father; } bool AStar::validPoint(const Point& p) { if(p.x< 0 || p.y<0 || p.x > (_mapSize.x -1) || p.y > (_mapSize.y -1) ) return false; return true; } bool AStar::findRoad() { Node *start = new Node(); initNode(*start,_startPos,0,nullptr); _openList.push_back(start); push_heap(_openList.begin(),_openList.end(),NodeCompare()); while(1) { if(_openList.empty()) { cout<<"can't find the road."<<endl; } Node *n = _openList.front(); if(n->pos == _endPos) //到达终点 { cout<<"Find the Road:"<<endl; return true; } //将第一个元素移到最后,并将剩余区间重新排序,组成新的heap pop_heap(_openList.begin(),_openList.end(),NodeCompare()); _openList.pop_back();//删除最后一个元素n expandNode(n); _closeList.push_back(n); } return false; } void AStar::expandNode(Node* n) { vector<Point> nodeList; if(kUseJumpNode) getJumpNodes(&nodeList,n); else getNabors(&nodeList,n); for(auto& pos:nodeList) { if(_map[pos.x][pos.y] == 1) //不可通过的点 continue; int gVal; if(pos.x == n->pos.x || pos.y == n->pos.y) gVal= n->g +10; else gVal=n->g +14; //对角线 vector<Node*>::iterator openResult; for( openResult=_openList.begin();openResult!= _openList.end();++openResult) { if((*openResult)->pos == pos) //在open列表 break; } //在open列表,并且open列表中已经是最优的,跳过 if(openResult!= _openList.end() && (*openResult)->g <= gVal) continue; //closed列表 vector<Node*>::iterator closeResult; for( closeResult=_closeList.begin();closeResult!= _closeList.end();closeResult++) { if((*closeResult)->pos == pos) //在close列表 break; } //在close列表,并且close列表中已经是最新的了,跳过 if(closeResult!= _closeList.end() && (*closeResult)->g <= gVal) continue; //新节点为最优节点 //如果在close列表 if(closeResult != _closeList.end()) { _closeList.erase(closeResult); delete *closeResult; } //如果在open列表 if(openResult!= _openList.end()) { _openList.erase(openResult); make_heap(_openList.begin(),_openList.end(),NodeCompare()); delete *openResult; } Node *child = new Node(); initNode(*child,pos,gVal,n); _openList.push_back(child); push_heap(_openList.begin(),_openList.end(),NodeCompare()); } } void AStar::getNabors(vector<Point> *nabors,Node* n) { for(int x = n->pos.x-1;x<= n->pos.x+1; x++) //针对每一个子节点 { if(x < 0 || x > _mapSize.x -1) continue; for(int y = n->pos.y-1; y<= n->pos.y+1; y++) { if(y < 0 || y > _mapSize.y -1) continue; Point pos(x,y); if(pos == n->pos) continue; nabors->push_back(pos); } } } void AStar::getJumpNodes(vector<Point> *jumps,Node* n) { if(!n->father) { getNabors(jumps,n); return; } auto fPos = n->father->pos; int x = n->pos.x; int y = n->pos.y; int dx = x - fPos.x; int dy = y - fPos.y; /* * 这里假设 1 0 从左下角不能到右上角 * 0 1 */ if(dx!= 0 && dy!= 0) //父节点和当前节点在对角线上 { //当前节点旁边的两个点,并且远离父节点 if(validPoint(Point(x,y+dy)) && _map[x][y+dy]==0) jumps->push_back(Point(x,y+dy)); if(validPoint(Point(x+dx,y)) && _map[x+dx][y]==0) jumps->push_back(Point(x+dx,y)); //对角线的另一个点 if(validPoint(Point(x+dx, y+ dy)) && ( _map[x][y+dy]==0 || _map[x+dx][y]==0)) jumps->push_back(Point(x+dx,y+dy)); //靠近父节点的两个角 if(validPoint(Point(x-dx,y+dy)) && _map[x - dx][y] ==1 && _map[x][y+dy]==0) jumps->push_back(Point(x - dx, y + dy)); if(validPoint(Point(x+dx,y-dy)) && _map[x][y-dy] ==1 && _map[x+dx][y]==0) jumps->push_back(Point(x + dx, y - dy)); } else //父节点和当前节点在水平或者垂直线上 { if(dx == 0) //水平线上 { if(validPoint(Point(x, y + dy)) && _map[x][y + dy]==0) { jumps->push_back(Point(x, y + dy)); if(validPoint(Point(x+1, y) ) && _map[x+1][y]==1) jumps->push_back(Point(x+dx, y+dy)); if(validPoint(Point(x-1, y) ) && _map[x-1][y]==1) jumps->push_back(Point(x-1, y+dy)); } } else { if(validPoint(Point(x+dx, y)) && _map[x+dx][y]==0) { jumps->push_back(Point(x+dx, y)); if(validPoint(Point(x, y+1) ) && _map[x][y+1]==1) jumps->push_back(Point(x+dx, y+1)); if(validPoint(Point(x, y-1) ) && _map[x][y-1]==1) jumps->push_back(Point(x+dx, y-1)); } } } } void AStar::printRoad() { int **tmpMap = new int*[_mapSize.x]; for(int i=0;i<_mapSize.x;i++) tmpMap[i] = new int[_mapSize.y]; for(int i=0;i<_mapSize.x;i++) for(int j=0;j<_mapSize.y;j++) tmpMap[i][j]=_map[i][j]; Node* n= _openList.front(); while(n) { tmpMap[n->pos.x][n->pos.y] = -1; n = n->father; } for(int i=_mapSize.x-1;i>=0;i--) { for(int j=0;j<_mapSize.y;j++) { if(tmpMap[i][j] == -1) cout<<"# "; else cout<<tmpMap[i][j]<<' '; } cout<<endl; } for(int i=0;i<_mapSize.x;i++) delete []tmpMap[i]; delete []tmpMap; } int main() { AStar astar; astar.readMap(MapStr); astar.printMap(); cout<<endl; if(astar.findRoad()) astar.printRoad(); return 0; }