| Code: |
function [thrustRow, thrustCol] = triple(chart, aIndex, bIndex, maxThrottle)
[thrustRow, thrustCol, score] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle);
%score0 = runsolution(thrustRow0, thrustCol0, chart(:,:,2), chart(:,:,1), aIndex, bIndex);
[thrustRow1, thrustCol1] = nickelfelpeterv3(chart, aIndex, bIndex, maxThrottle);
score1 = runsolution(thrustRow1, thrustCol1, chart(:,:,2), chart(:,:,1), aIndex, bIndex);
[thrustRow2, thrustCol2, score2] = zeemcv(chart, aIndex, bIndex, maxThrottle);
[~,k]=min([score score1 score2]);
switch k
case 1
return;
case 2
thrustRow=thrustRow1;
thrustCol=thrustCol1;
case 3
thrustRow=thrustRow2;
thrustCol=thrustCol2;
end
end
function [thrustRow, thrustCol, score] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle)
%REP
%by Gwendolyn Fischer
%r4537 t44
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;
dist_to_A = (X - ax).^2 + (Y - ay).^2;
maxIter = 1034;
SDBiter = [200 200];
SDBzoom = [inf 1/2];
INF = 100000;
dist_to_B_w = dist_to_B*1e-4;
dist_to_A_w = dist_to_A*1e-4;
y_dir = 2*(by > ay) - 1;
x_dir = 2*(bx > ax) - 1;
fuel = Inf(ny,nx);
fuel(aIndex) = 0;
fuel_to_reverse = fuel;
xvel = zeros(ny,nx);
yvel = zeros(ny,nx);
checked = zeros(ny,nx);
previous_stop = zeros(ny,nx);
min_fuel = 0;
[cost_to_B cost_to_B_idx] = min(fuel_to_reverse(:) + dist_to_B(:));
zoon = nx*ny*SDBzoom.^2;
for i = 1:2
SDB_tf = dist_to_B(:)<=zoon(i);%nx*ny*SDBzoom(i)*SDBzoom(i);
SDB_idx = find(SDB_tf);
SDBchecked = checked(SDB_tf);
SDBfuel = fuel(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);
it = 0;
slowDown = maxThrottle+SDBdist_to_B/4-4;
while ( min_fuel <= cost_to_B && ~all(SDBchecked) && it < SDBiter(i))
while ( min_fuel <= cost_to_B && it < SDBiter(i))
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);
i_thrust = i_thrust + abs(i_new_vy - i_vy);
i_fuel = i_thrust + min_fuel;
i_thrust_logic = i_thrust <= maxThrottle;
i_reacheable = abs(i_new_vy + y_winds(i_p_full));
i_reacheable = i_reacheable + abs(i_new_vx + x_winds(i_p_full));
i_reacheable_logic = i_reacheable < slowDown;
i_reacheable_logic = i_reacheable_logic & i_thrust_logic;
i_optimum = i_fuel < SDBfuel;
i_impr_tf = i_reacheable & i_optimum;
i_impr_idx = find(i_impr_tf);
if isempty(i_impr_idx)
SDBchecked(i_p) = 100; %true;
continue;
end
i_fuel_impr = i_fuel (i_impr_idx) ;
SDBxvel (i_impr_idx) = i_new_vx(i_impr_idx);
SDByvel (i_impr_idx) = i_new_vy(i_impr_idx);
SDBfuel (i_impr_idx) = i_fuel_impr;
SDBprevious_stop (i_impr_idx) = i_p_full;
cost_to_B = min(cost_to_B,min(SDBfuel(i_impr_idx) + SDBdist_to_B(i_impr_idx)));
SDBchecked(i_impr_idx) = 0; %false;
SDBchecked(i_p) = 100; %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 = zeros(ny,nx);
it = 0;
min_fuel = 0;
while ( min_fuel < cost_to_A && ~all(checked(:)) && it < maxIter )
while ( min_fuel < cost_to_A && 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);
if isempty(i_impr_idx)
checked(i_p) = 100; %true;
continue;
end
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(cost_to_A,min(fuel(i_impr_idx) + dist_to_A(i_impr_idx)));
%checked(i_impr_idx) = 0;
checked(i_p) = 100;
checked(i_impr_idx) = 0;
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);
xspeed = diff(xpos);
yspeed = diff(ypos);
xdrive = diff([0; xspeed]);
ydrive = diff([0; yspeed]);
thrustCol = xdrive - xw(1:end-1);
thrustRow = ydrive - yw(1:end-1);
score = min((xpos-bx).^2 + (ypos-by).^2) + sum(abs(thrustCol))+sum(abs(thrustRow)) + (ypos(end)-ay).^2 + (xpos(end)-ax).^2;
end
function [thrustRow, thrustCol] = nickelfelpeterv3(chart, aIndex, bIndex, maxThrottle)
%continue
%by Sebastian Ullmann
%r4107 t50
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.155;
dist_to_B_w = dist_to_B*1e-4;
dist_to_A_w = dist_to_A*1e-4;
fuel = 100*ones(ny,nx);
fuel(aIndex) = 0;
xvel = zeros(ny,nx);
yvel = xvel;
checked = xvel;
previous_stop = xvel;
min_fuel = 0;
cost_to_B = dist_to_B(aIndex);
zoon = nx*ny*[10 0.37];
ziter = [500 700];% .* mxrand(-0.05);
sf = [0.31 0.19];
for i = 1:2
SDB_tf = dist_to_B(:)<=zoon(i);
SDB_idx = find(SDB_tf);
SDBchecked = checked(SDB_tf);
SDBfuel = fuel(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);
slowdown = maxThrottle+SDBdist_to_B*sf(i);
it = 0;
while ( min_fuel < cost_to_B && ~all(SDBchecked) && it < ziter(i))
while ( min_fuel < cost_to_B && it < ziter(i))
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_y_winds = y_winds(i_p_full);
i_x_winds = x_winds(i_p_full);
i_vy = SDByvel(i_p) + i_y_winds;
i_vx = SDBxvel(i_p) + i_x_winds;
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+i_y_winds)+abs(i_new_vx+i_x_winds)<slowdown) & (i_fuel < SDBfuel);
i_impr_idx = find(i_impr_tf);
if isempty(i_impr_idx)
SDBchecked(i_p) = 100; %true;
continue;
end
i_fuel_impr = i_fuel (i_impr_idx) ;
SDBxvel (i_impr_idx) = i_new_vx(i_impr_idx);
SDByvel (i_impr_idx) = i_new_vy(i_impr_idx);
SDBfuel (i_impr_idx) = i_fuel_impr;
SDBprevious_stop (i_impr_idx) = i_p_full;
cost_to_B = min(cost_to_B,min(SDBfuel(i_impr_idx) + SDBdist_to_B(i_impr_idx)));
%SDBchecked(i_impr_idx) = 0; %false;
SDBchecked(i_p) = 100; %true;
SDBchecked(i_impr_idx) = 0; %false;
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 = min(fuel(:) + dist_to_A(:));
checked(:) = 0;
return_previous_stop = checked;
it = 0;
min_fuel = 0;
while ( min_fuel < cost_to_A && ~all(checked(:)) && it < min(nx*ny,2000))
while ( min_fuel < cost_to_A && it < min(nx*ny,2000))
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_impr_tf = i_thrust <= maxThrottle & i_fuel < fuel;
i_impr_idx = find(i_impr_tf);
if isempty(i_impr_idx)
checked(i_p) = 100; %true;
continue;
end
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(cost_to_A,min(fuel(i_impr_idx) + dist_to_A(i_impr_idx)));
checked(i_impr_idx) = 0;
checked(i_p) = 100;
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
% function r = myrand(x)
% r = 1 + x * (rand(1,2) - 0.5) * 2;
% end
function r = mxrand(x)
r = 1 + x * rand(1,2);
end
function score = runsolution(thrustRow, thrustCol, colWind, rowWind, aIndex, bIndex)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
[ny,nx] = size(colWind);
AR = rem(aIndex-1, ny) + 1;
AC = (aIndex - AR)/ny + 1;
BR = rem(bIndex-1, ny) + 1;
BC = (bIndex - BR)/ny + 1;
% Initialize variables at start point (A)
fR = AR; fC =AC;
fvR = 0; fvC = 0;
dB = (fR-BR)^2 + (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;
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
dBtmp = (fR-BR)^2 + (fC-BC)^2;
if dBtmp < dB
dB = dBtmp;
end
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end
function [thrustRow, thrustCol,score] = zeemcv(chart, aIndex, bIndex, maxThrottle)
% global Pcv
% Pcv=zeros(10000,1);
% global sw0
% sw = ceil(rand(1)*20);
% if sw==sw0
% kp = 1 - 0.05*sw;
% else
% kp = 1;
% end
winds = chart(:,:,1)*1i + chart(:,:,2);
[ny,nx] = size(winds);
winds = winds(:);
ay = rem(aIndex-1, ny) + 1;
ax = (aIndex - ay)/ny + 1;
az = ay*1i + ax;
by = rem(bIndex-1, ny) + 1;
bx = (bIndex - by)/ny + 1;
bz = by*1i + bx;
x = (1:nx);
y = (1:ny)';
Z = reshape(bsxfun(@plus,y*1i,x),[],1);
% targetScore = 0.013;
Z2B = Z - bz;
dist_to_B = conj(Z2B).*Z2B;
max_dist_to_B = nx^2+ny^2;
fract_d2B = dist_to_B/(max_dist_to_B + 1);
Z2A = Z - az;
dist_to_A = conj(Z2A).*Z2A;
max_dist_to_A = max_dist_to_B;
fract_d2A = dist_to_A/(max_dist_to_A + 1);
fuel = Inf(ny*nx,1);
fuel(aIndex) = 0;
checked = fract_d2B;
% checked = zeros(ny*nx,1);
previous_stop = zeros(ny*nx,1);
INF = 100000;
min_fuel = 0;
zoonB = dist_to_B < 2;
zoonA = dist_to_A < 2;
p = aIndex;
checked(p) = Inf;
bestz = Z(p);
target = bestz + winds(p);
% tvz = 0;
% it=0;
% md2B = (maxThrottle-2)+dist_to_B*0.25;
zwind = Z + winds;
SDBiter = [700 700];
SDBzoom = [Inf 0.36]*nx*ny;
pfull = p;
for i = 1:length(SDBzoom)
SDB_tf = dist_to_B(:)<=SDBzoom(i);
SDB_idx = find(SDB_tf);
SDBchecked = checked(SDB_tf);
SDBfuel = fuel(SDB_tf);
% SDBfuel_to_reverse = fuel_to_reverse(SDB_tf);
% SDBfract_d2B = fract_d2B(SDB_tf);
SDBdist_to_B = dist_to_B(SDB_tf);
% SDBzvel = zvel(SDB_tf);
SDBZ = Z(SDB_tf);
SDBprevious_stop = previous_stop(SDB_tf);
% SDBcost_to_B_idx = find(cost_to_B_idx==SDB_idx);
SDBwinds = winds(SDB_tf);
SDBzwind = zwind(SDB_tf);
% SDBmd2B = md2B(SDB_tf);
SDBzoonB = zoonB(SDB_tf);
slowdown = maxThrottle+SDBdist_to_B/4;
it = 0;
while ~any(SDBchecked(SDBzoonB)>1) && it < SDBiter(i)
it=it+1;
% Pcv(it)=p;
zp = SDBZ - target;
totalT = abs(real(zp)) + abs(imag(zp));
boxp = totalT <=maxThrottle; % & rowT<=maxThrottle & ~checked;
new_fuel = totalT + min_fuel;
update = boxp & new_fuel < SDBfuel;
% T = Z(update) - (bestz+winds(p));
T = SDBzwind(update) - bestz;
% T = zwind(update) + (tvz-target); % zp + tvz + winds;
update(update) = abs(real(T)) + abs(imag(T)) < slowdown(update);
SDBfuel(update) = new_fuel(update);
SDBprevious_stop(update) = pfull;
[mfd,p] = min(SDBfuel + SDBchecked);
if mfd == Inf
break
end
pfull = SDB_idx(p);
min_fuel = SDBfuel(p);
bestz = SDBZ(p);
z = Z(SDBprevious_stop(p));
tvz = bestz - z;
target = bestz + tvz + SDBwinds(p);
SDBchecked(p) = Inf;
end
fuel(SDB_tf) = SDBfuel;
previous_stop(SDB_tf) = SDBprevious_stop;
% zvel(SDB_tf) = SDBzvel;
end
fuel = fuel + dist_to_B;
[~,p] = min(fuel);
checked = fract_d2A;
checked(p) = Inf;
return_previous_stop = zeros(ny*nx,1);
target = 2*Z(p)-Z(previous_stop(p))+winds(p);
min_fuel = fuel(p);
while ~any(checked(zoonA)>1)
% it=it+1;
% Pcv(it)=p;
zp = Z - target;
totalT = abs(real(zp)) + abs(imag(zp));
boxp = totalT <=maxThrottle; % & rowT<=maxThrottle & ~checked;
new_fuel = totalT + min_fuel;
update = boxp & new_fuel < fuel;
fuel(update) = new_fuel(update);
return_previous_stop(update) = p;
[~,p] = min(fuel + checked);
min_fuel = fuel(p);
bestz = Z(p);
if ~return_previous_stop(p)
z = Z(previous_stop(p));
else
z = Z(return_previous_stop(p));
end
target = 2*bestz -z + winds(p);
checked(p) = INF;
end
[score,p] = min(fuel+dist_to_A);
% it=it+1;
% Pcv(it)=p;
tra=zeros(1,1000);
k=0;
turned=false;
while p
k=k+1;
tra(k)=p;
if turned||~return_previous_stop(p)
turned=true;
p=previous_stop(p);
else
p=return_previous_stop(p);
end
end
tra=tra(k:-1:1);
trar = rem(tra-1, ny) + 1;
trac = (tra - trar)/ny + 1;
accelr=diff([0 diff(trar)]);
accelc=diff([0 diff(trac)]);
if length(tra)<2
thrustRow=[];
thrustCol=[];
else
w = winds(tra(1:end-1)).';
thrustRow = accelr-imag(w);
thrustCol = accelc-real(w);
end
end
|
