function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Basic solver, does nothing
ywinds = chart(:,:,1);
xwinds = chart(:,:,2);
nrow = size(ywinds,1);
ay = rem(aIndex-1, nrow) + 1;
ax = (aIndex - ay)/nrow + 1;
by = rem(bIndex-1, nrow) + 1;
bx = (bIndex - by)/nrow + 1;
[xval,yval]=meshgrid(1:size(xwinds,2),1:size(xwinds,1));
position=reshape(1:numel(xwinds),size(xwinds));
targetScore = 20;
targetIterations = 500;
maxIter = 2000;
ydirection = 2*(by > ay) - 1;
xdirection = 2*(bx > ax) - 1;
%%
fuel = Inf(size(xwinds));
fuel(aIndex) = 0;
fueltoreverse = fuel;
xvel = zeros(size(xwinds));
yvel = zeros(size(xwinds));
checked = false(size(xwinds));
previousstop = zeros(size(xwinds));
iteration=0;
minval=0;
costtoB = Inf;
while(minval < costtoB && ~all(checked(:)) && costtoB > targetScore*iteration/targetIterations && iteration<maxIter)
iteration=iteration+1;
[minval,minidx]=min(fuel(~checked));
reference = position(~checked);
minidx = reference(minidx);
[i_y,i_x] = ind2sub(size(fuel),minidx);
i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
i_vx = xvel(minidx) + xwinds(minidx);
i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
i_newvy = yval-i_y;
i_newvx = xval-i_x;
i_fuel = i_thrust + minval; %fuel used to get to position.
i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
fuel(i_improvement) = i_fuel(i_improvement);
xvel(i_improvement) = i_newvx(i_improvement);
yvel(i_improvement) = i_newvy(i_improvement);
fueltoreverse(i_improvement) = fuel(i_improvement) + ...
abs(yvel(i_improvement)).*(yvel(i_improvement)*ydirection>0) + ...
abs(xvel(i_improvement)).*(xvel(i_improvement)*xdirection>0) ;
previousstop(i_improvement) = minidx;
costtoB = min(min(fueltoreverse + (xval-bx).^2+(yval-by).^2));
checked(i_improvement) = false;
checked(minidx) = true;
end
%%
fuel = fuel + (xval-bx).^2 + (yval-by).^2;
costtoA = min(min(fuel + (xval-ax).^2 + (yval-ay).^2));
checked = false(size(xwinds));
returnpreviousstop = zeros(size(xwinds));
iteration=0;
minval=0;
while(minval < costtoA && ~all(checked(:)) && (costtoA-costtoB) > targetScore*iteration/targetIterations && iteration<maxIter)
iteration=iteration+1;
[minval,minidx]=min(fuel(~checked));
reference = position(~checked);
minidx = reference(minidx);
[i_y,i_x] = ind2sub(size(fuel),minidx);
i_vy = yvel(minidx) + ywinds(minidx); %velocity to point minidx plus localwind
i_vx = xvel(minidx) + xwinds(minidx);
i_thrust = abs(xval-i_x-i_vx)+abs(yval-i_y-i_vy); %xpos=i_x+i_vx
i_newvy = yval-i_y;
i_newvx = xval-i_x;
i_fuel = i_thrust + minval; %fuel used to get to position.
i_improvement = i_thrust < maxThrottle & i_fuel < fuel;
fuel(i_improvement) = i_fuel(i_improvement);
xvel(i_improvement) = i_newvx(i_improvement);
yvel(i_improvement) = i_newvy(i_improvement);
returnpreviousstop(i_improvement) = minidx;
costtoA = min(min(fuel + (xval-ax).^2 + (yval-ay).^2));
checked(i_improvement) = false;
checked(minidx) = true;
end
%%
costtoA = fuel + (xval-ax).^2 + (yval-ay).^2;
[minval,minidx] = min(costtoA(:));
indices = minidx(1);
nextindex = returnpreviousstop(indices(end));
while(nextindex)
indices(end+1) = nextindex;
nextindex = returnpreviousstop(indices(end));
end
nextindex = previousstop(indices(end));
while(nextindex)
indices(end+1) = nextindex;
nextindex = previousstop(indices(end));
end
indices = rot90(indices,2);
[ypos,xpos] = ind2sub(size(xwinds),indices); %n+1 elements
xw = xwinds(indices); %n+1
yw = ywinds(indices); %n+1
xspeed = diff(xpos); %n
yspeed = diff(ypos); %n
xdrive = diff([0 xspeed]); %n
ydrive = diff([0 yspeed]); %n
if(~isempty(xdrive))
xmotor = xdrive - xw(1:end-1);
ymotor = ydrive - yw(1:end-1);
x_tomove = ax - xpos(end);
x_todrive = x_tomove - xspeed(end) - xw(end);
y_tomove = ay - ypos(end);
y_todrive = y_tomove - yspeed(end) - yw(end);
if( (abs(x_todrive)+abs(y_todrive)) < maxThrottle)
if( (abs(x_todrive)+abs(y_todrive)) < ( abs(ax-xpos(end))+abs(ay-ypos(end)) ) )
xmotor = [xmotor x_todrive];
ymotor = [ymotor y_todrive];
end
end
else
xmotor = [];
ymotor = [];
end
thrustRow = ymotor;
thrustCol = xmotor;
end
|
