function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
[thrustRow, thrustCol, colWind, rowWind, nC, nR, AC, AR, BC, BR] = nickelfel(chart, aIndex, bIndex, maxThrottle);
score1 = runsolution(thrustRow, thrustCol, colWind, rowWind, AC, AR, BC, BR);
if score1<32
return;
elseif score1>53
[thrustRowc, thrustColc] = nick(colWind, rowWind, nC, nR, AC, AR, BC, BR, maxThrottle);
scorea = runsolution(thrustRowc, thrustColc, colWind, rowWind, AC, AR, BC, BR);
if scorea < score1
thrustCol = thrustColc;
thrustRow = thrustRowc;
end
return;
else
end
[thrustRowa, thrustCola,scorea] = peter(chart, aIndex, bIndex, maxThrottle);
if scorea < score1 && size(thrustRowa,1) <= size(thrustRow,1)
thrustCol = thrustCola;
thrustRow = thrustRowa;
end
end
function [n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(iW, jW, ncol, nrow, aj, ai, bj, bi, maxThrottle)
thrustRow = zeros(1,1000);
thrustCol = zeros(1,1000);
vws = inf(1,1000);
% set up initial conditions
pi = ai;
pj = aj;
vi = 0;
vj = 0;
n = 0;
mtt = 405;
mt = ceil(0.71*maxThrottle);
mt2 = ceil(0.81*maxThrottle);
sqd = inf;
while (sqd>100)
sqd = inf;
mcum = 0;
stopn = 0;
srec = [pi pj vi vj];
while (all([n<1000 pi*pj>=1 pi<=nrow pj<=ncol])&&any([pi pj]~=[bi bj]))
[gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
[mgs,mgsi] = min(gs);
if (mgs > 1)
[wk,mi,mj,ws] = lesscycstill(n,sqd,mtt,ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,mt,mt2);
else
ws = inf;
wk = 1;
mi = 0;
mj = 0;
end;
if (mgs<100)&&(mgs <= ws(wk)+abs(mi)+abs(mj))
% end early
n = n+mgsi-1;
pi = ti(mgsi);
pj = tj(mgsi);
vi = ui(mgsi);
vj = uj(mgsi);
vws(n) = mgs;
break
end;
% stop if too much motor
mcum = mcum+abs(mi)+abs(mj);
if (mcum>sqd)
% undo moves:
thrustRow(stopn+1:n) = 0;
thrustCol(stopn+1:n) = 0;
n = stopn;
% return to position:
pi = srec(1);
pj = srec(2);
vi = srec(3);
vj = srec(4);
pi = pi+vi;
pj = pj+vj;
% add coasting
[gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
[~,mgsi] = min(gs);
n = n+mgsi-1;
pi = ti(mgsi);
pj = tj(mgsi);
vi = ui(mgsi);
vj = uj(mgsi);
break;
end;
% apply winning move
thrustRow(n+wk) = mi;
thrustCol(n+wk) = mj;
n = n+wk;
pi = ti(wk);
pj = tj(wk);
vi = ui(wk+1)+mi;
vj = uj(wk+1)+mj;
vws(n) = ws(wk);%+(pi-ai).^2+(pj-aj).^2;
if (ws(wk)<sqd)
sqd = ws(wk);
mcum = 0;
stopn = n;
srec = [pi pj vi vj];
end;
% advance one turn
pi = pi+vi;
pj = pj+vj;
end;
if (sqd > 100)
if (mt < mt2)
% reset and try again with full thrust
thrustRow = zeros(1,1000);
thrustCol = zeros(1,1000);
vws = inf(1,1000);
pi = ai;
pj = aj;
vi = 0;
vj = 0;
n = 0;
mt = mt2;
elseif (mt <= maxThrottle)
% reset and try again with full thrust
thrustRow = zeros(1,1000);
thrustCol = zeros(1,1000);
vws = inf(1,1000);
pi = ai;
pj = aj;
vi = 0;
vj = 0;
n = 0;
mt = maxThrottle;
else
% give up
sqd = 0;
end;
end;
end;
bs = ws(wk);
bn = n;
[vpi,vpj] = mapMoves(ai,aj,iW,jW,thrustRow(1:n),thrustCol(1:n));
dB = (vpi-bi).^2+(vpj-bj).^2;
end
function [wk,mi,mj,ws] = lesscycstill(n,sqd,mtt,ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,mt,mt2)
if (n>0)&&(sqd>mtt)
% use maxThrottle to get out of difficult situation
[wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,1000-n);
else
[wk,mi,mj,ws] = lowercyc1(ti,tj,ui,uj,bi,bj,iW,jW,mt2,n,maxThrottle,mt);
end;
end
function [thrustRow, thrustCol] = nick(jW, iW, ncol, nrow, aj, ai, bj, bi, maxThrottle)
mt = ceil(0.7*maxThrottle);
mt2 = ceil(0.77*maxThrottle);
vws = inf(1,1000);
% set up initial conditions
mtt = 403;
% try to approach b
[n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(iW, jW, ncol, nrow, aj, ai, bj, bi, maxThrottle);
% now try to approach a
sqd = inf;
while (bn>1)&&(sqd > dB(bn-1))
sqd = inf;
mcum = 0;
stopn = bn;
srec = [pi pj vi vj];
while (all([n<1000 pi*pj>=1 pi<=nrow pj<=ncol])&&any([pi pj]~=[ai aj]))
[gs,ti,tj,ui,uj] = score_glide(ai,aj,pi,pj,vi,vj,iW,jW,1000-n);
[mgs,mgsi] = min(gs);
if (mgs > 1)
if (n>bn)&&(sqd>mtt)
% use maxThrottle to get out of difficult situation
[wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
else
[wk,mi,mj,ws] = lowercyc2(ti,tj,ui,uj,ai,aj,iW,jW,mt2,n,maxThrottle,mt);
end;
else
ws = inf;
wk = 1;
mi = 0;
mj = 0;
end;
if (mgs<100)&&(mgs <= ws(wk)+abs(mi)+abs(mj))
% end early
n = n+mgsi-1;
pi = ti(mgsi);
pj = tj(mgsi);
vi = ui(mgsi);
vj = uj(mgsi);
vws(n) = mgs;
break
end;
% stop if too much motor
mcum = mcum+abs(mi)+abs(mj);
if (isinf(ws(wk)))||(mcum>sqd)
% undo moves:
thrustRow(stopn+1:n) = 0;
thrustCol(stopn+1:n) = 0;
n = stopn;
% return to position:
pi = srec(1);
pj = srec(2);
vi = srec(3);
vj = srec(4);
pi = pi+vi;
pj = pj+vj;
% add coasting
[gs,ti,tj,ui,uj] = score_glide2(bi,bj,pi,pj,vi,vj,iW,jW,1000-n,mt);
[~,mgsi] = min(gs);
n = n+mgsi-1;
pi = ti(mgsi);
pj = tj(mgsi);
vi = ui(mgsi);
vj = uj(mgsi);
break;
end;
% apply winning move
thrustRow(n+wk) = mi;
thrustCol(n+wk) = mj;
n = n+wk;
pi = ti(wk);
pj = tj(wk);
vi = ui(wk+1)+mi;
vj = uj(wk+1)+mj;
vws(n) = bs+ws(wk);
[sqd,mcum,stopn,srec] = lesscyc(ws,wk,sqd,n,pi,pj,vi,vj) ;
% advance one turn
pi = pi+vi;
pj = pj+vj;
end;
end;
% trim excess
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
end
function [sqd,mcum,stopn,srec] = lesscyc(ws,wk,sqd,n,pi,pj,vi,vj)
if (ws(wk)<sqd)
sqd = ws(wk);
mcum = 0;
stopn = n;
srec = [pi pj vi vj];
end;
end
function [wk,mi,mj,ws] = lowercyc2(ti,tj,ui,uj,ai,aj,iW,jW,mt2,n,maxThrottle,mt)
[wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,mt,1000-n);
if (isinf(ws(wk)))
[wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,mt2,1000-n);
if (isinf(ws(wk)))
[wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,ai,aj,iW,jW,maxThrottle,1000-n);
end;
end;
end
function [wk,mi,mj,ws] = lowercyc1(ti,tj,ui,uj,bi,bj,iW,jW,mt2,n,maxThrottle,mt)
[wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,mt,1000-n);
if (isinf(ws(wk))&&(mt~=maxThrottle))
[wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,mt2,1000-n);
if (isinf(ws(wk))&&(mt~=maxThrottle))
[wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,bi,bj,iW,jW,maxThrottle,1000-n);
end;
end;
end
function [wk,mi,mj,ws] = pickNext(ti,tj,ui,uj,gi,gj,iW,jW,mt,N)
n = numel(ti);
wvi = zeros(1,n);
wvj = zeros(1,n);
ws = zeros(1,n);
for k = 1:n
ks = inf(2*mt+1);
for i = -mt:mt
for j = -(mt-abs(i)):(mt-abs(i))
ks(i+mt+1,j+mt+1) = min(score_glide(gi,gj,ti(k),tj(k),ui(k)+i,uj(k)+j,iW,jW,N-k))+abs(i)+abs(j);
end;
end;
ks(mt+1,mt+1) = inf;
[m1,m1i] = min(ks);
[ws(k),m2i] = min(m1);
wvi(k) = m1i(m2i)-mt-1;
wvj(k) = m2i-mt-1;
ws(k) = ws(k)-abs(wvi(k))-abs(wvj(k));
end;
[~,wk] = min(ws);
mi = wvi(wk);
mj = wvj(wk);
end
function [wk,mi,mj,ws] = pickNext2(ti,tj,ui,uj,gi,gj,iW,jW,mt,N)
n = numel(ti);
wvi = zeros(1,n);
wvj = zeros(1,n);
ws = zeros(1,n);
for k = 1:n
ks = inf(2*mt+1);
for i = -mt:mt
for j = -(mt-abs(i)):(mt-abs(i))
ks(i+mt+1,j+mt+1) = min(score_glide2(gi,gj,ti(k),tj(k),ui(k)+i,uj(k)+j,iW,jW,N-k,mt))+abs(i)+abs(j);
end;
end;
ks(mt+1,mt+1) = inf;
[m1,m1i] = min(ks);
[ws(k),m2i] = min(m1);
wvi(k) = m1i(m2i)-mt-1;
wvj(k) = m2i-mt-1;
ws(k) = ws(k)-abs(wvi(k))-abs(wvj(k));
end;
[mws,wk] = min(ws);
if isinf(mws)
mi = 0;
mj = 0;
else
mi = wvi(wk);
mj = wvj(wk);
end;
end
function [s,ti,tj,ui,uj] = score_glide(gi,gj,pi,pj,vi,vj,iW,jW,n)
[ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n);
s = (ti-gi).^2+(tj-gj).^2;
s(1) = inf;
end
function [s,ti,tj,ui,uj] = score_glide2(gi,gj,pi,pj,vi,vj,iW,jW,n,mt)
[ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n);
s = (ti-gi).^2+(tj-gj).^2+max(0,abs(ui(2:end))+abs(uj(2:end))-mt/2).^2;
s(1) = inf;
end
% returns the future trajectory given initial point and velocity
% computes up to n steps ahead
function [ti,tj,ui,uj] = glide(pi,pj,vi,vj,iW,jW,n)
[nrow,ncol] = size(iW);
n = min(ceil(sqrt(nrow*ncol)),n);
seen = false(nrow,ncol);
ti = zeros(1,n);
tj = ti;
ui = ti;
uj = ti;
ti(1) = pi;
tj(1) = pj;
ui(1) = vi;
uj(1) = vj;
ui(2) = vi+iW(pi,pj);
uj(2) = vj+jW(pi,pj);
for k = 2:n
seen(pi,pj) = true;
vi = ui(k);
vj = uj(k);
pi = pi+vi;
pj = pj+vj;
if (((pi<1)||(pj<1)||(pi>nrow)||(pj>ncol))||(seen(pi,pj)))
ti = ti(1:k-1);
tj = tj(1:k-1);
ui = ui(1:k);
uj = uj(1:k);
break;
end;
ti(k) = pi;
tj(k) = pj;
ui(k+1) = vi+iW(pi,pj);
uj(k+1) = vj+jW(pi,pj);
end;
end
function [vpi,vpj,vvi,vvj] = mapMoves(ai,aj,iW,jW,thrustRow,thrustCol)
n = numel(thrustRow);
vpi = zeros(1,n+1);
vpj = zeros(1,n+1);
vvi = zeros(1,n+1);
vvj = zeros(1,n+1);
vpi(1) = ai;
vpj(1) = aj;
vvi(1) = 0;
vvj(1) = 0;
for k = 1:n
vvi(k+1) = vvi(k)+iW(vpi(k),vpj(k))+thrustRow(k);
vvj(k+1) = vvj(k)+jW(vpi(k),vpj(k))+thrustCol(k);
vpi(k+1) = vpi(k)+vvi(k+1);
vpj(k+1) = vpj(k)+vvj(k+1);
end;
end
function score = runsolution(thrustRow, thrustCol, colWind, rowWind, AC, AR, BC, BR)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
% Initialize variables at start point (A)
fR = AR; fC =AC;
fvR = 0; fvC = 0;
dB = (fR-BR)^2 + (fC-BC)^2;
for i = 1:numel(thrustRow)
ivR = fvR + thrustRow(i) + rowWind(fR,fC);
ivC = fvC + thrustCol(i) + colWind(fR,fC);
iR = fR + ivR;
iC = fC + ivC;
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
if ((fR-BR)^2 + (fC-BC)^2) < dB
dB = (fR-BR)^2 + (fC-BC)^2;
end
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end
function [thrustRow, thrustCol, x_winds, y_winds, nx, ny, ax, ay, bx, by] = nickelfel(chart, aIndex, bIndex, maxThrottle)
y_winds = chart(:,:,1);
x_winds = chart(:,:,2);
[ny,nx] = size(x_winds);
ay = rem(aIndex-1, ny) + 1;
ax = (aIndex - ay)/ny + 1;
by = rem(bIndex-1, ny) + 1;
bx = (bIndex - by)/ny + 1;
x = (1:nx);
y = (1:ny)';
X = x(ones(ny,1),:);
Y = y(:,ones(nx,1));
%maxIter = 1170;
maxIter = 1034;
INF = 100000;
dist_to_B = (X - bx).^2 + (Y - by).^2;
dist_to_A = (X - ax).^2 + (Y - ay).^2;
dist_to_B_w = dist_to_B/10000;
dist_to_A_w = dist_to_A/10000;
y_dir = 2*(by > ay) - 1;
x_dir = 2*(bx > ax) - 1;
fuel = Inf(ny,nx);
fuel(aIndex) = 0;
fuel_to_reverse = fuel;
xvel = zeros(ny,nx);
yvel = zeros(ny,nx);
checked = zeros(ny,nx);
previous_stop = zeros(ny,nx);
it = 0;
min_fuel = 0;
[cost_to_B cost_to_B_idx] = min(fuel_to_reverse(:) + dist_to_B(:));
while ( min_fuel <= cost_to_B && ~all(checked(:)) && it < maxIter)
it = it + 1;
metrix = fuel + checked + dist_to_B_w;
[~,i_p] = min(metrix(:));
min_fuel = fuel(i_p);
i_y = rem(i_p - 1,ny) + 1;
i_x = (i_p - i_y)/ny + 1;
i_vy = yvel(i_p) + y_winds(i_p);
i_vx = xvel(i_p) + x_winds(i_p);
i_new_vy = Y - i_y;
i_new_vx = X - i_x;
i_thrust = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
i_fuel = i_thrust + min_fuel;
i_reacheable = i_thrust <= maxThrottle;
i_optimum = i_fuel < fuel;
i_impr_tf = i_reacheable & i_optimum;
i_impr_idx = find(i_impr_tf);
i_new_vx_impr = i_new_vx(i_impr_idx);
i_new_vy_impr = i_new_vy(i_impr_idx);
i_fuel_impr = i_fuel (i_impr_idx) ;
i_fuel_to_reverse_impr = i_fuel_impr + ...
0.2*abs(i_new_vy_impr).*(i_new_vy_impr*y_dir>0) + ...
0.2*abs(i_new_vx_impr).*(i_new_vx_impr*x_dir>0) ;
xvel (i_impr_idx) = i_new_vx_impr;
yvel (i_impr_idx) = i_new_vy_impr;
fuel (i_impr_idx) = i_fuel_impr;
fuel_to_reverse(i_impr_idx) = i_fuel_to_reverse_impr;
previous_stop (i_impr_idx) = i_p;
if i_impr_tf(cost_to_B_idx)
[cost_to_B cost_to_B_idx] = min(fuel_to_reverse(:) + dist_to_B(:));
else
[cost_to_B_new cost_to_B_idx_new] = min(fuel_to_reverse(i_impr_idx) ...
+ dist_to_B(i_impr_idx));
if cost_to_B_new<cost_to_B
cost_to_B = cost_to_B_new;
cost_to_B_idx = i_impr_idx(cost_to_B_idx_new);
end
end
checked(i_impr_idx) = 0; %false;
checked(i_p) = INF; %true;
end
fuel = fuel + dist_to_B;
[cost_to_A cost_to_A_idx] = min(fuel(:) + dist_to_A(:));
checked = checked*0;
return_previous_stop = zeros(ny,nx);
it = 0;
min_fuel = 0;
while ( min_fuel < cost_to_A && ~all(checked(:)) && it < maxIter )
it = it + 1;
metrix = fuel + checked + dist_to_A_w; %****
[~,i_p] = min(metrix(:)); %****
min_fuel = fuel(i_p); %****
i_y = rem(i_p - 1,ny) + 1;
i_x = (i_p - i_y)/ny + 1;
i_vy = yvel(i_p) + y_winds(i_p);
i_vx = xvel(i_p) + x_winds(i_p);
i_new_vy = Y-i_y;
i_new_vx = X-i_x;
i_thrust = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
i_fuel = i_thrust + min_fuel;
i_reacheable = i_thrust <= maxThrottle;
i_optimum = i_fuel < fuel;
i_impr_tf = i_reacheable & i_optimum;
i_impr_idx = find(i_impr_tf);
fuel (i_impr_idx) = i_fuel(i_impr_idx);
xvel (i_impr_idx) = i_new_vx(i_impr_idx);
yvel (i_impr_idx) = i_new_vy(i_impr_idx);
return_previous_stop(i_impr_idx) = i_p;
if i_impr_tf(cost_to_A_idx)
[cost_to_A cost_to_A_idx] = min(fuel(:) + dist_to_A(:));
else
[cost_to_A_new cost_to_A_idx_new] = min(fuel(i_impr_idx) ...
+ dist_to_A(i_impr_idx));
if cost_to_A_new<cost_to_A
cost_to_A = cost_to_A_new;
cost_to_A_idx = i_impr_idx(cost_to_A_idx_new);
end
end
checked(i_impr_idx) = 0;
checked(i_p) = INF;
end
cost_to_A = fuel + dist_to_A;
[~,min_fuel] = min(cost_to_A(:));
indices = zeros(nx*ny,1);
indices(1) = min_fuel(1);
next_move = return_previous_stop(indices(1));
k = 1;
while(next_move)
k = k + 1;
indices(k) = next_move;
next_move = return_previous_stop(next_move);
end
next_move = previous_stop(indices(k));
while(next_move)
k = k + 1;
indices(k) = next_move;
next_move = previous_stop(next_move);
end
indices = indices(k:-1:1);
ypos = rem(indices - 1, ny) + 1;
xpos = (indices - ypos)/ny + 1;
xw = x_winds(indices);
yw = y_winds(indices);
xspeed = diff(xpos);
yspeed = diff(ypos);
xdrive = diff([0; xspeed]);
ydrive = diff([0; yspeed]);
thrustCol = xdrive - xw(1:end-1);
thrustRow = ydrive - yw(1:end-1);
end
function [thrustRow, thrustCol,m] = peter(chart, aIndex, bIndex, maxThrottle)
rowwind=chart(:,:,1);
colwind=chart(:,:,2);
[numrows,numcols]=size(rowwind);
ar = rem(aIndex-1, numrows) + 1;
ac = (aIndex - ar)/numrows + 1;
br = rem(bIndex-1, numrows) + 1;
bc = (bIndex - br)/numrows + 1;
% minr=max([1 min([ar-5 br-5])]);
% minc=max([1 min([ac-5 bc-5])]);
% maxr=min([numrows max([ar+5 br+5])]);
% maxc=min([numcols max([ac+5 bc+5])]);
%
% rowwind=rowwind(minr:maxr,minc:maxc);
% colwind=colwind(minr:maxr,minc:maxc);
%
% ar=ar-minr+1;
% br=br-minr+1;
% ac=ac-minc+1;
% bc=bc-minc+1;
%
% [numrows,numcols]=size(rowwind);
%
% aIndex=(ac-1)*numrows+ar;
% bIndex=(bc-1)*numrows+br;
[gridc,gridr]=meshgrid(1:numcols,1:numrows);
adist=(gridr-ar).^2+(gridc-ac).^2;
bdist=(gridr-br).^2+(gridc-bc).^2;
aenv= adist<4;
benv= bdist<4;
mvcx = (-maxThrottle:maxThrottle);
vc = mvcx(ones(2*maxThrottle+1,1),:);
vr=vc';
movecost=abs(vc)+abs(vr);
movecost(movecost>maxThrottle)=1000;
targetr=ar+rowwind(ar,ac);
targetc=ac+colwind(ar,ac);
done=zeros(numrows,numcols);
parent=done;
thr=ones(numrows,numcols)*1000;
thr(aIndex)=0;
done(aIndex)=1;
p=aIndex;
tvr=0;
tvc=0;
while ~any(done(benv))
i=max([1 targetr-maxThrottle]):min([numrows targetr+maxThrottle]);
j=max([1 targetc-maxThrottle]):min([numcols targetc+maxThrottle]);
ii=max([1 maxThrottle-targetr+2]):min([maxThrottle*2+1 maxThrottle*2+1-targetr-maxThrottle+numrows]);
jj=max([1 maxThrottle-targetc+2]):min([maxThrottle*2+1 maxThrottle*2+1-targetc-maxThrottle+numcols]);
update=~done(i,j).*(thr(i,j)>(movecost(ii,jj)+thr(p))).*(abs(vr(ii,jj)+tvr+rowwind(i,j))+abs(vc(ii,jj)+tvc+colwind(i,j))<maxThrottle+bdist(i,j)/2-3);
thr(i,j)=update.*(movecost(ii,jj)+thr(p))+~update.*thr(i,j);
parent(i,j)=update*p+~update.*parent(i,j);
[~,p]=min(thr(:)+1000*done(:));
besti = rem(p-1, numrows) + 1;
bestj = (p - besti)/numrows + 1;
r = rem(parent(p)-1, numrows) + 1;
c = (parent(p) - r)/numrows + 1;
tvr=besti-r;
tvc=bestj-c;
targetr=besti+tvr+rowwind(besti,bestj);
targetc=bestj+tvc+colwind(besti,bestj);
done(p)=1;
end
% and back home
thr=thr+bdist;
[~,p]=min(thr(:));
done=zeros(numrows,numcols);
parentback=done;
targetr = rem(p-1, numrows) + 1;
targetc = (p - targetr)/numrows + 1;
r = rem(parent(p)-1, numrows) + 1;
c = (parent(p) - r)/numrows + 1;
targetr=2*targetr-r+rowwind(p);
targetc=2*targetc-c+colwind(p);
done(p)=1;
while ~any(done(aenv))
i=max([1 targetr-maxThrottle]):min([numrows targetr+maxThrottle]);
j=max([1 targetc-maxThrottle]):min([numcols targetc+maxThrottle]);
ii=max([1 maxThrottle-targetr+2]):min([maxThrottle*2+1 maxThrottle*2+1-targetr-maxThrottle+numrows]);
jj=max([1 maxThrottle-targetc+2]):min([maxThrottle*2+1 maxThrottle*2+1-targetc-maxThrottle+numcols]);
update=~done(i,j).*(thr(i,j)>(movecost(ii,jj)+thr(p)));
thr(i,j)=update.*(movecost(ii,jj)+thr(p))+~update.*thr(i,j);
parentback(i,j)=update*p+~update.*parentback(i,j);
[~,p]=min(thr(:)+1000*done(:));
besti = rem(p-1, numrows) + 1;
bestj = (p - besti)/numrows + 1;
if ~parentback(p)
r = rem(parent(p)-1, numrows) + 1;
c = (parent(p) - r)/numrows + 1;
else
r = rem(parentback(p)-1, numrows) + 1;
c = (parentback(p) - r)/numrows + 1;
end
targetr=2*besti-r+rowwind(besti,bestj);
targetc=2*bestj-c+colwind(besti,bestj);
done(p)=1;
end
[m,p]=min(thr(:)+adist(:));
tra=[];
turned=0;
while p
tra(end+1)=p;
if turned||~parentback(p)
turned=1;
p=parent(p);
else
p=parentback(p);
end
end
tra=tra(end:-1:1);
trar = rem(tra-1, numrows) + 1;
trac = (tra - trar)/numrows + 1;
accelr=diff([0 diff(trar)]);
accelc=diff([0 diff(trac)]);
thrustRow = accelr-rowwind(tra(1:end-1));
thrustCol = accelc-colwind(tra(1:end-1));
end
|
