A*算法的实现

看见CSDN博主小满（bill man）个人原创实现A*算法，没有全部的代码，且实现的是游戏中的应用。故想到不如自己在他的基础上把代码补全，同时将算法独立出来。只实现A*算法，不涉及游戏引擎部分。

算法思想在这篇博客里有很详尽的说明：http://blog.vckbase.com/panic/archive/2005/03/20/3778.html

代码核心部分皆取自小满（bill man）的博文：http://blog.csdn.net/bill_man/article/details/7296560

 

这是我的第一篇CSDN博客，感谢老婆一直鼓励我，我会继续努力的！哈哈……

 

直接上代码：

//*** Map.h ***

#ifndef __MAP_H#define __MAP_Hclass Map{public:Map(int wid, int hgt);~Map();int  width()  {return m_width; }int  height() {return m_height; }void setobstacle(int col, int row);void setstep(int col, int row);char value(int col, int row);private:char* matrix;int   m_width;int   m_height;};#endif

 

 

//*** Map.cpp ***

#include <string>#include "Map.h"Map::Map(int wid, int hgt){m_width  = wid;m_height = hgt;int size = wid * hgt;matrix = new char[size];memset(matrix, '0', size * sizeof(char)); //大小不是size ！！！}Map::~Map(){delete[] matrix;}void Map::setobstacle(int col, int row){if(matrix)matrix[row * m_width + col] = '1';}void Map::setstep(int col, int row){if(matrix)matrix[row * m_width + col] = '*';}char Map::value(int col, int row){return matrix[row * m_width + col];}

 

 

//*** AstarItem.h ***

#ifndef __ASTARITEM_H#define __ASTARITEM_Hclass AstarItem{public:AstarItem(void)  {}~AstarItem(void) {}void setpos(int col, int row);int col() { return id_col; }int row() { return id_row; }int  getcol() { return id_col; }int  getrow() { return id_row; }void setf(int f);int  getf()   { return id_f;   }void setg(int g);int  getg()   { return id_g;   }void seth(int h);int  geth()   { return id_h;   }void setfid(int fid);int  getfid() { return id_fid; }private:int id_row;  //行int id_col;  //列int id_f;    //估价函数f = g + hint id_g;    //实际代价int id_h;    //估计代价int id_fid;  //父节点id};#endif

 

 

 

//*** AstarItem.cpp ***

#include "AStarItem.h"void AstarItem::setpos(int col, int row){id_col = col;id_row = row;}void AstarItem::setf(int f){id_f = f;}void AstarItem::setg(int g){id_g = g;}void AstarItem::seth(int h){id_h = h;}void AstarItem::setfid(int fid){id_fid = fid;}

 

 

//***** AStar.h ****

#ifndef __ASTAR_H#define __ASTAR_H#include <vector>#include <stack>using namespace std;class Map;class AstarItem;class Astar{public:Astar(Map* the_map) : map(the_map) {}~Astar(void);void reset();void findpath(int curX, int curY, int aimX, int aimY);int  getH(int col, int row);int  getG(int col, int row, int fid);void fromopentoclose();void removefromopen();void starsearch(int fid);void getpath();void showpath();bool checkmap(int col, int row);bool checkopen(int col, int row, int fid);bool checkclose(int col, int row);void addtoopen(int col, int row, int fid);void resetsort(int last);private:Map* map;int  curCol;int  curRow;int  aimCol;int  aimRow;stack<AstarItem*>  path;vector<AstarItem*>  open;vector<AstarItem*>  close;};#endif

//**** AStar.cpp ****

