C++核心代码，使用 python 做可视化！
//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')