MENU

A* 算法实战!

2020 年 05 月 18 日 • 技术

C++核心代码,使用 python 做可视化!

1号数据可视化
2号数据可视化
3号数据可视化
5号数据可视化

//A* 寻路代码
#include <iostream>
#include <algorithm>
#include <string>
#include <cmath>
#include <queue>
#include <stack>
#include <fstream>
using namespace std;

#define XMAX 99//[0]-[99]
#define YMAX 99
int map[XMAX+1][YMAX+1];
int costMap[XMAX][YMAX];
int desX=80,desY=80;//不用管。。。
int strX=3,strY=10;
struct Block
{
    int x;
    int y;
    int pX;//上一个Block,即箭头指向的block坐标
    int pY;
    int f;//总代价
    int g;//离起点代价
    int h;//离终点预估代价,这里使用曼哈顿距离
};
vector<Block> open;
vector<Block> close;

bool cmp(Block a, Block b) {
    return a.f > b.f;
}

inline bool operator == (const Block &a,const Block &b)
{
    return a.x==b.x&&a.y==b.y;
}

bool isSafe(int x,int y)//无路障(也无膨胀路障)
{
    if(x<0||y<0||x>XMAX||y>YMAX)//超边界了
        return false;
    return map[x][y]==0&&costMap[x][y]==0;
}

bool isStr(int x,int y)//是否起点
{
    return x==strX&&y==strY;
}

bool isDes(int x,int y)//是否为目的地
{
    return x==desX&&y==desY;
}


void calH(struct Block &block)//“刷新”h,曼哈顿
{
    block.h = abs(desX-block.x) + abs(desY-block.y);
    block.f=block.g+block.h;
}

Block init(int x, int y, int pX,int pY,int g)//初始化block
{
    Block block;
    block.x=x;
    block.y=y;
    block.pX=pX;
    block.pY=pY;
    calH(block);
    block.g=g;
    block.f=block.g+block.h;

    
    return block;
}

Block chooseMinInOpen()//选择open里面f最小的
{
    return open[0];
}

void insertOpen(Block block)
{//最小的放首位
    for(int i=0;i<open.size();i++)
    {
        if(open[i].f>=block.f)
        {
            open.insert(open.begin()+i,block);
            return;
        }
    }
    open.emplace_back(block);
}
void insertClose(Block block)
{
    close.emplace_back(block);
}

void removeOpen(Block block)
{
    vector<Block>::iterator iter=find(open.begin(),open.end(),block);
    open.erase(iter);
}

int inClose(int x,int y)//返回close里的下标,无返-1
{
    for(int i=0;i<close.size();i++)
    {
        if(close[i].x==x&&close[i].y==y)
            return i;
    }
    return -1;
}

int inOpen(int x,int y)
{
    for(int i=0;i<open.size();i++)
    {
        if(open[i].x==x&&open[i].y==y)
            return i;
    }
    return -1;
}

bool next(int x,int y,Block parent)//next来的有des则返回true
{
    if(!isSafe(x,y)||inClose(x,y)!=-1)//在close或封堵
        return false;
    int iopen = inOpen(x,y);
    if(iopen!=-1)
    {//在open,看看会不会更优
        if(parent.g+1<open[iopen].g)
        {
            open[iopen].g=parent.g+1;
            open[iopen].pX=parent.x;
            open[iopen].pY=parent.y;
        }
    }
    else//不在close,不在open,不是障碍
    {
        Block b = init(x,y,parent.x,parent.y,parent.g+1);
        insertOpen(b);
    }
    if(isDes(x,y))
        return true;
    else
        return false;
}

void printpath()
{
    ofstream ofs;
    ofs.open("output.txt", ios::app);
    ofs<<"[PATH]"<<endl;
    stack<Block> rt;
    Block cur = open[inOpen(desX,desY)];
    while (true)
    {
        rt.push(cur);
        if(!isStr(cur.pX,cur.pY))
        {
            int t = inOpen(cur.pX,cur.pY);
            if(t!=-1)
                cur = open[t];
            else
                cur = close[inClose(cur.pX,cur.pY)];
        }
        else
        {
            break;
        }
    }
    cout<<strX<<" "<<strY<<endl;
    ofs<<strX<<" "<<strY<<endl;
    while (!rt.empty())
    {
        cout<<rt.top().x<<" "<<rt.top().y<<endl;
        ofs<<rt.top().x<<" "<<rt.top().y<<endl;
        rt.pop();
    }
    ofs.close();
}

void printgraph()
{
    ofstream ofs;
    ofs.open("output.txt", ios::out);
    ofs<<XMAX<<","<<YMAX<<endl;
    Block block;
    for(int y=0;y<=YMAX;y++)
    {
        for(int x=0;x<=XMAX;x++)
        {
            if(isStr(x,y))
            {
                cout<<"S";
                ofs<<"S";
                continue;
            }
            if(isDes(x,y))
            {
                cout<<"E";
                ofs<<"E";
                continue;
            }
            if(map[x][y])
            {
                cout<<"1";
                ofs<<"1";
                continue;
            }
            else
            {
                int t;
                t = inOpen(x,y);
                if(t==-1)
                {
                    t = inClose(x,y);
                    if(t==-1)
                    {
                        cout<<"0";
                        ofs<<"0";
                        continue;
                    }
                    else
                    {
                        block = close[t];
                    }
                }
                else
                {
                    block = open[t];
                }
                //有具体方向
                if(block.pY>y)
                {
                    cout<<"^";
                    ofs<<"^";
                }
                else if(block.pY<y)
                {
                    cout<<"v";
                    ofs<<"v";
                }
                else if(block.pX<x)
                {
                    cout<<">";
                    ofs<<">";
                }
                else if(block.pX>x)
                {
                    cout<<"<";
                    ofs<<"<";
                }
            }
        }
        cout<<endl;
        ofs<<endl;
    }
    ofs.close();
}

