常见寻路算法
寻路
深度
四个方向，依次寻找，入栈，都走完了就出栈回退
1 #include <iostream> 2 using namespace std; 3 4 #include <graphics.h> 5 6 #include "MyStack.h" 7 8 //格子宽高 9 #define SPACE 80 10 11 12 //Y 竖着 13 #define ROWS 10 14 //X 横着 15 #define COLS 10 16 // 路 墙 17 enum states{ Road,Wall }; 18 //试探方向 19 enum direct{p_up,p_down,p_left,p_right}; 20 21 class MyPoint{ 22 public: 23 int row, col; 24 friend bool operator==(const MyPoint& p1, const MyPoint& p2); 25 }; 26 27 bool operator==(const MyPoint& p1, const MyPoint& p2){ 28 if ((p1.row == p2.row) && (p1.col == p2.col) ) return true; 29 return false; 30 } 31 32 class PathNode{ 33 public: 34 //states val; //地图上的值 35 direct dir; //当前试探方向 36 bool isFind; //是否走过 37 }; 38 39 40 41 //图片变量 42 IMAGE pos, ren, road, wall; 43 44 45 void printMap(int map[ROWS][COLS], MyPoint pos); 46 void drawMap(int map[ROWS][COLS],MyPoint pos); 47 48 int main(){ 49 initgraph(SPACE*COLS, SPACE*ROWS,SHOWCONSOLE); 50 loadimage(&road, "road.bmp", SPACE, SPACE, true); 51 loadimage(&wall, "wall.bmp", SPACE, SPACE, true); 52 loadimage(&ren, "ren.bmp", SPACE, SPACE, true); 53 loadimage(&pos, "pos.bmp", SPACE/2, SPACE/2, true); 54 //地图 55 int map[ROWS][COLS] = { 56 { Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall }, 57 { Wall, Road, Wall, Road, Road, Wall, Wall, Road, Road, Wall }, 58 { Wall, Road, Wall, Road, Wall, Wall, Wall, Road, Wall, Wall }, 59 { Wall, Road, Wall, Road, Road, Road, Wall, Road, Wall, Wall }, 60 { Wall, Road, Road, Road, Wall, Road, Wall, Road, Wall, Wall }, 61 { Wall, Road, Wall, Wall, Wall, Road, Wall, Road, Wall, Wall }, 62 { Wall, Road, Wall, Wall, Wall, Road, Road, Road, Road, Wall }, 63 { Wall, Road, Wall, Road, Road, Road, Wall, Road, Wall, Wall }, 64 { Wall, Road, Wall, Road, Wall, Road, Wall, Road, Road, Wall }, 65 { Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall, Wall } 66 }; 67 //辅助地图 68 PathNode pathMap[ROWS][COLS] = { p_up }; 69 /* 70 for (int i = 0; i < ROWS; i++){ 71 for (int j = 0; j < COLS; j++){ 72 pathMap[i][j] = map[i][j]; 73 } 74 } 75 */ 76 MyPoint begPos = { 1, 1 };//起点 77 MyPoint endPos = { 8, 8 };//终点 78 79 //准备一个栈 80 MyStack<MyPoint> stack; 81 //起点标记走过 82 pathMap[begPos.row][begPos.col].isFind = true; 83 //起点入栈 84 stack.push(begPos); 85 86 //当前点 87 MyPoint currentPos = begPos; 88 //试探点 89 MyPoint searchPos; 90 //是否找到终点 91 bool isFindEnd = false; 92 //循环寻路 93 while (1){ 94 95 searchPos = currentPos; 96 switch (pathMap[currentPos.row][currentPos.col].dir){ 97 case p_up: 98 searchPos.row--;//1 找到试探点 99 //2 判断是否能走 100 //试探方向改变 101 pathMap[currentPos.row][currentPos.col].dir = p_down; 102 if (pathMap[searchPos.row][searchPos.col].isFind == false &&//没有走过 103 map[searchPos.row][searchPos.col] == Road){//是路 可以走 104 //2.1 能走 105 currentPos = searchPos;//走 106 pathMap[currentPos.row][currentPos.col].isFind = true;//标记走过 107 stack.push(currentPos);//入栈 108 } 109 break; 110 case p_down: 111 searchPos.row++;//1 找到试探点 112 //2 判断是否能走 113 //试探方向改变 114 pathMap[currentPos.row][currentPos.col].dir = p_left; 115 if (pathMap[searchPos.row][searchPos.col].isFind == false &&//没有走过 116 map[searchPos.row][searchPos.col] == Road){//是路 可以走 117 //2.1 能走 118 currentPos = searchPos;//走 119 pathMap[currentPos.row][currentPos.col].isFind = true;//标记走过 120 stack.push(currentPos);//入栈 121 } 122 break; 123 case p_left: 124 searchPos.col--;//1 找到试探点 125 //2 判断是否能走 126 //试探方向改变 127 pathMap[currentPos.row][currentPos.col].dir = p_right; 128 if (pathMap[searchPos.row][searchPos.col].isFind == false &&//没有走过 129 map[searchPos.row][searchPos.col] == Road){//是路 可以走 130 //2.1 能走 131 currentPos = searchPos;//走 132 pathMap[currentPos.row][currentPos.col].isFind = true;//标记走过 133 stack.push(currentPos);//入栈 134 } 135 break; 136 case p_right: 137 searchPos.col++;//1 找到试探点 138 //2 判断是否能走 139 if (pathMap[searchPos.row][searchPos.col].isFind == false &&//没有走过 140 map[searchPos.row][searchPos.col] == Road){//是路 可以走 141 //2.1 能走 142 currentPos = searchPos;//走 143 pathMap[currentPos.row][currentPos.col].isFind = true;//标记走过 144 stack.push(currentPos);//入栈 145 } 146 else{//2.2.2 第四个方向还不能走 147 //回退 148 stack.pop(); 149 currentPos = stack.getTop(); 150 } 151 break; 152 }//end of switch (pathMap[currentPos.row][currentPos.col].dir) 153 printMap(map, currentPos); 154 drawMap(map, currentPos); 155 Sleep(300); 156 //3 判断是否结束循环 157 //3.1 找到终点了 158 if (endPos == currentPos){ 159 isFindEnd = true; 160 break; 161 } 162 //3.2 栈空了 163 if (stack.isEmpty()) break; 164 } 165 166 if (isFindEnd){ 167 cout << "喜大普奔，找到终点啦!" << endl; 168 169 while (!stack.isEmpty()){ 170 cout << "(" << stack.getTop().row << "," 171 << stack.getTop().col << ") "; 172 putimage(stack.getTop().col*SPACE + SPACE/4, 173 stack.getTop().row*SPACE + SPACE / 4, &pos); 174 stack.pop(); 175 } 176 cout << endl; 177 178 } 179 180 181 while (1); 182 return 0; 183 } 184 185 186 void drawMap(int map[ROWS][COLS], MyPoint pos){ 187 for (int i = 0; i < ROWS; i++){ 188 for (int j = 0; j < COLS; j++){ 189 if (i == pos.row && j == pos.col){ 190 putimage(j*SPACE, i*SPACE, &ren); 191 } 192 else if (Wall == map[i][j]){ 193 putimage(j*SPACE, i*SPACE, &wall); 194 } 195 else if (Road == map[i][j]){ 196 putimage(j*SPACE, i*SPACE, &road); 197 } 198 } 199 } 200 } 201 void printMap(int map[ROWS][COLS], MyPoint pos){ 202 system("cls"); 203 for (int i = 0; i < ROWS; i++){ 204 for (int j = 0; j < COLS; j++){ 205 if (i == pos.row && j == pos.col){ 206 cout << "人"; 207 } 208 else if (Wall == map[i][j]){ 209 cout << "墙"; 210 } 211 else if (Road == map[i][j]){ 212 cout << " "; 213 } 214 } 215 cout << endl; 216 } 217 }
广度
走过的路入树，把能走的路是下一次寻路的基础
1 #include <iostream> 2 #include <vector> 3 using namespace std; 4 //Y 竖着 5 #define ROWS 10 6 //X 横着 7 #define COLS 10 8 //试探方向 9 enum direct{ p_up, p_down, p_left, p_right }; 10 class MyPoint{ 11 public: 12 int row, col; 13 friend bool operator==(const MyPoint& p1, const MyPoint& p2); 14 }; 15 bool operator==(const MyPoint& p1, const MyPoint& p2){ 16 if ((p1.row == p2.row) && (p1.col == p2.col)) return true; 17 return false; 18 } 19 20 struct treeNode{ 21 MyPoint pos; //点 22 treeNode* pParent; //指向父节点的指针 23 vector<treeNode*> child; //动态数组 数组里存指向孩子节点指针 24 treeNode(){} 25 ~treeNode(){ 26 pos.row = 0; 27 pos.col = 0; 28 pParent = NULL; 29 child.clear(); 30 } 31 32 }; 33 treeNode* createTreeNode(int row, int col); 34 //判断pos点能不能走，能返回ture，不能返回false 35 bool canWalk(int map[ROWS][COLS], bool pathMap[ROWS][COLS], MyPoint pos); 36 37 38 int main(){ 39 //地图 40 int map[ROWS][COLS] = { 41 { 0, 1, 1, 1, 1, 1, 1, 0, 0, 0 }, 42 { 0, 0, 0, 0, 0, 0, 0, 0, 1, 1 }, 43 { 0, 1, 1, 0, 1, 0, 1, 1, 1, 0 }, 44 { 0, 0, 0, 0, 1, 0, 1, 1, 1, 0 }, 45 { 0, 1, 1, 1, 1, 0, 1, 1, 1, 0 }, 46 { 0, 0, 0, 0, 0, 0, 1, 1, 1, 0 }, 47 { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 }, 48 { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 }, 49 { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 }, 50 { 0, 1, 1, 1, 1, 1, 1, 1, 1, 0 } 51 }; 52 //辅助地图 53 bool pathMap[ROWS][COLS] = { 0 }; 54 55 MyPoint begPos = { 0, 0 };//起点 56 MyPoint endPos = { 0, 9 };//终点 57 58 pathMap[begPos.row][begPos.col] = true; 59 treeNode* pRoot = NULL;//准备一棵树 60 61 //起点成为树的根节点 62 pRoot = createTreeNode(begPos.row, begPos.col); 63 64 65 vector<treeNode*> buff; //存储当前层 66 buff.push_back(pRoot);//当前层里是树的根节点 67 vector<treeNode*> nextBuff; //存储下一层 68 bool isFindEnd = false; 69 treeNode* temp = NULL; 70 while (1){ 71 nextBuff.clear();//清空下一层 72 for (int i = 0; i < buff.size(); i++){//遍历当前层 73 //找出 buff[i] 所有相邻能走节点 存放到nextBuff中 并且 入树 74 for (int j = 0; j < 4; j++){ 75 temp = createTreeNode(buff[i]->pos.row, buff[i]->pos.col); 76 switch (j){ 77 case p_up: temp->pos.row--;break; 78 case p_down: temp->pos.row++; break; 79 case p_left: temp->pos.col--; break; 80 case p_right: temp->pos.col++; break; 81 } 82 if (canWalk(map, pathMap, temp->pos)){ 83 //标记走过 84 pathMap[temp->pos.row][temp->pos.col] = true; 85 //入树 86 buff[i]->child.push_back(temp); //temp成为buff[i]的子 87 temp->pParent = buff[i]; //buff[i]成为temp的父 88 #if 1 89 if (endPos == temp->pos){ 90 isFindEnd = true; 91 break; 92 } 93 #endif 94 //存入nextBuff中 95 nextBuff.push_back(temp); 96 } 97 else{//不能走 98 delete temp;//释放 99 temp = NULL; 100 } 101 }// end of for(j) 102 if (isFindEnd) break; 103 }// end of for(i) 104 if (isFindEnd) break; 105 if (0 == nextBuff.size()) break;//地图都遍历完了 106 buff = nextBuff;//切换到下一层去 107 }//end of while(1) 108 109 110 if (isFindEnd){ 111 cout << "找到终点啦!" << endl; 112 while (temp){ 113 cout << "("<<temp->pos.row << ","<< temp->pos.col << ") "; 114 temp = temp->pParent; 115 } 116 cout << endl; 117 } 118 119 //层序遍历 120 121 122 while (1); 123 return 0; 124 } 125 126 127 treeNode* createTreeNode(int row, int col){ 128 treeNode* pNew = new treeNode; 129 memset(pNew, 0, sizeof treeNode); 130 pNew->pos.row = row; 131 pNew->pos.col = col; 132 return pNew; 133 } 134 //判断pos点能不能走，能返回ture，不能返回false 135 bool canWalk(int map[ROWS][COLS], bool pathMap[ROWS][COLS], MyPoint pos){ 136 //越界 137 if (pos.row < 0 || pos.row >= ROWS || pos.col < 0 || pos.col >= COLS) return false; 138 //是障碍物 139 if (1 == map[pos.row][pos.col]) return false; 140 //走过 141 if (true == pathMap[pos.row][pos.col]) return false; 142 return true; 143 }
A*
#include <iostream> #include <vector> using namespace std; #define ZXDJ 10 #define XXDJ 14 #define ROWS 10 #define COLS 10 enum direct{p_up,p_down,p_left,p_right,p_lup,p_ldown,p_rup,p_rdown}; struct MyPoint{ //private: int row; int col; int f; int g; int h; public: int getRow()const{ return row; } int getCol()const{ return col; } void setH(int h){ this->h = h; } //计算出f void getF(){ f = g + h; } MyPoint(){ row = col = f = g = h = 0; } MyPoint(int y, int x){ row = y; col = x; f = g = h = 0; } friend bool operator==(const MyPoint& p1, const MyPoint& p2); friend ostream& operator<<(ostream& o, const MyPoint& p); }; ostream& operator<<(ostream& o, const MyPoint& p){ return cout << "(" << p.row << "," << p.col << ")"; } bool operator==(const MyPoint& p1, const MyPoint& p2){ if ((p1.row == p2.row) && (p1.col == p2.col)) return true; return false; } //树的节点 struct treeNode{ MyPoint pos; treeNode* pParent; vector<treeNode*> child; treeNode(){ pParent = NULL; } treeNode(const MyPoint& p){ this->pos = p; pParent = NULL; } //void getH(const MyPoint& p){ } }; //判断pos点能不能走，能走返回true 否则返回false bool canWalk(int map[ROWS][COLS], bool pathMap[ROWS][COLS], const MyPoint& pos); //计算出pos的h值并返回 int getH(const MyPoint& pos, const MyPoint& endPos); int main(){ //1 地图 int map[ROWS][COLS] = { { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 1, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 } }; //辅助地图 bool pathMap[ROWS][COLS] = { 0 }; //2 起点终点 MyPoint begPos = { 1, 1 }; MyPoint endPos = { 7, 8 }; //3 准备一颗树 treeNode* pRoot = NULL; //起点成为树的根节点 pRoot = new treeNode(begPos); //标记起点走过 pathMap[begPos.getRow()][begPos.getCol()] = true; //4 准备一个数组用来存储所有需要找最小f的节点指针 vector<treeNode*> buff; vector<treeNode*>::iterator it; //用来遍历 vector<treeNode*>::iterator itMin; //保存f最小的元素地址 //5 寻路 treeNode* pCurrent = pRoot; treeNode* pChild = NULL; bool isFindEnd = false; while (!isFindEnd){ //5.1 找到周围能走的点 for (int i = 0; i < 8; i++){ pChild = new treeNode; pChild->pos = pCurrent->pos; switch (i){ case p_up: pChild->pos.row--; pChild->pos.g += ZXDJ; break; case p_down: pChild->pos.row++; pChild->pos.g += ZXDJ; break; case p_left: pChild->pos.col--; pChild->pos.g += ZXDJ; break; case p_right: pChild->pos.col++; pChild->pos.g += ZXDJ; break; case p_lup: pChild->pos.row--; pChild->pos.col--;pChild->pos.g += XXDJ; break; case p_ldown: pChild->pos.row++; pChild->pos.col--; pChild->pos.g += XXDJ; break; case p_rup: pChild->pos.row--; pChild->pos.col++; pChild->pos.g += XXDJ; break; case p_rdown: pChild->pos.row++; pChild->pos.col++; pChild->pos.g += XXDJ; break; }//end of switch if (canWalk(map,pathMap,pChild->pos)){//能走 pChild->pos.h = getH(pChild->pos, endPos);//先算出h pChild->pos.getF();//再算出f //入树 pCurrent->child.push_back(pChild); pChild->pParent = pCurrent; //入buff buff.push_back(pChild); } else{//不能走 delete pChild; pChild = NULL; } }//for (int i = 0; i < 8; i++) //5.2 找出buff中f最小的点 itMin = buff.begin();//假设第一个最小 for (it = buff.begin(); it != buff.end(); it++){ itMin = ( ( (*itMin)->pos.f<(*it)->pos.f ) ? itMin : it ); } //5.3 pCurrent指向buff中f最小的点 pCurrent = *itMin; buff.erase(itMin);//5.4 把它从buff中删掉 //5.5 判断是否找到终点 或者 永远找不到终点了 if (pCurrent->pos == endPos){//找到终点 isFindEnd = true; } if (0 == buff.size()) break;//永远找不到终点了 }//end of while //6 显示路径 if (isFindEnd){ cout << "找到终点了:"; while (pCurrent){ cout << pCurrent->pos << " "; pCurrent = pCurrent->pParent; } cout << endl; } while (1); return 0; } //判断pos点能不能走，能走返回true 否则返回false bool canWalk(int map[ROWS][COLS], bool pathMap[ROWS][COLS], const MyPoint& pos){ if (pos.row < 0 || pos.col < 0 || pos.row >= ROWS || pos.col >= COLS) return false; if (map[pos.row][pos.col]) return false; if (pathMap[pos.row][pos.col]) return false; return true; } //计算出pos的h值并返回 int getH(const MyPoint& pos, const MyPoint& endPos){ int x = ((endPos.col > pos.col) ? (endPos.col - pos.col) : (pos.col - endPos.col)); int y = ((endPos.row > pos.row) ? (endPos.row - pos.row) : (pos.row - endPos.row)); return ZXDJ*(x + y); }