function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxthrottle)
% PRECOMP
nrows = size(chart, 1);
ncols = size(chart, 2);
wv = chart(:,:,1);
wh = chart(:,:,2);
% create motormask
throttles = -maxthrottle:maxthrottle;
absthrottles = abs(throttles);
motormask = bsxfun(@(x,y) x+y, absthrottles', absthrottles);
motormask(motormask>maxthrottle) = inf;
% motormask = motormask .^2;
% find pos
[av, ah] = ind2sub(size(chart(:,:,1)), aIndex);
[bv, bh] = ind2sub(size(chart(:,:,1)), bIndex);
distanceab = sqrt((av-bv).^2+(ah-bh).^2);
ixv = (1:nrows)';
ixh = 1:ncols;
ixsh = ixh(ones(nrows, 1), :);
ixsv = ixv(:, ones(1, ncols));
% some distances
dha = (ixh - ah);
dha = dha(ones(nrows, 1), :);
dhaw = dha + wh;
dva = (ixv - av);
dva = dva(:, ones(1, ncols));
dvaw = dva + wv;
dhb = (ixh - bh);
dhb = dhb(ones(nrows, 1), :);
dhbw = dhb + wh;
dvb = (ixv - bv);
dvb = dvb(:, ones(1, ncols));
dvbw = dvb + wv;
scoresa = dha .^2 + dva .^2;
scoresb = dhb .^2 + dvb .^2;
scoresaw = dhaw.^2 + dvaw.^2;
scoresbw = dhbw.^2 + dvbw.^2;
% vchangemasks
vhchangemask = throttles(ones(2*maxthrottle+1, 1), :);
vvchangemask = vhchangemask';
% spacetoborder
spaceleft = 0:(ncols-1);
spaceleft = spaceleft(ones(nrows, 1), :);
spaceright = fliplr(spaceleft);
spacetop = (0:(nrows-1))';
spacetop = spacetop(:, ones(1, ncols));
spacebottom = flipud(spacetop);
% PRECOMP DONE
nummethods = 5;
pars = 0:(nummethods^2-1);
pars = [pars,pars+100];
npars = numel(pars);
results = cell(npars, 4);
for ii = 1:npars;
p = pars(ii);
[thrustRow, thrustCol, score] = coresolver(chart, aIndex, bIndex, maxthrottle, p);
results(ii, :) = {p, thrustRow, thrustCol, score};
end
% results
[~,pick] = min([results{:,4}]);
par = results{pick,1};
% pick
% pick = 5
thrustRow = results{pick, 2};
thrustCol = results{pick, 3};
function [thrustRow, thrustCol, result] = coresolver(chart, aIndex, bIndex, maxthrottle, par)
methodcombined = mod(par, 100);
methoddepart = 1+mod(methodcombined,nummethods);
methodreturn = floor(1+(methodcombined)/nummethods);
% if methoddepart == 4 || methodreturn == 4;
% thrustRow = 0;
% thrustCol = 0;
% result = inf;
% return;
% end
% core
vv = 0;
vh = 0;
ph = ah;
pv = av;
done = false;
onreturn = false;
scores = scoresb;
scoresw = scoresbw;
state = nan(1000, 2);
step = 1;
newtargetmap = false(nrows, ncols);
beenthere = newtargetmap;
justturned = false;
resultmotor = 0;
bestdb = inf;
method = methoddepart;
goaljump = par>100;
while ~done
nextstep = false;
beenthere(pv, ph) = true;
if scoresb(pv, ph) < bestdb; bestdb = scoresb(pv, ph); end;
if ~justturned
% add winds to v
vh = vh + wh(pv,ph);
vv = vv + wv(pv,ph);
end
nph = ph + vh;
npv = pv + vv;
ixv = npv + vvchangemask;
ixh = nph + vhchangemask;
h1 = nph-maxthrottle; if h1 < 1 ; h1 = 1; end;
h2 = nph+maxthrottle; if h2 > ncols; h2 = ncols; end;
dh = h2-h1+1;
v1 = npv-maxthrottle; if v1 < 1 ; v1 = 1; end;
v2 = npv+maxthrottle; if v2 > nrows; v2 = nrows; end;
dv = v2-v1+1;
targetmap = newtargetmap;
targetmap(v1:v2,h1:h2) = true;
maskmap = ixv > 0 & ixv <= nrows & ixh > 0 & ixh <= ncols;
targetmotor = motormask(maskmap);
targetscores = scores(targetmap);
targetscoresw = scoresw(targetmap);
targetmotorh = vhchangemask(maskmap);
targetmotorv = vvchangemask(maskmap);
speedsh = vh + targetmotorh;
speedsv = vv + targetmotorv;
speedsh2 = speedsh + wh(targetmap);
speedsv2 = speedsv + wv(targetmap);
targetixsv = ixsv(targetmap);
targetixsh = ixsh(targetmap);
targetspaceleft = spaceleft(targetmap);
targetspaceright = spaceright(targetmap);
targetspacetop = spacetop(targetmap);
targetspacebottom = spacebottom(targetmap);
speeds2 = abs(speedsh2)+abs(speedsv2);
% skip = speedsh2 > targetspaceright | ...
% speedsh2 < -targetspaceleft | ...
% speedsv2 > targetspacebottom | ...
% speedsv2 < -targetspacetop | ...
% beenthere(targetmap);
skip = (max( speedsh2-targetspaceright , 0) + ...
max(-speedsh2-targetspaceleft , 0) + ...
max( speedsv2-targetspacebottom, 0) + ...
max(-speedsv2-targetspacetop , 0)) > maxthrottle;
skip = skip | beenthere(targetmap);
% speedsh2 > targetspaceright | ...
% speedsh2 < -targetspaceleft | ...
% speedsv2 > targetspacebottom | ...
% speedsv2 < -targetspacetop | ...
% if onreturn; skip(targetscores==0) = false; end;
targetscores(skip) = inf;
% fail = speedsh + speedsv > maxthrottle;
% % speedpunish = speedsv+speedsh
% targetscores(fail) = inf;
% SCORING
% if pv ==14 && ph==6
% disp('buh');
% end
if goaljump && onreturn && any(targetscores==0 & ~isinf(targetmotor));
moveix = find(targetscores==0 & ~isinf(targetmotor));
elseif justturned
[~,moveix] = min(targetscores+targetscoresw+speeds2.^2+targetmotor.^2, [], 1);
else
%distanceab
distancegoal = sqrt(scores(pv, ph));
relativedistance = min(distancegoal/distanceab, 1);
if method == 1;
[result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
elseif method == 2;
scaledscore =sqrt(targetscoresw)*relativedistance+sqrt(targetscores)*(1-relativedistance);
[result,moveix] = min(2*scaledscore + speeds2+targetmotor, [], 1);
elseif method == 3;
[result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^2, [], 1);
elseif method == 4;
scaledscore = targetscoresw * relativedistance+targetscores * (1-relativedistance);
[result,moveix] = min(2*scaledscore + speeds2.^2+targetmotor.^0.001, [], 1);
elseif method == 5;
[result,moveix] = min(targetscores*2 + targetscoresw + speeds2.^3+targetmotor.^3, [], 1);
elseif method == 6;
% [result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^0.001, [], 1);
distance = sqrt(scores(pv,ph));
if relativedistance < 0.5
myscores = (targetscores+targetscoresw)./2;
else
myscores = targetscoresw;
end
[result, moveix] = min(myscores+targetmotor.^2, [], 1);
else
% elseif method == 7;
% if ~onreturn;
% [result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
% else
% [result,moveix] = min(targetscores*2 + targetscoresw + speeds2.^3+targetmotor.^3, [], 1);
% end
% elseif method == 8;
% if ~onreturn;
% [result,moveix] = min(targetscores*2 + targetscoresw + speeds2.^3+targetmotor.^3, [], 1);
% else
% [result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
% end
end
% debugmat = reshape(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, dv,dh)
if isinf(result)
step = step - 1;
break;
end
% fprintf('| step %d | s %d | sw %d | speed %d | motor %d\n', step, targetscores(moveix), targetscoresw(moveix), speeds2(moveix), targetmotor(moveix));
end
if isempty(moveix)
step = step - 1;
break;
end
newmh = targetmotorh(moveix);
newmv = targetmotorv(moveix);
newvh = vh + newmh;
newvv = vv + newmv;
newpv = pv + newvv;
newph = ph + newvh;
% nextstep = false;
if newvv == 0 && newvh == 0;
% disp('==== Stationary (must not happen)');
nextstep = true;
end
if ~onreturn && step >= 14;
% disp('==== #Steps to');
nextstep = true;
end
if step >= 26;
% disp('==== #Steps return');
nextstep = true;
end
curscore = scores(pv, ph);
nextscore = targetscores(moveix);
nextscorew = targetscoresw(moveix);
%TODO: CHECK
% if (curscore - nextscorew < abs(newmh) + abs(newmv)) && ~justturned
% if curscore < 1000 && (curscore - min(nextscore, nextscorew) < abs(newmh) + abs(newmv)) && ~justturned
if curscore < 10 && (curscore - nextscore < abs(newmh) + abs(newmv)) % && ~justturned
nextstep = true;
end
if nextstep
if onreturn
step = step - 1;
break;
else
justturned = true;
scores = scoresa;
scoresw = scoresaw;
beenthere = newtargetmap;
onreturn = true;
method = methodreturn;
end
else
justturned = false;
state(step, :) = [newmv, newmh];
step = step + 1;
pv = newpv;
ph = newph;
vv = newvv;
vh = newvh;
resultmotor = resultmotor + abs(newmh) + abs(newmv);
end
if step > 20;
step = step - 1;
done = true;
end;
end
thrustRow = state(1:step, 1);
thrustCol = state(1:step, 2);
da = scoresa(pv, ph);
result = resultmotor + bestdb +da;
% fprintf('| m %.0f | a %.0f | b %.0f | s %.0f |\n', resultmotor, da, bestdb, result);
end
end
|
