main
John Lauer Astra ESC routing: independent planner, exact replay and narration handoff 2cc6a9e 23d ago
#include <bits/stdc++.h>
using namespace std;
int main(){int W,H,s;cin>>W>>H>>s;int N=W*H;vector<unsigned char>b(3*N),goal(2*N);ifstream f("mask.bin",ios::binary);f.read((char*)b.data(),3*N);ifstream g("goal.bin",ios::binary);g.read((char*)goal.data(),2*N);int tx,ty;cin>>tx>>ty;auto heur=[&](int a){int p=a%N,x=p%W,y=p/W;double dx=abs(x-tx),dy=abs(y-ty);return max(dx,dy)+.41421356*min(dx,dy);}; vector<double>d(2*N,1e30);vector<int>prev(2*N,-1);priority_queue<pair<double,int>,vector<pair<double,int>>,greater<pair<double,int>>>q;d[s]=0;q.push({heur(s),s});int end=-1,count=0;int dx[]={1,1,0,-1,-1,-1,0,1},dy[]={0,1,1,1,0,-1,-1,-1};while(!q.empty()){auto [cost,a]=q.top();q.pop();if(cost>d[a]+heur(a)+1e-5)continue;if(goal[a]){end=a;break;}if(++count>1500000)break;int z=a/N,p=a%N,x=p%W,y=p/W;for(int k=0;k<9;k++){int v;double c;if(k==8){if(b[2*N+p])continue;v=(1-z)*N+p;c=35;}else{int xx=x+dx[k],yy=y+dy[k];if(xx<0||yy<0||xx>=W||yy>=H)continue;v=z*N+yy*W+xx;c=k%2?1.41421356:1.;if(k%2&&(b[z*N+y*W+xx]||b[z*N+yy*W+x]))continue;}if(b[v])continue;double nd=d[a]+c;if(nd+1e-8<d[v]){d[v]=nd;prev[v]=a;q.push({nd+heur(v),v});}}} if(end<0){cout<<"[]";return 0;}vector<int>path;for(int a=end;a>=0;a=prev[a])path.push_back(a);reverse(path.begin(),path.end());cout<<'[';for(int i=0;i<path.size();i++){if(i)cout<<',';cout<<path[i];}cout<<']';}