function [tBestRow, tBestCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Darren Rowland
tBestRow = [];
tBestCol = [];
rowWind = int8(chart(:,:,1));
colWind = int8(chart(:,:,2));
[nR,nC] = size(rowWind);
AR = int8(rem(aIndex-1,nR)+1);
AC = int8(ceil(aIndex/nR));
BR = int8(rem(bIndex-1,nR)+1);
BC = int8(ceil(bIndex/nR));
NullScore = double(AR-BR)^2 + double(AC-BC)^2;
bestScore = NullScore;
iter = 0;
maxIter = 4;
%sFactor = 1;
goodHead = true;
while bestScore>NullScore/10 && iter < maxIter
sFactor = 0.9 + 0.5*rand;
[thrustRow,thrustCol,headtoB] = getTrip(rowWind,colWind,nR,nC,...
AR,AC,BR,BC,maxThrottle,sFactor,false);
score = runsolution(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
thrustRow, thrustCol);
iter = iter + 1;
if score<bestScore
bestScore = score;
tBestRow = thrustRow;
tBestCol = thrustCol;
goodHead = headtoB;
end
end
if bestScore>NullScore/10
% Determine why this board has failed
[dA, dB, fuel,SR,SC,vFinal] = getAB(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
tBestRow, tBestCol);
if fuel>dA && fuel>dB
% not much we can do at this time
elseif dA>dB
[extraRow,extraCol] = getHome(rowWind, colWind,nR,nC,AR,AC,...
SR,SC,vFinal,maxThrottle);
tBestRow = [tBestRow(:); extraRow(:)];
tBestCol = [tBestCol(:); extraCol(:)];
elseif dB>dA
if goodHead
iter = 0;
maxIter = 4;
sFactor = 1.15;
while iter<maxIter
iter = iter + 1;
sFactor = sFactor^2;
[thrustRow,thrustCol] = getTrip(rowWind,colWind,nR,nC,...
AR,AC,BR,BC,maxThrottle,sFactor,false);
score = runsolution(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
thrustRow, thrustCol);
if score<bestScore
bestScore = score;
tBestRow = thrustRow;
tBestCol = thrustCol;
end
end
else
% treat as going home
[extraRow,extraCol] = getHome(rowWind, colWind,nR,nC,AR,AC,...
SR,SC,vFinal,maxThrottle);
tBestRow = [tBestRow(:); extraRow(:)];
tBestCol = [tBestCol(:); extraCol(:)];
end
end
end
% if bestScore>NullScore/5
% iter = 0;
% maxIter = 4;
% sFactor = 1.15;
% while iter<maxIter
% iter = iter + 1;
% sFactor = sFactor + 0.1;
% [thrustRow,thrustCol] = getTrip(rowWind,colWind,nR,nC,...
% AR,AC,BR,BC,maxThrottle,sFactor,true);
% score = runsolution(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
% thrustRow, thrustCol);
% if score<bestScore
% bestScore = score;
% tBestRow = thrustRow;
% tBestCol = thrustCol;
% end
% end
%
% end
end
function [thrustRow,thrustCol,headtoB] = getTrip(rowWind,colWind,nR,nC,...
AR,AC,BR,BC,maxThrottle,sFactor,allowNegB)
thrustRow = zeros(1000,1,'int8');
thrustCol = zeros(1000,1,'int8');
turn = 0;
n = 0;
i = -1;
headtoB = true;
vStart = zeros(1,2,'int8');
sR = AR;
sC = AC;
targetRow = BR;
targetCol = BC;
while turn < 1000 && i < 0
[tRow, tCol, dB, nB] = checkAllThrusts(rowWind,colWind,nR,nC,...
targetRow,targetCol,sR,sC,vStart,maxThrottle,sFactor,allowNegB);
if nB == 0
if headtoB
headtoB = false;
targetRow = AR;
targetCol = AC;
else
break;
end
else
n = turn+1;
thrustRow(n) = tRow;
thrustCol(n) = tCol;
turn = turn + nB;
[sIndex, vStart, i] = ...
applyThrust(rowWind,colWind,nR,nC,targetRow,targetCol,sR,sC,...
vStart,thrustRow(n:turn),thrustCol(n:turn),false);
sR = int8(rem(sIndex-1,nR)+1);
sC = int8(ceil(sIndex/nR));
end
end
if i >= 0
turn = n-1;
end
thrustRow(turn+1:end) = [];
thrustCol(turn+1:end) = [];
end
function [tRow, tCol, dB, nB] = checkAllThrusts(rowWind,colWind,nR,nC,...
BR,BC,SR,SC,vStart,maxThrottle,sFactor,allowNegB)
% Determine the row and column thrusts which take us closest to B from
% our position at sIndex with velocity vStart.
m = maxThrottle;
dB = (double(SR-BR))^2 + (double(SC-BC))^2;
objmin = inf;
tRow = [];
tCol = [];
for tR = -m:m
cMax = m-abs(tR);
for tC = -cMax:cMax
if rand < max(min(750/(nR*nC),0.5),0.1)
% check only a percentage of cases
[fIndex, vFinish, i, thisdB, thisnB] = ...
applyThrust(rowWind,colWind,nR,nC,BR,BC,SR,SC,vStart,tR,tC,true);
if ~allowNegB
% Look for moves which cheaply reduce the distance to B,
% i.e. low motor usage, keeping the velocity under maxThrust
if thisdB < dB
[fIndex, vFinish, i, thisdB] = ...
applyThrust(rowWind,colWind,nR,nC,BR,BC,SR,SC,vStart,...
[tR zeros(1,thisnB-1,'int8')],[tC zeros(1,thisnB-1,'int8')],false);
obj = (abs(tR) + abs(tC)) + thisdB + sum(abs(vFinish));
if obj < objmin && sum(abs(vFinish+...
[rowWind(fIndex) colWind(fIndex)])) <= sFactor*m
tRow = tR;
tCol = tC;
objmin = obj;
end
end
else
% Favour moves which are cheap, and arrive at squares with
% benign winds
[fIndex, vFinish] = ...
applyThrust(rowWind,colWind,nR,nC,BR,BC,SR,SC,vStart,tR,tC,false);
if fIndex
obj = 0.2*(abs(tR)+abs(tC));
obj = obj-(vFinish(1)+rowWind(fIndex))*sign(BR-SR);
obj = obj-(vFinish(2)+colWind(fIndex))*sign(BC-SC);
if obj < objmin
tRow = tR;
tCol = tC;
objmin = obj;
end
end
end
end
end
end
[fIndex, vFinish, i, dB, nB] = ...
applyThrust(rowWind,colWind,nR,nC,BR,BC,SR,SC,vStart,tRow,tCol,false);
end
function [fIndex, vFinish, i, dB, nB] = applyThrust(rowWind,colWind,nR,nC,...
BR,BC,SR,SC,vStart,thrustRow,thrustCol,freeRoam)
% Determine the final position and velocity attained after applying
% [thrustRow,thrustCol] from sIndex with velocity vStart.
% i == -1 signals success, else trajectory runs OOB at turn i.
fR = SR; fC = SC;
fvR = vStart(1); fvC = vStart(2);
dB = (double(fR-BR))^2 + (double(fC-BC))^2;
nB = 0;
fIndex = 0;
vFinish = zeros(1,2,'int8');
%for i = 1:numel(thrustRow)
i = 0;
while i<numel(thrustRow) || (freeRoam && i<11)
i = i + 1;
if i <= numel(thrustRow)
tR = thrustRow(i);
tC = thrustCol(i);
else
tR = 0; tC = 0;
end
ivR = fvR + tR + rowWind(fR,fC);
ivC = fvC + tC + colWind(fR,fC);
iR = fR + ivR;
iC = fC + ivC;
if iR>nR || iR<1 || iC>nC || iC<1
return % out of bounds
end
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
if (double(fR-BR)^2 + double(fC-BC)^2) < dB
nB = i;
dB = double(fR-BR)^2 + double(fC-BC)^2;
end
end
% Successful thrusting
i = -1;
%fIndex = sub2ind([nR,nC],fR,fC);
fIndex = double(fR) + double(fC-1)*nR;
vFinish = [fvR, fvC];
end
function [homeRow,homeCol] = getHome(rowWind, colWind,nR,nC,AR,AC,...
SR,SC,vStart,maxThrottle)
% With some distance to get home we will use up additional fuel to make it
% [SR,SC] are [row,column] indices of current location
% [AR,AC] are home coordinates, dA is distance between S and A
m = maxThrottle;
dA = double(SR-AR)^2 + double(SC-AC)^2;
objmin = dA;
tRow = [];
tCol = [];
fuelUsed = 0;
homeRow = [];
homeCol = [];
k = 0;
while k<10
k = k+1;
for tR = -m:m
cMax = m-abs(tR);
for tC = -cMax:cMax
if rand < max(min(1000/(nR*nC),0.6),0.2)
% check only a percentage of cases
[fIndex, vFinish, i, obj] = ...
applyThrust(rowWind,colWind,nR,nC,AR,AC,SR,SC,vStart,tR,tC,false);
% Look for moves which cheaply reduce the distance to home,
% i.e. prefer low motor usage but don't really care about the
% resulting velocity
obj = obj + (abs(tR) + abs(tC)) + fuelUsed;
if obj < objmin
tRow = tR;
tCol = tC;
objmin = obj;
end
end
end
end
if ~isempty(tRow)
homeRow(k) = tRow;
homeCol(k) = tCol;
fuelUsed = fuelUsed + abs(tRow) + abs(tCol);
[fIndex, vStart] = applyThrust(rowWind,colWind,nR,nC,...
AR,AC,SR,SC,vStart,tRow,tCol,false);
if fIndex == 0
break;
end
SR = int8(rem(fIndex-1,nR)+1);
SC = int8(ceil(fIndex/nR));
end
end
end
function score = runsolution(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
thrustRow, thrustCol)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
[dA, dB, fuel] = getAB(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
thrustRow, thrustCol);
score = dB + dA + fuel;
end
function [dA, dB, fuel, fR,fC,vFinal] = getAB(rowWind, colWind,nR,nC,AR,AC,BR,BC,...
thrustRow, thrustCol)
fR = AR; fC = AC;
fvR = 0; fvC = 0;
dB = (double(fR-BR))^2 + (double(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
return % out of bounds
end
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
if (double(fR-BR)^2 + double(fC-BC)^2) < dB
dB = double(fR-BR)^2 + double(fC-BC)^2;
end
end
dA = double(fR-AR)^2 + double(fC-AC)^2;
fuel = sum(abs(double(thrustRow))) + sum(abs(double(thrustCol)));
vFinal = [fvR, fvC];
end
|
