function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
rowwind=squeeze(chart(:,:,1));
colwind=squeeze(chart(:,:,2));
beamsize=400;
depth=100;
thrust=ones(1,beamsize);
positionr=zeros(depth,beamsize);
positionc=zeros(depth,beamsize);
velocityr=zeros(1,beamsize);
velocityc=zeros(1,beamsize);
parent=zeros(depth,beamsize);
[rows,cols]=size(rowwind);
if maxThrottle<2
movesr=[-1 0 1 0 0];
movesc=[0 -1 0 -1 0];
movethrust=[1 1 1 1 0];
else
movesr1=[-1 -1 0 1 1 1 0 -1];
movesc1=[0 -1 -1 -1 0 1 1 1];
movesr=[0 movesr1];
movesc=[0 movesc1];
for r=2:maxThrottle-1
for c=2:maxThrottle-r
movesr=[movesr r*movesr1];
movesc=[movesc c*movesc1];
end
end
select=rand(size(movesr))<80/length(movesr);
movesr=movesr(select);
movesc=movesc(select);
movethrust=abs(movesr)+abs(movesc);
end
iter=1;
ind=1;
bestvelc=0;
bestvelr=0;
bestthrust=0;
bestiter=1;
bestscore=1e10;
turned=0;
sols=1;
[r,c]=ind2sub(size(rowwind),aIndex);
positionr(1,1)=r;
positionc(1,1)=c;
[targetr,targetc]=ind2sub(size(rowwind),bIndex);
%distance=sqrt((targetr-r)^2+(targetc-c)^2);
notfound=1;
while notfound&&iter<depth
scores=zeros(1,sols*length(movesr));
newposr=zeros(1,sols*length(movesr));
newposc=zeros(1,sols*length(movesr));
newvelr=zeros(1,sols*length(movesr));
newvelc=zeros(1,sols*length(movesr));
newthrust=zeros(1,sols*length(movesr));
index=0;
for i=1:sols
for j=1:length(movesr)
index=index+1;
if positionr(iter,i)~=0
newvelr(index)=velocityr(i)+movesr(j)+rowwind(positionr(iter,i),positionc(iter,i));
newvelc(index)=velocityc(i)+movesc(j)+colwind(positionr(iter,i),positionc(iter,i));
newposr(index)=positionr(iter,i)+newvelr((i-1)*length(movesr)+j);
newposc(index)=positionc(iter,i)+newvelc((i-1)*length(movesr)+j);
newthrust(index)=movethrust(j)+thrust(i);
else
newposr(index)=0;
end
if newposr(index)<1||newposr(index)>rows||newposc(index)<1|| ...
newposc(index)>cols;
scores(index)=1e10;
newposr(index)=0;
else
scores(index)=newthrust(index)*((newposc(index)-targetc).^2+(newposr(index)-targetr).^2+20) ...
+(1-turned)*20*(newvelc(index)^2+newvelr(index)^2);
end
end
end
iter=iter+1;
if length(scores)>beamsize
[s,ind]=sort(scores);
ind=ind(1:beamsize);
thrust=newthrust(ind);
velocityr=newvelr(ind);
velocityc=newvelc(ind);
positionr(iter,:)=newposr(ind);
positionc(iter,:)=newposc(ind);
parent(iter,:)=floor((ind-1)/length(movesr))+1;
sols=beamsize;
else
thrust(1:length(scores))=newthrust;
velocityr(1:length(scores))=newvelr;
velocityc(1:length(scores))=newvelc;
positionr(iter,1:length(scores))=newposr;
positionc(iter,1:length(scores))=newposc;
parent(iter,1:length(scores))=floor((0:length(scores)-1)/length(movesr))+1;
sols=length(scores);
end
if scores(ind(1))<bestscore
bestscore=scores(ind(1));
bestiter=iter;
bestthrust=thrust;
bestvelr=velocityr;
bestvelc=velocityc;
end
if iter-bestiter>5||(~turned&&iter>depth/2)
if turned
notfound=0;
else
turned=1;
[targetr,targetc]=ind2sub(size(rowwind),aIndex);
iter=bestiter;
thrust=bestthrust;
velocityr=bestvelr;
velocityc=bestvelc;
sols=1;
ind=1;
bestscore=1e10;
end
end
end
p=1;
for i=bestiter:-1:1
trajectoryr(i)=positionr(i,p);
trajectoryc(i)=positionc(i,p);
p=parent(i,p);
end
accelr=diff([0 diff(trajectoryr)]);
accelc=diff([0 diff(trajectoryc)]);
if length(trajectoryc)==1
thrustRow = [];
thrustCol = [];
else
thrustRow = accelr-rowwind(sub2ind(size(rowwind),trajectoryr(1:end-1),trajectoryc(1:end-1)));
thrustCol = accelc-colwind(sub2ind(size(rowwind),trajectoryr(1:end-1),trajectoryc(1:end-1)));
end
end
|