三维地图中的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;
}

　　测试样例：

````#```
``###```
``###```
``#`````
``#`##``
``#`#``#
`##`#``#
`###`##`
`##`##`
####`#``

````#```
``#`#```
``#`#```
``#`````
``#`##``
``#`#``#
`##`#`##
`###`##`
``##`##`
####`#``

````#```
``#`#```
``#`#```
``#`````
``#`##``
``#`#``#
`##`#``#
`###`##`
``#``##`
####`#`

输出结果：