function [myv, mxv] = solverbwb04(chart, aIndex, bIndex, t)
[myv mxv] = solverlead(chart, aIndex, bIndex, t);
chartx = chart(:,:,2);
chartxv = chartx(:);
charty = chart(:,:,1);
chartyv = charty(:);
[gy gx] = ind2sub(size(chart(:,:,1)),bIndex);
[hy hx] = ind2sub(size(chart(:,:,1)),aIndex);
[sy sx] = size(chartx);
n = 1;
sizesolmat = numel(mxv) + 1;
initsolmat();
solmat(1,:) = [hx hy 0 0 nan nan nan 0 0 (gx-hx)^2 0 false false false];
lastidx = 1;
for id = 1:numel(mxv)
cpold(mxv(id),myv(id));
end
scorelead = solmat(id+1,9) + solmat(id+1,10) + solmat(id+1,11);
[~,idgoal] = min(solmat(:,10));
solmat(1:idgoal-1,12:13) = true;
maxpotentialhome = solmat(id+1,11);
maxpotentialgoal = solmat(id+1,10);
maxpotentialpath = scorelead - solmat(idgoal,9) - solmat(idgoal,10);
maxpotential = maxpotentialhome + maxpotentialpath;
maxthrottlelimit = scorelead - solmat(idgoal,9) - maxpotentialgoal - 1;
if maxthrottlelimit == 0, return, end
t = min(t, maxthrottlelimit);
[tcx tcy] = inittc(t);
maxtc = numel(tcx{end});
%%%%%%%%%%%%%%%%%%%% Parameter %%%%%%%%%%%%%%%%%%%%%
%if maxpotentialgoal ~= 0, return, end
maxdriftsteps = 30;
maxnumelidsolutions = 1000;
sizesolmat = round(70 * maxpotential * sqrt(sx*sy));
%%%%%%%%%%%%%%%%%%%%%%% start %%%%%%%%%%%%%%%%%%%%%%
solmatmini = solmat;
initsolmat();
solmat(1:size(solmatmini,1),:) = solmatmini;
drift(0);
while true
%calculated = ;
%outofboard = solmat(:,14);
myscore = solmat(:,9);
myscore(solmat(:,12)|solmat(:,14)) = inf;
myscoremin = min(myscore);
idsolutions = find(myscoremin == myscore);
if myscoremin >= scorelead || lastidx + numel(idsolutions) * maxtc > sizesolmat || numel(idsolutions) > maxnumelidsolutions;
break;
end
for id = idsolutions'
cp();
end
drift(0);
lastidx;
end
outofboard = logical(solmat(:,14));
myscore = solmat(:,9) + solmat(:,10) + solmat(:,11);
myscore(outofboard) = inf;
[scoremysolver idsol] = min(myscore);
if scoremysolver < scorelead
f();
end
function f()
myv = [];
mxv = [];
for i=1:998
if idsol==1
break
end
mxv = [solmat(idsol,5);mxv];
myv = [solmat(idsol,6);myv];
idsol = solmat(idsol,7);
end
end
%%%%%%%%%%%%%%%%%%%% functions %%%%%%%%%%%%%%%%%%%%%%
function cp()
px = solmat(id,1);
py = solmat(id,2);
vx = solmat(id,3);
vy = solmat(id,4);
et = solmat(id,9);
eg = solmat(id,10);
wx = chartx(py,px);
wy = charty(py,px);
maxthrottlehere = scorelead - solmat(id,9) - maxpotentialgoal - 1;
idcp = max(1,min(maxthrottlehere,t));
mytcx = tcx{idcp};
mytcy = tcy{idcp};
n = numel(mytcx);
solmat(id,12) = true;
vvx = vx + mytcx + wx;
vvy = vy + mytcy + wy;
vpx = px + vvx;
vpy = py + vvy;
veh = (hx - vpx).^2 + (hy - vpy).^2;
veg = (gx - vpx).^2 + (gy - vpy).^2;
% outside or stay at point:
solmat((lastidx+1):(lastidx+n),:) = [vpx vpy vvx vvy mytcx mytcy id.*ones(n,1) zeros(n,1) et+abs(mytcx)+abs(mytcy) min(veg,eg) veh false(n,2) (vpx < 1 | vpy < 1 | vpx > sx | vpy > sy | (vvx == vx & vvy == vy & vpx == px & vpy == py))];
lastidx = lastidx + n;
end
function cpold(tcx,tcy)
px = solmat(id,1);
py = solmat(id,2);
vx = solmat(id,3);
vy = solmat(id,4);
et = solmat(id,9);
eg = solmat(id,10);
wx = chartx(py,px);
wy = charty(py,px);
vvx = vx + tcx + wx;
vvy = vy + tcy + wy;
vpx = px + vvx;
vpy = py + vvy;
veh = (hx - vpx).^2 + (hy - vpy).^2;
veg = (gx - vpx).^2 + (gy - vpy).^2;
%no error check:
solmat((lastidx+1):(lastidx+n),:) = [vpx vpy vvx vvy tcx tcy id.*ones(n,1) zeros(n,1) et+abs(tcx)+abs(tcy) min(veg,eg) veh false(n,3)];
lastidx = lastidx + 1;
end
function step = drift(step)
idv = find(~solmat(1:lastidx,13) & ~solmat(1:lastidx,14));
nd = numel(idv);
if lastidx+nd > sizesolmat
idv = idv(1:sizesolmat-lastidx);
end
if numel(idv) == 0 || step > maxdriftsteps;
return
end
solmat(idv,13) = true;
vpxold = solmat(idv,1);
vpyold = solmat(idv,2);
vvxold = solmat(idv,3);
vvyold = solmat(idv,4);
tix = vpyold + (vpxold-1) * sy;
vvx = vvxold + chartxv(tix);
vvy = vvyold + chartyv(tix);
vet = solmat(idv,9);
veg = solmat(idv,10);
vpx = vpxold + vvx;
vpy = vpyold + vvy;
nd = numel(vpx);
veh = (hx - vpx).^2 + (hy - vpy).^2;
if lastidx+nd > sizesolmat
solmat(lastidx+1:sizesolmat) = newmat(1:size(solmat,1)-(lastidx));
lastidx = sizesolmat;
return
else
solmat((lastidx+1):(lastidx+nd),:) = [vpx vpy vvx vvy zeros(nd,2) idv zeros(nd,1) vet veg.*ones(nd,1) veh false(nd,2) (vpx < 1 | vpy < 1 | vpx > sx | vpy > sy | (vvx == vvxold & vvy == vvyold & vpx == vpxold & vpy == vpyold))];
end
lastidx = lastidx + nd;
step = drift(step + 1);
end
function initsolmat()
solmat = zeros(sizesolmat,14);
solmat(:,9) = inf;
solmat(:,10) = inf;
solmat(:,11) = inf;
solmat(:,13) = true;
solmat(:,14) = true;
solmat(:,12) = false;
solmat(2:end,7) = 1;
end
end
function [tcx tcy] = inittc(t)
a = -t:t;
a = a(ones(2*t-1,1),:);
b = a';
a = a(:);
b = b(:);
pick = a~=0 | b~=0;
a = a(pick);
b = b(pick);
aa = abs(a);
bb = abs(b);
tcx = cell(t,1);
tcy = tcx;
for i = 1:t
pick = aa+bb <= i;
tcx{i} = a(pick);
tcy{i} = b(pick);
end
end
function [thrustRow, thrustCol] = solverlead(chart, aIndex, bIndex, maxThrottle)
[thrustRow, thrustCol] = nickelfelpeterv3(chart, aIndex, bIndex, maxThrottle);
score1 = runsolution(thrustRow, thrustCol, chart(:,:,2), chart(:,:,1), aIndex, bIndex);
if score1<16
return
end
[thrustRow0, thrustCol0, score0] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle);
if score0<score1,
thrustRow=thrustRow0;
thrustCol=thrustCol0;
end
end
function [thrustRow, thrustCol, score] = nickelfelpeterv2(chart, aIndex, bIndex, maxThrottle)
%REP
%by Gwendolyn Fischer
%r4537 t44
ONEHUNDRED = 100;
ONE = 1;
y_winds = chart(:,:,1);
x_winds = chart(:,:,2);
[ny,nx] = size(x_winds);
ay = rem(aIndex-ONE, ny) + ONE;
ax = (aIndex - ay)/ny + ONE;
by = rem(bIndex-ONE, ny) + ONE;
bx = (bIndex - by)/ny + ONE;
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];
INF = 100000;
SDBzoom = [INF 1/2];
dist_to_B_w = dist_to_B*1e-4;
dist_to_A_w = dist_to_A*1e-4;
y_dir = 2*(by > ay) - ONE;
x_dir = 2*(bx > ax) - ONE;
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);
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).^.77/4-3;
while ( min_fuel <= cost_to_B && ~all(SDBchecked) && 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) + ONE;
i_x = (i_p_full - i_y)/ny + ONE;
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_reacheable = (i_thrust <= maxThrottle) & (abs(i_new_vy+y_winds(i_p_full))+abs(i_new_vx+x_winds(i_p_full))<slowDown);
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) = ONEHUNDRED; %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) = ONEHUNDRED; %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 )
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) + ONE;
i_x = (i_p - i_y)/ny + ONE;
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) = ONEHUNDRED; %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))
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_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))
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 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))+1;
end
|
