三维地图中的A*寻路-白红宇

三维地图中的A*寻路

阅读量：5266 次

发布时间：2019-06-14

本文共 3415 字，大约阅读时间需要 11 分钟。

跟二维地图原理一样，只不过搜索方向多了，二维只搜8个方向，而三维要搜26个方向。

不懂的看我以前写的文章，这里直接贴代码：

#include 
     
      #include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #include 
           
            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
            
              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
             
               mlist){ for(list
              
               ::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
               
                 &mlist){ for(list
                
                 ::iterator it=mlist.begin(); it!=mlist.end(); it++) { if(pnode==(*it)) { mlist.erase(it); return true; } } return false;}//向列表中添加节点void add(Node *pnode,list
                 
                   &mlist){ mlist.push_back(pnode); return;}//从列表中获取f最小的节点Node* getMin(list
                  
                    mlist){ double mmin=100000000; Node *temp=NULL; for(list
                   
                    ::iterator it=mlist.begin(); it!=mlist.end(); it++) { if((*it)->f
                    
                     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
                     
                      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<<"-.-"<
                      
                       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<<"自动寻路失败"<
                       
                        >k>>m>>n) { if(k==0||n==0||m==0) break; for(int kk=0; kk
                        
                         >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

输出结果：