void astar()
{
    Block start = init(strX,strY,0,0,0);
    Block cur = start;
    insertOpen(cur);
    while (!isDes(cur.x,cur.y))
    {
        insertClose(cur);
        removeOpen(cur);
        bool rt = //上下左右访问
        next(cur.x,cur.y+1,cur)||
        next(cur.x,cur.y-1,cur)||
        next(cur.x-1,cur.y,cur)||
        next(cur.x+1,cur.y,cur);
        if(rt)
            break;//找到终点了
        if(open.size()==0)
        {
            cout<<"No path"<<endl;
            return;
        }
        cur=chooseMinInOpen();
        cout<<"";
    }
}

bool expand(int r)
{
    for(int y=0;y<YMAX;y++)
    {
        for(int x=0;x<XMAX;x++)
        {
            if(map[x][y])
            {
                for(int i=1;i<=r;i++)
                {
                    if( isStr(x+i,y)||isStr(x-i,y)||isStr(x,y+i)||isStr(x,y-i)||//膨胀区域内有起始点
                        isDes(x+i,y)||isDes(x-i,y)||isDes(x,y+i)||isDes(x,y-i))//膨胀区域内有结束点
                    {
                        return false;
                    }
                    costMap[x+i][y]=1;//右
                    costMap[x-i][y]=1;//左
                    costMap[x][y+i]=1;//上
                    costMap[x][y-i]=1;//下
                    costMap[x+i][y+i]=1;
                    costMap[x-i][y+i]=1;
                    costMap[x+i][y-i]=1;
                    costMap[x-i][y-i]=1;
                }
            }
        }
    }
    return true;
}

int main()
{
    string in;
    ifstream file;
    file.open("map.txt");
    char t;
    int x,y;
    x=0;y=0;
    while (!file.eof())
    {
        file>>t;
        if(t=='1')
        {
            map[x][y]=1;
        }
        else if(t=='S')
        {
            strX=x;
            strY=y;
        }
        else if(t=='E')
        {
            desX=x;
            desY=y;
        }
        if(x>=XMAX)
        {
            x=0;
            y++;
        }
        else
        {
            x++;
        }
    }
    int exr;
    cout<<"expand R:";
    cin>>exr;
    if(!expand(exr))
    {
        cout<<"expand failed"<<endl;
        return 0;
    }
    astar();
    printgraph();//先graph再path,因为里面有写文件逻辑
    printpath();
    cout<<"output.txt created. Use py file to visualize it!";
}
import matplotlib.pyplot as plt
import matplotlib.patches as patches
import numpy as np

fig = plt.figure(figsize=(20, 20))
ax1 = fig.add_subplot(111, aspect='equal')

file = open("output.txt")
maxX=0
maxY=0
x=0
y=0
lx=-1
ly=-1
pathFlag = False
for line in file:
    if line =='\n':
        break
    if line =='[PATH]\n':
        pathFlag = True
        continue
    
    if pathFlag:
        if lx==-1:
            x=int(line.split(' ')[0])
            y=int(line.split(' ')[1])
            lx=0
            continue
        lx=x
        ly=y
        x=int(line.split(' ')[0])
        y=int(line.split(' ')[1])
        ax1.add_patch(patches.Rectangle((lx+0.5, ly+0.5),x-lx+0.1,y-ly+0.1,color='blue'))
        #ax1.add_patch(patches.ConnectionPatch((lx+0.5,ly+0.5),(x+0.5,y+0.5),'data',color='red'))
        continue

    if maxX==0:
        maxX=int(line.split(',')[0])
        maxY=int(line.split(',')[1])
        continue
    x=0
    
    for single in line:
        if single == '0':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='grey',edgecolor='black'))
        elif single == '1':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='purple',edgecolor='black'))
        elif single == 'S':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='green',edgecolor='black'))
        elif single == 'E':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='red',edgecolor='black'))
        elif single == '^':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='orange',edgecolor='black'))
            ax1.add_patch(patches.Arrow(x+0.5,y+0.7,0,-0.5,width=0.5,facecolor='white',edgecolor='black'))
        elif single == 'v':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='orange',edgecolor='black'))
            ax1.add_patch(patches.Arrow(x+0.5,y+0.3,0,+0.5,width=0.5,facecolor='white',edgecolor='black'))
        elif single == '<':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='orange',edgecolor='black'))
            ax1.add_patch(patches.Arrow(x+0.8,y+0.5,-0.5,0,width=0.5,facecolor='white',edgecolor='black'))
        elif single == '>':
            ax1.add_patch(patches.Rectangle((x, y),1,1,facecolor='orange',edgecolor='black'))
            ax1.add_patch(patches.Arrow(x+0.3,y+0.5,+0.5,0,width=0.5,facecolor='white',edgecolor='black'))
        x=x+1
    y=y+1


plt.xlim(0,maxX+1)
plt.ylim(0,maxY+1)
ax = plt.gca()
ax.xaxis.set_ticks_position('top')
ax.invert_yaxis()
plt.savefig('astarv.png', bbox_inches='tight')
最后编辑于: 2020 年 07 月 24 日
返回文章列表 文章二维码
本页链接的二维码
打赏二维码