function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
nruns = 15;
Pa_array = [2.0 1.0 2.5 1.5 0.5 2.0 1.0 2.0 1.0 2.0 1.0 0.5 2.0 1.0 0.5];
Pb_array = [3 3 3 3 3 1.5 1.5 6 6 3 3 3 5 5 5 ];
V_adj = [0 0 0 0 0 0 0 0 0 -2 -2 -2 3 3 3 ];
Score = 99999*(ones(nruns,1));
V1 = [6 8];
V2 = [4 8];
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);
thrustRow = zeros(800,1);
thrustCol = zeros(800,1);
temp = -10:10;
penalty1 = [temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp];
penalty2 = penalty1';
chart1 = chart(:,:,1);
chart2 = chart(:,:,2);
CHART(1,:,:) = chart1;
CHART(2,:,:) = chart2;
[xlimit, ylimit] = size(chart(:,:,1));
pos(1)=aRow; pos(2)=aCol; pos_old = pos; START = pos;
goal(1) = bRow; goal(2) = bCol;
%Move the goal if it's in a bad place_____________
xrange = max(goal(1)-5,1):min(goal(1)+5,xlimit);
yrange = max(goal(2)-5,1):min(goal(2)+5,ylimit);
Xrange = xrange-goal(1)+11;
Yrange = yrange-goal(2)+11;
newX = chart(xrange,yrange,1) + goal(1);
newY = chart(xrange,yrange,2) + goal(2);
OUCH = (newX<1).*(1-newX) + (newX>xlimit).*(newX-xlimit) + (newY<1).*(1-newY) + (newY>ylimit).*(newY-ylimit) +...
(penalty1(Xrange,Yrange).^2+penalty2(Xrange,Yrange).^2);
[temp, ix] = min(OUCH);
[temp, iy] = min(temp);
ix = ix(iy);
goal(1) = xrange(ix);
goal(2) = yrange(iy);
% end move goal_________________________________
GOAL = goal;
goal2 = pos;
for RUN = 1:nruns,
vel = zeros(1,2);
turns = 0; Complete1 = 0; force = 0;
pos = START; goal = GOAL; GoalVect = goal - pos;
ErrorFlag=0; stuck = 0;
Pa = Pa_array(RUN);
Pb = Pb_array(RUN);
Vadj = V_adj(RUN);
for DIR = 1:2,
while ~Complete1
wind = CHART(:,pos(1),pos(2))';
newvel1 = vel + wind;
newpos1 = pos + newvel1;
if sum(abs(GoalVect)) > 12
GoalVel = min(V1(DIR)+Vadj,floor(maxThrottle*.75)) * GoalVect / sqrt(sum(GoalVect.^2));
GoalVel(1) = GoalVel(1) + max(0,10-newpos1(1))/Pb;
GoalVel(1) = GoalVel(1) - max(0,9+newpos1(1)-xlimit)/Pb;
GoalVel(2) = GoalVel(2) + max(0,10-newpos1(2))/Pb;
GoalVel(2) = GoalVel(2) - max(0,9+newpos1(2)-xlimit)/Pb;
else
GoalVel = min(V2(DIR)+Vadj,floor(maxThrottle*.4)) * GoalVect / sqrt(sum(GoalVect.^2));
force = 1;
end
%check if approaching the goal
if sum(abs(goal-newpos1)) < 6 && ( sum(abs(newvel1)) < 5 || DIR==2 ) && ~(sum(newpos1 < [1 1]) || sum(newpos1 > [xlimit ylimit]))
vel_old = vel; pos_old2 = pos_old; pos_old=pos;
vel = newvel1; pos=newpos1;
turns = turns+1;
xThr=0; yThr=0;
GoalVect = goal - pos;
break
end
minT = zeros(1,2);
if newpos1(1) < 1,
minT(1) = 1-newpos1(1);
elseif newpos1(1) > xlimit;
minT(1) = xlimit - newpos1(1);
end
if newpos1(2) < 1,
minT(2) = 1-newpos1(2);
elseif newpos1(2) > ylimit;
minT(2) = ylimit - newpos1(2);
end
if sum(abs(minT)) > maxThrottle
%disp('Into the abyss...')
ErrorFlag=1; break
end
newvel2 = vel + minT + wind;
Verror = newvel2 - GoalVel;
Vadd = -1*fix(Verror*.5);
newvel2 = vel + minT + Vadd + wind;
newpos2 = pos + newvel2;
xrange = max(newpos2(1)-6,1):min(newpos2(1)+6,xlimit);
yrange = max(newpos2(2)-6,1):min(newpos2(2)+6,ylimit);
Xrange = xrange-newpos2(1)+11;
Yrange = yrange-newpos2(2)+11;
p2 = penalty2(Xrange,Yrange);
p1 = penalty1(Xrange,Yrange);
if force,
E1 = abs(0.5*chart(xrange,yrange,1)+p2+Verror(1)+Vadd(1));
E2 = abs(0.5*chart(xrange,yrange,2)+p1+Verror(2)+Vadd(2));
temp = (abs(p2+Vadd(1)+minT(1))+abs(p1+Vadd(2)+minT(2)));
windhelp = E1 + E2 + 0.2*temp + 999*(temp>maxThrottle);
else
E1 = abs(chart(xrange,yrange,1)+p2+Verror(1)+Vadd(1));
E2 = abs(chart(xrange,yrange,2)+p1+Verror(2)+Vadd(2));
temp = (abs(p2+Vadd(1)+minT(1))+abs(p1+Vadd(2)+minT(2)));
windhelp = E1 + E2 + (~stuck)*Pa*temp + 999*(temp>maxThrottle);
end
[temp, ix] = min(windhelp);
[temp, iy] = min(temp);
ix = ix(iy);
xThr = minT(1) + Vadd(1) + p2(ix,iy);
yThr = minT(2) + Vadd(2) + p1(ix,iy);
if sum(abs(xThr)+abs(yThr)) > maxThrottle,
temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
xThr = xThr - sign(xThr) * floor(temp/2);
yThr = yThr - sign(yThr) * ceil(temp/2);
end
vel_old = vel; pos_old2 = pos_old; pos_old=pos;
vel = vel + [xThr yThr] + wind;
pos_temp = pos+vel;
% Break any loops we might get caught in
if sum(abs(vel)) == 0 || sum(pos_temp == pos_old2)==2 %|| sum(pos_temp == pos_old)==2 %%&& sum(abs(vel_old))==0
stuck = stuck+1;
if (stuck >2)
THR = maxThrottle * GoalVect / sum(abs(GoalVect));
xThr = fix(THR(1));
yThr = fix(THR(2));
end
vel = vel_old + [xThr yThr] + wind;
else
stuck = 0;
end
pos=pos + vel;
turns = turns+1;
thrustRow(turns) = xThr;
thrustCol(turns) = yThr;
if sum(pos < [1 1]) || sum(pos > [xlimit ylimit]) || turns>200
%disp('OOOOOPS!!')
ErrorFlag=1; break
end
%disp([pos vel xThr yThr force])
GoalVect = goal - pos;
if sum(abs(GoalVect)) < 5,
Complete1 = 1;
end
end
if DIR==1 && ~ErrorFlag,
xThr = xThr+GoalVect(1);
yThr = yThr+GoalVect(2);
if sum(abs(xThr)+abs(yThr)) > maxThrottle,
temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
xThr = xThr - sign(xThr) * floor(temp/2);
yThr = yThr - sign(yThr) * ceil(temp/2);
%This is trouble!
end
vel_temp = vel_old + [xThr yThr] + wind;
vel_next = vel_temp + CHART(:,goal(1),goal(2))';
if sum(abs(vel_temp))>7 || sum(abs(vel_next))>maxThrottle
wind = CHART(:,pos(1),pos(2))';
xThr = GoalVect(1) - wind(1) - vel(1);
yThr = GoalVect(2) - wind(2) - vel(2);
%disp([pos vel xThr yThr turns Complete1])
if sum(pos < [1 1]) || sum(pos > [xlimit ylimit])
%disp('OOOOOPS!!')
ErrorFlag=1;
end
if sum(abs(xThr)+abs(yThr)) > maxThrottle,
temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
xThr = xThr - sign(xThr) * floor(temp/2);
yThr = yThr - sign(yThr) * ceil(temp/2);
%This is trouble!
end
turns = turns +1;
vel = vel + [xThr yThr] + wind;
pos_old2 = pos_old; pos_old = pos;
pos = pos + vel;
thrustRow(turns) = xThr;
thrustCol(turns) = yThr;
else
vel = vel_temp;
pos = pos_old + vel;
thrustRow(turns) = xThr;
thrustCol(turns) = yThr;
end
if sum(pos < [1 1]) || sum(pos > [xlimit ylimit])
%disp('OOOOOPS!!')
ErrorFlag=1;
end
pos_old = [-1 -1]; pos_old2=[-1 -1];
%disp([pos vel xThr yThr turns Complete1 goal])
end
if ErrorFlag
break
end
% Head Back! %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%disp(['Head Back!'])
goal = goal2; GoalVect = goal -pos;
Complete1 = 0; force = 0;
end
if ~ErrorFlag
xThr = xThr+GoalVect(1);
yThr = yThr+GoalVect(2);
if sum(abs(xThr)+abs(yThr)) > maxThrottle,
temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
xThr = xThr - sign(xThr) * floor(temp/2);
yThr = yThr - sign(yThr) * ceil(temp/2);
end
thrustRow(turns) = xThr;
thrustCol(turns) = yThr;
vel = vel_old + [xThr yThr] + wind;
pos = pos_old + vel;
%disp([pos vel xThr yThr goal])
thrustRow = thrustRow(1:turns);
thrustCol = thrustCol(1:turns);
end
[tRow, tCol] = validatesolution(thrustRow, thrustCol, maxThrottle);
if sum(tRow - thrustRow) || sum(tCol - thrustCol),
Score(RUN) = 99999;
else
[dA,dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, 0);
Score(RUN) = scoresolution(dA,dB,thrustRow,thrustCol);
TROW{RUN} = thrustRow;
TCOL{RUN} = thrustCol;
end
end %end of RUNs loop
[junk, bestrun] = min(Score);
if junk < 99000
thrustRow = TROW{bestrun};
thrustCol = TCOL{bestrun};
end
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
if flagVisualize
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
|
