function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
[thrustRowb, thrustColb] = nickel(chart, aIndex, bIndex, maxThrottle);
scoreb = runsolution(thrustRowb, thrustColb, chart, aIndex, bIndex);
if scoreb>40
[thrustRowa, thrustCola] = nick(chart, aIndex, bIndex, maxThrottle);
else
thrustCol = thrustColb;
thrustRow = thrustRowb;
return;
end
scorea = runsolution(thrustRowa, thrustCola, chart, aIndex, bIndex);
if scorea < scoreb
thrustCol = thrustCola;
thrustRow = thrustRowa;
else
thrustCol = thrustColb;
thrustRow = thrustRowb;
end
end
function [n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(chart, aIndex, bIndex, maxThrottle)
iW = chart(:,:,1);
jW = chart(:,:,2);
nrow = size(iW,1);
ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
bi = rem(bIndex-1, nrow) + 1;
bj = (bIndex - bi)/nrow+ 1;
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 = 400;
mt = ceil(maxThrottle/2);
mt2 = ceil(3*maxThrottle/4);
sqd = inf;
[nrow,ncol] = size(iW);
ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
while (sqd>100)
sqd = inf;
mcum = 0;
stopn = 0;
srec = [pi pj vi vj];
while ((n<1000)&&((pi>=1)&&(pj>=1)&&(pi<=nrow)&&(pj<=ncol))&&((pi~=bi)|(pj~=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)
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;
if (isinf(ws(wk)))
break;
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 (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 [thrustRow, thrustCol] = nick(chart, aIndex, bIndex, maxThrottle)
iW = chart(:,:,1);
jW = chart(:,:,2);
mt2 = ceil(3*maxThrottle/4);
[nrow,ncol] = size(iW);
ai = rem(aIndex-1, nrow) + 1;
aj = (aIndex - ai)/nrow + 1;
bi = rem(bIndex-1, nrow) + 1;
bj = (bIndex - bi)/nrow+ 1;
vws = inf(1,1000);
% set up initial conditions
mtt = 400;
% try to approach b
[n,thrustRow,thrustCol,pi,pj,vi,vj,dB,bs,bn] = firstwhile(chart, aIndex, bIndex, maxThrottle);
mt = ceil(2*maxThrottle/3);
% 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 ((n<1000)&&((pi>=1)&(pj>=1)&(pi<=nrow)&(pj<=ncol))&&((pi~=ai)|(pj~=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;
if (isinf(ws(wk)))
break;
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);
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 (bn>1)&&(sqd > dB(bn-1))
% prepare for repeat
thrustRow(bn:n) = 0;
thrustCol(bn:n) = 0;
bn = bn-1;
n = bn;
pi = ai;
pj = aj;
vi = 0;
vj = 0;
for k = 1:n
vi = vi+iW(pi,pj)+thrustRow(k);
vj = vj+jW(pi,pj)+thrustCol(k);
pi = pi+vi;
pj = pj+vj;
end;
sqd = inf;
end;
end;
% trim excess
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
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/3).^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 = cell(nrow,ncol);
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, chart, aIndex, bIndex)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);
[AR,AC] = ind2sub([nR,nC],aIndex);
[BR,BC] = ind2sub([nR,nC],bIndex);
% 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;
if iR>nR || iR<1 || iC>nC || iC<1
break % out of bounds
end
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 [ai,aj] = ind2sub(sz,aIndex)
ai = rem(aIndex-1, sz(1)) + 1;
aj = (aIndex - ai)/sz(1)+ 1;
end
function [thrustRow, thrustCol] = nickel(chart, aIndex, bIndex, maxThrottle)
ywinds = chart(:,:,1);
xwinds = chart(:,:,2);
[nr,nc] = size(xwinds);
ne = nr*nc;
ay = rem(aIndex-1, nr) + 1;
ax = (aIndex - ay)/nr+ 1;
by = rem(bIndex-1, nr) + 1;
bx = (bIndex - by)/nr+ 1;
x = (1:nc);
y = (1:nr)';
xval = x(ones(nr,1),:);
yval = y(:,ones(nc,1));
position = zeros(nr,nc);
position(:) = 1:ne;
distanceToA = (xval-ax).^2+(yval-ay).^2;
distanceToB = (xval-bx).^2+(yval-by).^2;
targetScore = 0.0222;
maxIter = 2000;
ydirection = 2*(by > ay) - 1;
xdirection = 2*(bx > ax) - 1;
%%
fuel = Inf(nr,nc);
fuel(aIndex) = 0;
fueltoreverse = fuel;
xvel = zeros(nr,nc);
yvel = zeros(nr,nc);
checked = false(nr,nc);
previousstop = zeros(nr,nc);
iteration=0;
minval=0;
costtoB = Inf;
while(minval < costtoB && ~all(checked(:)) && costtoB > targetScore*iteration && iteration<maxIter)
iteration=iteration+1;
[minval,minidx]=min(fuel(~checked));
reference = position(~checked);
minidx = reference(minidx);
i_y = rem(minidx-1, size(fuel,1)) + 1;
i_x = (minidx - i_y)/size(fuel,1) + 1;
i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
i_vx = xvel(minidx) + xwinds(minidx);
i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
i_newvy = yval-i_y;
i_newvx = xval-i_x;
i_fuel = i_thrust + minval; %fuel used to get to position.
i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
fuel(i_improvement) = i_fuel(i_improvement);
xvel(i_improvement) = i_newvx(i_improvement);
yvel(i_improvement) = i_newvy(i_improvement);
fueltoreverse(i_improvement) = fuel(i_improvement) + ...
abs(yvel(i_improvement)).*(yvel(i_improvement)*ydirection>0) + ...
abs(xvel(i_improvement)).*(xvel(i_improvement)*xdirection>0) ;
previousstop(i_improvement) = minidx;
costtoB = min(min(fueltoreverse + distanceToB));
checked(i_improvement) = false;
checked(minidx) = true;
end
%%
fuel = fuel + distanceToB;
costtoA = min(min(fuel + distanceToA));
checked = false(nr,nc);
returnpreviousstop = zeros(nr,nc);
iteration=0;
minval=0;
while(minval < costtoA && ~all(checked(:)) && (costtoA-costtoB) > targetScore*iteration && iteration<maxIter)
iteration=iteration+1;
[minval,minidx]=min(fuel(~checked));
reference = position(~checked);
minidx = reference(minidx);
i_y = rem(minidx-1, size(fuel,1)) + 1;
i_x = (minidx - i_y)/size(fuel,1) + 1;
i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
i_vx = xvel(minidx) + xwinds(minidx);
i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
i_newvy = yval-i_y;
i_newvx = xval-i_x;
i_fuel = i_thrust + minval; %fuel used to get to position.
i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
fuel(i_improvement) = i_fuel(i_improvement);
xvel(i_improvement) = i_newvx(i_improvement);
yvel(i_improvement) = i_newvy(i_improvement);
returnpreviousstop(i_improvement) = minidx;
costtoA = min(min(fuel + distanceToA));
checked(i_improvement) = false;
checked(minidx) = true;
end
%%
costtoA = fuel + distanceToA;
[minval,minidx] = min(costtoA(:));
indices = minidx(1);
nextindex = returnpreviousstop(indices(end));
indices = lowercyc4(nextindex,indices,returnpreviousstop,previousstop);
indices = indices(end:-1:1);
ypos = rem(indices-1, nr) + 1;
xpos = (indices - ypos)/nr + 1;
xw = xwinds(indices); %n+1
yw = ywinds(indices); %n+1
xspeed = diff(xpos); %n
yspeed = diff(ypos); %n
xdrive = diff([0 xspeed]); %n
ydrive = diff([0 yspeed]); %n
[xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw);
thrustRow = ymotor;
thrustCol = xmotor;
end
function [indices] = lowercyc4(nextindex,indices,returnpreviousstop,previousstop)
while(nextindex)
indices(end+1) = nextindex;
nextindex = returnpreviousstop(indices(end));
end
nextindex = previousstop(indices(end));
while(nextindex)
indices(end+1) = nextindex;
nextindex = previousstop(indices(end));
end
end
function [xmotor,ymotor] = lowercyc(xdrive,ydrive,xw,yw)
if(~isempty(xdrive))
xmotor = xdrive - xw(1:end-1);
ymotor = ydrive - yw(1:end-1);
else
xmotor = [];
ymotor = [];
end
end
|
