seveibar/am3352-dev-board

scripts/maze.cpp

#include <cstdio>
#include <vector>
#include <queue>
#include <cmath>
#include <algorithm>
using namespace std;
struct Node{float f,g;int i;bool operator<(const Node&o)const{return f>o.f;}};
int main(int argc,char**argv){FILE*f=fopen(argv[1],"rb");int W,H,L,S;fread(&W,4,1,f);fread(&H,4,1,f);fread(&L,4,1,f);fread(&S,4,1,f);int N=W*H,T=N*L;vector<unsigned char>b(T),target(T),via(N);vector<int>starts(S);fread(b.data(),1,T,f);fread(target.data(),1,T,f);fread(via.data(),1,N,f);fread(starts.data(),4,S,f);fclose(f);
 int xmin=W,xmax=0,ymin=H,ymax=0;for(int i=0;i<T;i++)if(target[i]){int k=i%N;xmin=min(xmin,k%W);xmax=max(xmax,k%W);ymin=min(ymin,k/W);ymax=max(ymax,k/W);}auto heuristic=[&](int i){int k=i%N,x=k%W,y=k/W;float dx=max({xmin-x,0,x-xmax}),dy=max({ymin-y,0,y-ymax});return max(dx,dy)+.41421356f*min(dx,dy);};
 vector<float>d(T,1e30);vector<int>p(T,-1);priority_queue<Node>q;for(int s:starts)if(!b[s]){d[s]=0;q.push({heuristic(s),0,s});}int end=-1,iterations=0;int dx[]={1,-1,0,0,1,1,-1,-1},dy[]={0,0,1,-1,1,-1,1,-1};
 while(!q.empty()){auto n=q.top();q.pop();int i=n.i;if(n.g!=d[i])continue;if(target[i]){end=i;break;}iterations++;int k=i%N,x=k%W,y=k/W,z=i/N;auto relax=[&](int j,float cost){float nd=n.g+cost;if(nd<d[j]&&!b[j]){d[j]=nd;p[j]=i;q.push({nd+heuristic(j),nd,j});}};for(int a=0;a<8;a++){int xx=x+dx[a],yy=y+dy[a];if(xx<0||xx>=W||yy<0||yy>=H)continue;if(a>=4&&(b[z*N+y*W+xx]||b[z*N+yy*W+x]))continue;relax(z*N+yy*W+xx,a<4?1:1.41421356f);}if(via[k])for(int zz=0;zz<L;zz++)if(zz!=z)relax(zz*N+k,via[k]==2?.2:60);}
 vector<int>path;if(end>=0)for(int i=end;i>=0;i=p[i])path.push_back(i);reverse(path.begin(),path.end());FILE*out=fopen(argv[2],"wb");int size=path.size();fwrite(&size,4,1,out);fwrite(path.data(),4,size,out);fclose(out);printf("Maze expanded %d nodes; path %d nodes\n",iterations,size);return end<0?2:0;}