function [thrustRow, thrustCol] = nickelfelpeter(chart, aIndex, bIndex, maxThrottle)
y_winds = chart(:,:,1);
x_winds = chart(:,:,2);
[ny,nx] = size(x_winds);
ay = rem(aIndex-1, ny) + 1;
ax = (aIndex - ay)/ny + 1;
by = rem(bIndex-1, ny) + 1;
bx = (bIndex - by)/ny + 1;
x = (1:nx);
y = (1:ny)';
X = x(ones(ny,1),:);
Y = y(:,ones(nx,1));
dist_to_B = ((X - bx).^2 + (Y - by).^2).^1.15;
dist_to_A = ((X - ax).^2 + (Y - ay).^2).^1.16;
maxIter = 1670;
INF = 100000;
SDBzoom = [INF 0.6 0.4];
dist_to_B_w = dist_to_B*1e-4;
dist_to_A_w = dist_to_A*1e-4;
fuel = INF*ones(ny,nx);
fuel(aIndex) = 0;
fuel_to_reverse = fuel;
xvel = zeros(ny,nx);
yvel = xvel;
checked = xvel;
previous_stop = xvel;
min_fuel = 0;
cost_to_B = dist_to_B(aIndex);
cost_to_B_idx = aIndex;
for i = 1:length(SDBzoom)
SDB_tf = dist_to_B(:)<=nx*ny*SDBzoom(i)*SDBzoom(i);
SDB_idx = find(SDB_tf);
SDBchecked = checked(SDB_tf);
SDBfuel = fuel(SDB_tf);
SDBfuel_to_reverse = fuel_to_reverse(SDB_tf);
SDBdist_to_B_w = dist_to_B_w(SDB_tf);
SDBdist_to_B = dist_to_B(SDB_tf);
SDBxvel = xvel(SDB_tf);
SDByvel = yvel(SDB_tf);
SDBX = X(SDB_tf);
SDBY = Y(SDB_tf);
SDBprevious_stop = previous_stop(SDB_tf);
SDBcost_to_B_idx = find(cost_to_B_idx==SDB_idx);
slowdown = maxThrottle+SDBdist_to_B/4;
it = 0;
while ( min_fuel <= cost_to_B && ~all(SDBchecked) && it < 500)
it = it + 1;
metrix = SDBfuel + SDBchecked + SDBdist_to_B_w;
[dummy,i_p] = min(metrix(:));
i_p_full = SDB_idx(i_p);
min_fuel = SDBfuel(i_p);
i_y = rem(i_p_full - 1,ny) + 1;
i_x = (i_p_full - i_y)/ny + 1;
i_vy = SDByvel(i_p) + y_winds(i_p_full);
i_vx = SDBxvel(i_p) + x_winds(i_p_full);
i_new_vy = SDBY - i_y;
i_new_vx = SDBX - i_x;
i_thrust = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
i_fuel = i_thrust + min_fuel;
i_impr_tf = (i_thrust <= maxThrottle) & (abs(i_new_vy+y_winds(i_p_full))+abs(i_new_vx+x_winds(i_p_full))<slowdown) & (i_fuel < SDBfuel);
i_impr_idx = find(i_impr_tf);
i_new_vx_impr = i_new_vx(i_impr_idx);
i_new_vy_impr = i_new_vy(i_impr_idx);
i_fuel_impr = i_fuel (i_impr_idx) ;
SDBxvel (i_impr_idx) = i_new_vx_impr;
SDByvel (i_impr_idx) = i_new_vy_impr;
SDBfuel (i_impr_idx) = i_fuel_impr;
SDBprevious_stop (i_impr_idx) = i_p_full;
if i_impr_tf(SDBcost_to_B_idx)
[cost_to_B SDBcost_to_B_idx] = min(SDBfuel(:) + SDBdist_to_B(:));
else
[cost_to_B_new SDBcost_to_B_idx_new] = min(SDBfuel(i_impr_idx) ...
+ SDBdist_to_B(i_impr_idx));
if cost_to_B_new<cost_to_B
cost_to_B = cost_to_B_new;
SDBcost_to_B_idx = i_impr_idx(SDBcost_to_B_idx_new);
end
end
SDBchecked(i_impr_idx) = 0; %false;
SDBchecked(i_p) = INF; %true;
end
fuel(SDB_tf) = SDBfuel;
previous_stop(SDB_tf) = SDBprevious_stop;
xvel(SDB_tf) = SDBxvel;
yvel(SDB_tf) = SDByvel;
end
fuel = fuel + dist_to_B;
[cost_to_A cost_to_A_idx] = min(fuel(:) + dist_to_A(:));
checked = checked*0;
return_previous_stop = checked;
it = 0;
min_fuel = 0;
while ( min_fuel < cost_to_A && ~all(checked(:)) && it < maxIter )
it = it + 1;
metrix = fuel + checked + dist_to_A_w; %****
[dummy,i_p] = min(metrix(:)); %****
min_fuel = fuel(i_p); %****
i_y = rem(i_p - 1,ny) + 1;
i_x = (i_p - i_y)/ny + 1;
i_vy = yvel(i_p) + y_winds(i_p);
i_vx = xvel(i_p) + x_winds(i_p);
i_new_vy = Y-i_y;
i_new_vx = X-i_x;
i_thrust = abs(i_new_vx - i_vx) + abs(i_new_vy - i_vy);
i_fuel = i_thrust + min_fuel;
i_reacheable = i_thrust <= maxThrottle;
i_optimum = i_fuel < fuel;
i_impr_tf = i_reacheable & i_optimum;
i_impr_idx = find(i_impr_tf);
fuel (i_impr_idx) = i_fuel(i_impr_idx);
xvel (i_impr_idx) = i_new_vx(i_impr_idx);
yvel (i_impr_idx) = i_new_vy(i_impr_idx);
return_previous_stop(i_impr_idx) = i_p;
[cost_to_A] = min(fuel(:) + dist_to_A(:));
checked(i_impr_idx) = 0;
checked(i_p) = INF;
end
cost_to_A = fuel + dist_to_A;
[dummy,min_fuel] = min(cost_to_A(:));
indices = zeros(nx*ny,1);
indices(1) = min_fuel(1);
next_move = return_previous_stop(indices(1));
k = 1;
[indices, k] = while_next_move(indices, k, next_move, return_previous_stop);
next_move = previous_stop(indices(k));
[indices, k] = while_next_move(indices, k, next_move, previous_stop);
indices = indices(k:-1:1);
ypos = rem(indices - 1, ny) + 1;
xpos = (indices - ypos)/ny + 1;
xw = x_winds(indices);
yw = y_winds(indices);
thrustCol = diff([0; diff(xpos)]) - xw(1:end-1);
thrustRow = diff([0; diff(ypos)]) - yw(1:end-1);
end
function [indices, k] = while_next_move(indices, k, next_move, previous_stop)
while(next_move)
k = k + 1;
indices(k) = next_move;
next_move = previous_stop(next_move);
end
end
|
