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

}

　　测试样例：

````#```

``###```

``###```

``#`````

``#`##``

``#`#``#

`##`#``#

`###`##`

`##`##`

####`#``

````#```

``#`#```

``#`#```

``#`````

``#`##``

``#`#``#

`##`#`##

`###`##`

``##`##`

####`#``

````#```

``#`#```

``#`#```

``#`````

``#`##``

``#`#``#

`##`#``#

`###`##`

``#``##`

####`#`

输出结果：

巴特西

三维地图中的A*寻路

最新文章

热门文章