function [thrustRow, thrustCol] = solver1(chart, aIndex, bIndex, maxthrottle)
% maxwind = max(abs(chart(:)));
% npad = maxwind + maxthrottle;
nrows = size(chart, 1);
ncols = size(chart, 2);
% create padded chart
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);
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);
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);
justturned = false;
while ~done
% fprintf('Step %4d\n', step);
if ~justturned
% add winds to v
vh = vh + wh(pv,ph);
vv = vv + wv(pv,ph);
end
% get new middle-pos
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;
targetscores(skip) = inf;
% fail = speedsh + speedsv > maxthrottle;
% % speedpunish = speedsv+speedsh
% targetscores(fail) = inf;
if justturned
[~,moveix] = min(targetscores+targetscoresw+speeds2.^2+targetmotor.^2, [], 1);
% elseif onreturn
% [~,moveix] = min(targetscores+targetscoresw + targetmotor.^3, [], 1);
else
% myscore = min(targetscores, targetscoresw);
% [~,moveix] = min(myscore+speeds2.^2+targetmotor.^3, [], 1);
% [~,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
%good2
% [~,moveix] = min(targetscores*2 + targetscoresw + speeds2.^3+targetmotor.^3, [], 1);
%good1
[result,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
if isinf(result)
[thrustRow, thrustCol] = solver2(chart, aIndex, bIndex, maxthrottle);
return;
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;
% try the more defensive solver
[thrustRow, thrustCol] = solver2(chart, aIndex, bIndex, maxthrottle);
return;
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;
nextstep = true;
end
if ~onreturn && step >= 10;
nextstep = true;
end
if step >= 20;
nextstep = true;
end
curscore = scores(pv, ph);
nextscore = targetscores(moveix);
nextscorew = targetscoresw(moveix);
if (curscore - nextscorew < abs(newmh) + abs(newmv)) && ~justturned
nextstep = true;
end
% fprintf('%d %d - %d %d %d power %d\n', newpv, newph, curscore, nextscore, nextscorew, abs(newmh)+abs(newmv));
% if scores(pv, ph) - scores(newpv, newph) < abs(mh) + abs(mv)
if nextstep
% switch target or finish
if onreturn
step = step - 1;
break;
else
justturned = true;
scores = scoresa;
scoresw = scoresaw;
onreturn = true;
end
else
justturned = false;
% continue to target
state(step, :) = [newmv, newmh];
step = step + 1;
pv = newpv;
ph = newph;
vv = newvv;
vh = newvh;
end
if step > 20;
step = step - 1;
done = true;
end;
end
thrustRow = state(1:step, 1);
thrustCol = state(1:step, 2);
% thrustRow = 0;
% thrustCol = 0;
end
function [thrustRow, thrustCol] = solver2(chart, aIndex, bIndex, maxthrottle)
% maxwind = max(abs(chart(:)));
% npad = maxwind + maxthrottle;
nrows = size(chart, 1);
ncols = size(chart, 2);
% create padded chart
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);
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);
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);
justturned = false;
while ~done
% fprintf('Step %4d\n', step);
if ~justturned
% add winds to v
vh = vh + wh(pv,ph);
vv = vv + wv(pv,ph);
end
% get new middle-pos
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;
targetscores(skip) = inf;
% fail = speedsh + speedsv > maxthrottle;
% % speedpunish = speedsv+speedsh
% targetscores(fail) = inf;
if justturned
[~,moveix] = min(targetscores+targetscoresw+speeds2.^2+targetmotor.^2, [], 1);
% elseif onreturn
% [~,moveix] = min(targetscores+targetscoresw + targetmotor.^3, [], 1);
else
% myscore = min(targetscores, targetscoresw);
% [~,moveix] = min(myscore+speeds2.^2+targetmotor.^3, [], 1);
% [~,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
%good2
[r,moveix] = min(targetscores*2 + targetscoresw + speeds2.^3+targetmotor.^3, [], 1);
if isinf(r)
[thrustRow, thrustCol] = solver3(chart, aIndex, bIndex, maxthrottle);
return;
end
%good1
% [~,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
% 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)
[thrustRow, thrustCol] = solver3(chart, aIndex, bIndex, maxthrottle);
return;
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;
nextstep = true;
end
if ~onreturn && step >= 10;
nextstep = true;
end
if step >= 20;
nextstep = true;
end
curscore = scores(pv, ph);
nextscore = targetscores(moveix);
nextscorew = targetscoresw(moveix);
if (curscore - nextscorew < abs(newmh) + abs(newmv)) && ~justturned
nextstep = true;
end
% fprintf('%d %d - %d %d %d power %d\n', newpv, newph, curscore, nextscore, nextscorew, abs(newmh)+abs(newmv));
% if scores(pv, ph) - scores(newpv, newph) < abs(mh) + abs(mv)
if nextstep
% switch target or finish
if onreturn
step = step - 1;
break;
else
justturned = true;
scores = scoresa;
scoresw = scoresaw;
onreturn = true;
end
else
justturned = false;
% continue to target
state(step, :) = [newmv, newmh];
step = step + 1;
pv = newpv;
ph = newph;
vv = newvv;
vh = newvh;
end
if step > 20;
step = step - 1;
done = true;
end;
end
thrustRow = state(1:step, 1);
thrustCol = state(1:step, 2);
% thrustRow = 0;
% thrustCol = 0;
end
function [thrustRow, thrustCol] = solver3(chart, aIndex, bIndex, maxthrottle)
% maxwind = max(abs(chart(:)));
% npad = maxwind + maxthrottle;
nrows = size(chart, 1);
ncols = size(chart, 2);
% create padded chart
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);
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);
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);
justturned = false;
while ~done
% fprintf('Step %4d\n', step);
if ~justturned
% add winds to v
vh = vh + wh(pv,ph);
vv = vv + wv(pv,ph);
end
% get new middle-pos
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;
targetscores(skip) = inf;
% fail = speedsh + speedsv > maxthrottle;
% % speedpunish = speedsv+speedsh
% targetscores(fail) = inf;
if justturned
[~,moveix] = min(targetscores+targetscoresw+speeds2.^2+targetmotor.^2, [], 1);
% elseif onreturn
% [~,moveix] = min(targetscores+targetscoresw + targetmotor.^3, [], 1);
else
% myscore = min(targetscores, targetscoresw);
% [~,moveix] = min(myscore+speeds2.^2+targetmotor.^3, [], 1);
% [~,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
%good2
[r,moveix] = min(targetscores*2 + targetscoresw + speeds2.^4+targetmotor.^3, [], 1);
%if isinf(r); while(true); 1+1; end;
%good1
% [~,moveix] = min(targetscores + targetscoresw + speeds2.^2+targetmotor.^3, [], 1);
% 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)
%while(true); 1+1; end;
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;
nextstep = true;
end
if ~onreturn && step >= 10;
nextstep = true;
end
if step >= 20;
nextstep = true;
end
curscore = scores(pv, ph);
nextscore = targetscores(moveix);
nextscorew = targetscoresw(moveix);
if (curscore - nextscorew < abs(newmh) + abs(newmv)) && ~justturned
nextstep = true;
end
% fprintf('%d %d - %d %d %d power %d\n', newpv, newph, curscore, nextscore, nextscorew, abs(newmh)+abs(newmv));
% if scores(pv, ph) - scores(newpv, newph) < abs(mh) + abs(mv)
if nextstep
% switch target or finish
if onreturn
step = step - 1;
break;
else
justturned = true;
scores = scoresa;
scoresw = scoresaw;
onreturn = true;
end
else
justturned = false;
% continue to target
state(step, :) = [newmv, newmh];
step = step + 1;
pv = newpv;
ph = newph;
vv = newvv;
vh = newvh;
end
if step > 20;
step = step - 1;
done = true;
end;
end
thrustRow = state(1:step, 1);
thrustCol = state(1:step, 2);
% thrustRow = 0;
% thrustCol = 0;
end
|
