三维地图中的A*寻路
跟二维地图原理一样,只不过搜索方向多了,二维只搜8个方向,而三维要搜26个方向。
不懂的看我以前写的文章,这里直接贴代码:
#include <iostream> #include <cstring> #include <algorithm> #include <windows.h> #include <cmath> #include <list> #include <cstdio> using namespace std; const double LEN=10; const int MAX=500; const char ROAD='*'; const char WALL='#'; const char START='0'; const char STOP='1'; typedef struct node { node() { x=y=z=0; f=g=h=0; parent=NULL; } int x,y,z; double f,g,h; struct node *parent; } Node; char mmap[MAX][MAX][MAX]; //地图 list<Node*> startList,stopList; //开启列表和关闭列表 int sx,sy,sz,ex,ey,ez; //起点坐标(sx,sy,sz)和终点坐标(ex,ey,ez) int k,n,m; //高度k,m行n列 //26个方向,三阶魔方模型自己想象一下 int dx[26]= {-1,1,0,0,-1,1,-1,1,0,-1,1,0,0,-1,1,-1,1,0,-1,1,0,0,-1,1,-1,1}; int dy[26]= {0,0,-1,1,-1,-1,1,1,0,0,0,-1,1,-1,-1,1,1,0,0,0,-1,1,-1,-1,1,1}; int dz[26]= {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,-1,-1,-1,-1,-1,-1,-1,-1,-1}; //计算两点距离 double getDis(int x1,int y1,int z1,int x2,int y2,int z2) { double xx1=x1*LEN+LEN/2.0; //获取中心点坐标 double yy1=y1*LEN+LEN/2.0; double zz1=z1*LEN+LEN/2.0; double xx2=x2*LEN+LEN/2.0; double yy2=y2*LEN+LEN/2.0; double zz2=z2*LEN+LEN/2.0; return sqrt((xx1-xx2)*(xx1-xx2)+(yy1-yy2)*(yy1-yy2)+(zz1-zz2)*(zz1-zz2)); } //判断节点是否在列表中 bool in_List(Node *pnode,list<Node*> mlist) { for(list<Node*>::iterator it=mlist.begin(); it!=mlist.end(); it++) { if(pnode->x==(*it)->x&&pnode->y==(*it)->y&&pnode->z==(*it)->z) return true; } return false; } //从列表中删除节点 bool del(Node *pnode,list<Node*> &mlist) { for(list<Node*>::iterator it=mlist.begin(); it!=mlist.end(); it++) { if(pnode==(*it)) { mlist.erase(it); return true; } } return false; } //向列表中添加节点 void add(Node *pnode,list<Node*> &mlist) { mlist.push_back(pnode); return; } //从列表中获取f最小的节点 Node* getMin(list<Node*> mlist) { double mmin=100000000; Node *temp=NULL; for(list<Node*>::iterator it=mlist.begin(); it!=mlist.end(); it++) { if((*it)->f<mmin) { mmin=(*it)->f; temp=(*it); } } return temp; } //设置路径 void setRoad(Node *root) { while(root->parent!=NULL) { if(root->x==ex&&root->y==ey&&root->z==ez) { mmap[root->z][root->x][root->y]=STOP; } else mmap[root->z][root->x][root->y]=ROAD; root=root->parent; } } void printRoad() { for(int kk=0; kk<k; kk++) { for(int i=0; i<m; i++) { for(int j=0; j<n; j++) { if(mmap[kk][i][j]==ROAD||mmap[kk][i][j]==START||mmap[kk][i][j]==STOP) { SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY|FOREGROUND_GREEN); cout<<mmap[kk][i][j]<<" "; } else { SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY); cout<<mmap[kk][i][j]<<" "; } } cout<<endl; } cout<<endl<<endl; } cout<<endl<<endl<<endl; SetConsoleTextAttribute(GetStdHandle(STD_OUTPUT_HANDLE),FOREGROUND_INTENSITY); } //搜索 void bfs() { startList.clear(); stopList.clear(); Node *preNode = new Node; preNode->x=sx; preNode->y=sy; preNode->z=sz; preNode->g=0; preNode->h=getDis(sx,sy,sz,ex,ey,ez); preNode->f=preNode->g+preNode->h; preNode->parent=NULL; add(preNode,startList); while(!startList.empty()) { //cout<<"-.-"<<endl; preNode=getMin(startList); if(preNode==NULL) { cout<<"自动寻路失败"<<endl; return; } del(preNode,startList); add(preNode,stopList); for(int d=0; d<26; d++) { int cx=preNode->x+dx[d]; int cy=preNode->y+dy[d]; int cz=preNode->z+dz[d]; Node *curNode=new Node; curNode->x=cx; curNode->y=cy; curNode->z=cz; curNode->g=preNode->g+getDis(cx,cy,cz,preNode->x,preNode->y,preNode->z); curNode->h=getDis(cx,cy,cz,ex,ey,ez); curNode->f=curNode->g+curNode->h; curNode->parent=preNode; if(cx<0||cy<0||cz<0||cx>=m||cy>=n||cz>=k) continue; //越界 或碰墙 else if(mmap[cz][cx][cy]==WALL) continue; else if(in_List(curNode,startList)||in_List(curNode,stopList)) continue; //在开启或关闭列表 if(cx==ex&&cy==ey&&cz==ez) { setRoad(curNode); printRoad(); return; } add(curNode,startList); } } cout<<"自动寻路失败"<<endl; return; } int main() { while(cin>>k>>m>>n) { if(k==0||n==0||m==0) break; for(int kk=0; kk<k; kk++) { for(int i=0; i<m; i++) { for(int j=0; j<n; j++) { cin>>mmap[kk][i][j]; if(mmap[kk][i][j]==START) { sx=i,sy=j,sz=kk; } if(mmap[kk][i][j]==STOP) { ex=i,ey=j,ez=kk; } } } } bfs(); //printRoad(); } return 0; }
测试样例:
3 10 8 ````#``` ``###``` ``###``` ``#````` ``#`##`` ``#`#``# `##`#``# `###`##` `0##`##` ####`#`` ````#``` ``#`#``` ``#`#``` ``#````` ``#`##`` ``#`#``# `##`#`## `###`##` ``##`##` ####`#`` ````#``` ``#`#``` ``#`#``` ``#````` ``#`##`` ``#`#``# `##`#``# `###`##` ``#``##` ####`#`1
输出结果:
人生如修仙,岂是一日间。何时登临顶,上善若水前。