#include <iostream>#include <stdlib.h>#include "AStar.h"#include "Map.h"#include "AStarItem.h"using namespace std;Astar::~Astar(void){reset();}void Astar::reset(){open.clear();close.clear();while(!path.empty())path.pop();}int Astar::getH(int col, int row){return abs(aimCol - col) * 10 + abs(aimRow - row) * 10;}int Astar::getG(int col, int row, int fid){int fx = close[fid]->getcol();int fy = close[fid]->getrow();int fg = close[fid]->getg();if((fx - col != 0) && (fy - row != 0)){ // 上下左右return fg + 14;}else{ // 四角return fg + 10;}}void Astar::findpath(int curX, int curY, int aimX, int aimY){curCol = curX;curRow = curY;aimCol = aimX;aimRow = aimY;//优先队列的0项，无意义AstarItem* temp = new AstarItem();open.push_back(temp);AstarItem* temp1 = new AstarItem();temp1->setpos(curCol, curRow);temp1->setg(0);int ag = getH(curCol, curRow);temp1->seth(ag);temp1->setf(ag);temp1->setfid(0);open.push_back(temp1);while(open.size() > 1){fromopentoclose();int fatherid = close.size() - 1;int mycol = close.back()->getcol();int myrow = close.back()->getrow();if((aimCol == mycol) && (aimRow == myrow)){getpath();break;}else{starsearch(fatherid);}}//show path and resetshowpath();reset();}void Astar::fromopentoclose(){AstarItem* temp = open[1];close.push_back(temp);removefromopen();}void Astar::removefromopen(){int last = open.size() - 1;open[1] = open[last];open.pop_back();last = open.size() - 1;int head = 1;while((head * 2) <= last){ // shift downint child = head * 2;if((child + 1) <= last && open[child + 1]->getf() < open[child]->getf())child++;if(open[head]->getf() <= open[child]->getf())break;AstarItem* temp = open[child];open[child] = open[head];open[head]  = temp;head = child;}}void Astar::starsearch(int fid){int col = close[fid]->getcol();int row = close[fid]->getrow();int mycol = col;int myrow = row - 1;if(myrow >= 0 && checkmap(mycol, myrow)){if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow)){addtoopen(mycol, myrow, fid);}}mycol = col - 1;myrow = row;if(mycol >= 0 && checkmap(mycol, myrow)){if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow)){addtoopen(mycol, myrow, fid);}}mycol = col;myrow = row + 1;int w = map->width();if(myrow < map->width() && checkmap(mycol, myrow)){if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow)){addtoopen(mycol, myrow, fid);}}mycol = col + 1;myrow = row;if(mycol < map->height() && checkmap(mycol, myrow)){if(checkopen(mycol, myrow, fid) && checkclose(mycol, myrow)){addtoopen(mycol, myrow, fid);}}}bool Astar::checkmap(int col, int row){// 是否为障碍物return (map->value(col, row) == '0');}bool Astar::checkopen(int col, int row, int fid){ // 是否在open表中。若在，比较g值vector<AstarItem*>::iterator iter = open.end() - 1; //第0个为填充项，无意义int i = open.size() - 1;for( ; iter != open.begin(), i > 0; --iter, --i){AstarItem* item = *iter;if((item->getcol() == col) && (item->getrow() == row)){int tempG = getG(col, row, fid);if(tempG < (item->getg())){item->setg(tempG);item->setf(tempG + item->geth());item->setfid(fid);resetsort(i);}return false;}}return true;}bool Astar::checkclose(int col, int row){ // 是否在open表中，避免重复vector<AstarItem*>::iterator iter = close.begin();for( ; iter != close.end(); ++iter){AstarItem* item = *iter;if((item->getcol() == col) && (item->getrow() == row)){return false;}}return true;}void Astar::addtoopen(int col, int row, int fid){AstarItem* temp = new AstarItem();temp->setpos(col, row);temp->setfid(fid);int g = getG(col, row, fid);int h = getH(col, row);temp->setg(g);temp->seth(h);temp->setf(g + h);open.push_back(temp);resetsort(open.size() - 1);}void Astar::resetsort(int last){ // shift up(因为加入的是一个更小值，所以只需向上调整！)while(last > 1){int half = last / 2;if(open[half]->getf() <= open[last]->getf()){break;}AstarItem* temp = open[last];open[last] = open[half];open[half] = temp;last = half;}}void Astar::getpath(){path.push(close.back());while(true){if(path.top()->getg() == 0){break;}path.push(close[path.top()->getfid()]);}}void Astar::showpath(){int width  = map->width();int height = map->height();while(!path.empty()){AstarItem* item = path.top();path.pop();map->setstep(item->col(), item->row());}cout << "the step:" << endl;for(int i = 0; i < height; ++i){for(int j = 0; j < width; ++j){cout << map->value(i, j) << " " ;}cout << endl;}}

//***** main.cpp *****

#include <iostream>#include "Map.h"#include "AStar.h"using namespace std;int main(){Map   map(10, 10);Astar astar(&map);//set obstaclemap.setobstacle(0, 2);map.setobstacle(1, 2);map.setobstacle(2, 2);map.setobstacle(3, 2);map.setobstacle(4, 2);map.setobstacle(2, 5);map.setobstacle(3, 5);map.setobstacle(4, 5);map.setobstacle(5, 5);map.setobstacle(6, 5);map.setobstacle(7, 5);map.setobstacle(8, 5);map.setobstacle(9, 5);//show mapcout << "the map: " << endl;for(int i = 0; i < 10; ++i){for(int j = 0; j < 10; ++j){cout << map.value(i, j) << " " ;}cout << endl;}cout << endl;//find pathastar.findpath(1, 0, 4, 8);cin.get();return 0;}

运行结果：