跟二维地图原理一样，只不过搜索方向多了，二维只搜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
输出结果：
