function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% current opposition
% Copyright 2010 The MathWorks, Inc.
thrustRow = [];
thrustCol = [];
flagVisualize = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% try several routes, take the best
%at a,b pts
rtei = 0;
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 4);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 5);
%buffer region
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 6);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 7);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 8);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 9);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 10);
%max thr
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 101);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 102);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 103);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 104);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 105);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 106);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 107);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 108);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 109);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 110);
% %drift
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 201);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 202);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 203);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 204);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 205);
%miss size 2
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+1);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+2);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+3);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+4);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+5);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+6);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+7);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+8);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+9);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+10);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+101);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+102);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+103);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+104);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+105);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+106);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+107);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+108);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+109);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+110);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+201);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+202);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+203);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+204);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 1000+205);
%miss size 3
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+1);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+2);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+3);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+4);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+5);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+6);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+7);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+8);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+9);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+10);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+101);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+102);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+103);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+104);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+105);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+106);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+107);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+108);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+109);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+110);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+201);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+202);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+203);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+204);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 2000+205);
%miss size 4
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+1);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+2);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+3);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+4);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+5);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+6);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+7);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+8);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+9);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+10);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+101);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+102);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+103);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+104);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+105);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+106);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+107);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+108);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+109);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+110);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+201);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+202);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+203);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+204);
rtei = rtei + 1; [sscore(rtei), thRow{rtei}, thCol{rtei}] = routesolve(chart, aIndex, bIndex, maxThrottle, 3000+205);
[tmp, ii] = min(sscore);
fi = ii(1);
thrustRow = thRow{fi};
thrustCol = thCol{fi};
end
function [rscore, thrustRow, thrustCol] = routesolve(chart, aIndex, bIndex, maxThrottle, rflag)
% current opposition
rscore = Inf;
thrustRow = [];
thrustCol = [];
flagVisualize = 0;
pathr = [];
pathc = [];
rwind = chart(:,:,1);
cwind = chart(:,:,2);
[maxr, maxc, tmp] = size(chart);
%starting r,c
[ra, ca] = ind2sub(size(chart(:,:,1)),aIndex);
[rb, cb] = ind2sub(size(chart(:,:,1)),bIndex);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
maxerr = 7;
maxN = 999;
buff = floor(maxThrottle / 2);
if maxThrottle >= 9,
speedgoal = floor(maxThrottle / 3);
elseif maxThrottle >= 6,
speedgoal = 4;
elseif maxThrottle >= 4,
speedgoal = 2;
else
speedgoal = maxThrottle;
end
rs = 0;
cs = 0;
driftfl = 0;
%current pos
k = aIndex;
r = ra;
c = ca;
pathr = [r];
pathc = [c];
if (rflag > 1000) & (rflag < 2000),
rflag = rflag - 1000;
maxerr = 5;
end
if (rflag > 2000) & (rflag < 3000),
rflag = rflag - 2000;
maxerr = 4;
end
if (rflag > 3000) & (rflag < 4000),
rflag = rflag - 3000;
maxerr = 2;
end
if (rflag > 100) & (rflag < 200),
% max throt
rflag = rflag - 100;
speedgoal = maxThrottle;
end
if (rflag > 200) & (rflag < 300),
rflag = rflag - 200;
driftfl = 1;
end
switch rflag,
case 1,
% directed
wayr = [rb; ra];
wayc = [cb; ca];
case 2,
%keep row corner
wayr = [ra; rb; ra; ra];
wayc = [cb; cb; cb; ca];
case 3,
%keep col corner
wayr = [rb; rb; rb; ra];
wayc = [ca; cb; ca; ca];
case 4
%circulate
wayr = [ra; rb; rb; ra];
wayc = [cb; cb; ca; ca];
case 5
%countercirculate
wayr = [rb; rb; ra; ra];
wayc = [ca; cb; cb; ca];
case 6,
% directed
wayr = [rb-buff; ra];
wayc = [cb-buff; ca];
case 7,
%keep row corner
if ra > maxr/2,
rm = ra - buff;
else
rm = ra + buff;
end
if cb > maxc/2,
cm = cb - buff;
else
cm = cb + buff;
end
wayr = [rm ; rb; rm; ra];
wayc = [cm; cb; cm; ca];
case 8,
%keep col corner
if rb > maxr/2,
rm = rb - buff;
else
rm = rb + buff;
end
if ca > maxc/2,
cm = ca - buff;
else
cm = ca + buff;
end
wayr = [rm; rb; rm; ra];
wayc = [cm; cb; cm; ca];
case 9
%circulate
if ra > maxr/2,
rm1 = ra - buff;
else
rm1 = ra + buff;
end
if cb > maxc/2,
cm1 = cb - buff;
else
cm1 = cb + buff;
end
if rb > maxr/2,
rm2 = rb - buff;
else
rm2 = rb + buff;
end
if ca > maxc/2,
cm2 = ca - buff;
else
cm2 = ca + buff;
end
wayr = [rm1; rb; rm2; ra];
wayc = [cm1; cb; cm2; ca];
case 10
%countercirculate
if ra > maxr/2,
rm1 = ra - buff;
else
rm1 = ra + buff;
end
if cb > maxc/2,
cm1 = cb - buff;
else
cm1 = cb + buff;
end
if rb > maxr/2,
rm2 = rb - buff;
else
rm2 = rb + buff;
end
if ca > maxc/2,
cm2 = ca - buff;
else
cm2 = ca + buff;
end
wayr = [rm2; rb; rm1; ra];
wayc = [cm2; cb; cm1; ca];
end
%proceed through each waypoint
wi = 0;
wdone = 0;
n = 0;
while wdone == 0,
wi = wi + 1;
goalr = wayr(wi);
goalc = wayc(wi);
%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Head to next pt
done = 0;
while (done == 0 & n < maxN);
n = n + 1;
%delta to b point
totd = (abs(goalr - r) + abs(goalc - c));
dr = (goalr-r) ;
dc = (goalc-c) ;
speedgoalr = fix(speedgoal * dr / totd);
speedgoalc = fix(speedgoal * dc / totd);
%new speeds from winds
rs1 = rs + rwind(r,c);
cs1 = cs + cwind(r,c);
%correct to nominal or not
if driftfl == 1,
tmpr = r + rs1;
tmpc = c + cs1;
nomhead = atan2((goalr - r),(goalc - c + eps));
newhead = atan2((tmpr - r) , (tmpc - c + eps));
if (abs(nomhead - newhead) < 30 * pi/180) & (tmpr >= 1 & tmpc >= 1 & tmpr <= maxr & tmpc <= maxc),
thr0 = 0;
thc0 = 0;
else
thr0 = speedgoalr - rs1;
thc0 = speedgoalc - cs1;
end
else
thr0 = speedgoalr - rs1;
thc0 = speedgoalc - cs1;
end
if abs(thr0) + abs(thc0) > maxThrottle,
thr = fix(thr0 * maxThrottle / ( abs(thr0) + abs(thc0)));
thc = fix(thc0 * maxThrottle / ( abs(thr0) + abs(thc0)));
% %check if limited thrust puts us at risk
% tmpr = r + rs1 + thr;
% tmpc = c + rs1 + thc;
% if (tmpr < 1 | tmpc < 1 | tmpr > maxr | tmpc > maxc),
% %at risk
% if (tmpr < 1 | tmpr > maxr),
% thr = thr0;
% thc = sign(thc0) * min([abs(thc0) maxThrottle - abs(thr0)]);
% else
% thc = thc0;
% thr = sign(thr0) * min([abs(thr0) maxThrottle - abs(thc0)]);
% end
% end
else
thr = thr0;
thc = thc0;
end
thrustRow = [thrustRow; thr];
thrustCol = [thrustCol; thc];
%new speeds
rs = rs1 + thr;
cs = cs1 + thc;
%new point
r = r + rs;
c = c + cs;
pathr = [pathr r];
pathc = [pathc c];
if (r < 1 | c < 1 | r > maxr | c > maxc),
% call it here
thrustRow = thrustRow(1:end-1);
thrustCol = thrustCol(1:end-1);
done = 1;
wdone = 1;
end
if (abs(r-goalr) + abs(c-goalc)) <= maxerr,
done = 1;
end
if n > 999,
done = 1;
end
end
if (wi == length(wayr)),
wdone = 1;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% score the result
[dA,dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, flagVisualize);
rscore = scoresolution(dA,dB,thrustRow,thrustCol);
end
function [thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle)
% VALIDATESOLUTION Validates the solution vectors given by the solver
% Ensure column vectors and reals
thrustRow = real(thrustRow(:));
thrustCol = real(thrustCol(:));
% Check maximum number of moves
mnmoves = 1000;
if numel(thrustRow) > mnmoves
thrustRow = thrustRow(1:mnmoves);
end
if numel(thrustCol) > mnmoves
thrustCol = thrustCol(1:mnmoves);
end
% Ensure integers
if any(rem(thrustRow,1))
thrustRow = round(thrustRow);
end
if any(rem(thrustCol,1))
thrustCol = round(thrustCol);
end
% Ensure same length
ntR = numel(thrustRow);
ntC = numel(thrustCol);
if ntR~=ntC
n = min(ntR,ntC);
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
end
% Check maximum throttle
h = (abs(thrustRow) + abs(thrustCol)) > maxThrottle;
if any(h)
% Motor is blown for every turn the throttle exceeds the motor capacity!
thrustRow(h) = 0;
thrustCol(h) = 0;
end
end
function [dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, flagVisualize)
% 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
end
function score = scoresolution(dA,dB,thrustRow,thrustCol)
% SCORESOLUTION Calculates the score for the solution.
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end
|
