function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% Initialize
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);
rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);
% Maximize iterations
max_iter = 8;
% Get empty table
table = get_empty_table(nR,nC,aRow,aCol,bRow,bCol,0,0);
% Direction A -> B
[thrustRow_to thrustCol_to mR mC vR vC] = get_trip(table,bRow,bCol);
% Get empty table
table = get_empty_table(nR,nC,mR,mC,aRow,aCol,vR,vC);
% Direction A <- B
[thrustRow_back thrustCol_back] = get_trip(table,aRow,aCol);
% Connect trips
thrustRow = [thrustRow_to thrustRow_back];
thrustCol = [thrustCol_to thrustCol_back];
[dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex);
scoreTomi = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
[my_thrustRow, my_thrustCol] = my_solver(chart, aIndex, bIndex, maxThrottle);
[dA, dB] = runsolution(my_thrustRow, my_thrustCol, chart, aIndex, bIndex);
scoreMe = dB + dA + sum(abs(my_thrustRow)) + sum(abs(my_thrustCol));
if scoreTomi > scoreMe
thrustRow = my_thrustRow;
thrustCol = my_thrustCol;
end
%% ------------------------------------------------------------[ get_trip ]
function [thrustRow thrustCol mR mC vR vC] = get_trip(table,bRow,bCol)
% Iterate positions
num_iter = 1;
while nnz(table.status) && num_iter <= max_iter
% Get next steps from minimum
next_steps = get_next_steps(table,maxThrottle);
% Clear status
table.status = false(nR,nC);
for s=1:size(next_steps,1)
% Update one position
table = update_table(table,nR,nC,rowWind,colWind,...
next_steps(s,:),bRow,bCol,maxThrottle);
end
% Increment iteration counter
num_iter = num_iter + 1;
end
% Get the path with the best score to B
[thrustRow thrustCol mR mC vR vC] = get_best_path(table,nR,nC);
end
end
%% =====================================================[ get_empty_table ]
function table = get_empty_table(nR,nC,aRow,aCol,bRow,bCol,vR,vC)
% Best score for the given position
table.score = Inf * ones(nR,nC);
% The thrustRow for the best score for the given position
table.thrustRow = cell(nR,nC);
% The thrustCol for the best score for the given position
table.thrustCol = cell(nR,nC);
% The row throttle for the best score for the given position
table.vR = zeros(nR,nC);
% The column throttle for the best score for the given position
table.vC = zeros(nR,nC);
% Change flag for the last update
table.status = false(nR,nC);
% The closest point for the best score for the given position
table.dB = ((aRow-bRow)^2 + (aCol-bCol)^2) * ones(nR,nC);
% Initialize position for A
table.score(aRow,aCol) = 0;
table.thrustRow{aRow,aCol} = [];
table.thrustCol{aRow,aCol} = [];
table.status(aRow,aCol) = true;
table.vR(aRow,aCol) = vR;
table.vC(aRow,aCol) = vC;
end
%% ======================================================[ get_next_steps ]
function next_steps = get_next_steps(table,maxThrottle)
list_max_limit = 20;
list_min_limit = 10;
% Get start point
[listR listC]=find(table.status);
list_len = length(listR);
if list_len > list_max_limit
% Reduce list
sel = randi(list_len,1,list_max_limit);
listR = listR(sel);
listC = listC(sel);
list_len=list_max_limit;
elseif list_len < list_min_limit
% Extend list
[listR_ext listC_ext]=find(table.score < Inf);
if length(listR_ext) > list_min_limit - list_len
sel = randi(length(listR_ext),1,list_min_limit - list_len);
else
sel = 1:length(listR_ext);
end
listR = [listR; listR_ext(sel)];
listC = [listC; listC_ext(sel)];
list_len=length(listR);
end
throttle_limit = 3;
if maxThrottle>throttle_limit/2
throttle_vec = floor(-maxThrottle:maxThrottle/throttle_limit*2:maxThrottle);
else
throttle_vec = -maxThrottle:maxThrottle;
end
% Initialize output
next_steps = zeros(list_len*maxThrottle^2,4);
n=1;
for listN = 1:list_len
mR = listR(listN);
mC = listC(listN);
% Iterate all throttle combination
for vR = throttle_vec
maxThrottle_rest = maxThrottle-abs(vR);
if maxThrottle_rest>throttle_limit/2
throttle_rest_vec = floor(-maxThrottle_rest:...
maxThrottle_rest/throttle_limit*2:maxThrottle_rest);
else
throttle_rest_vec = -maxThrottle_rest:maxThrottle_rest;
end
for vC = throttle_rest_vec
next_steps(n,1) = mR;
next_steps(n,2) = mC;
next_steps(n,3) = vR;
next_steps(n,4) = vC;
n = n + 1;
end
end
end
% Remove not needed elements
next_steps = next_steps(1:n-1,:);
end
%% ===================================================[ get_minimal_point ]
function [mR, mC] = get_minimal_point(map)
% Get the position for the minimal score point of the updated
[min_row, idx_row] = min(map);
[~, idx_col] = min(min_row);
mC = idx_col;
mR = idx_row(idx_col);
end
%% ========================================================[ update_table ]
function table = update_table(table,nR,nC,rowWind,colWind,next_step,bRow,bCol,maxThrottle)
% Step start point
vfR = next_step(3);
vfC = next_step(4);
fR = next_step(1);
fC = next_step(2);
% Step end point
viR = vfR + rowWind(fR,fC) + table.vR(fR,fC);
viC = vfC + colWind(fR,fC) + table.vC(fR,fC);
iR = fR + viR;
iC = fC + viC;
% Return if out of board
if iR>nR || iR<1 || iC>nC || iC<1
return
end
% Return if big throttle
throttle_limit = maxThrottle;
if abs(viR) + abs(viC) > throttle_limit
return
end
% Check if better than previous
score = table.score(fR,fC) + abs(vfR) + abs(vfC) + abs(viR) + abs(viC) ;
% Update if better than actual
if score < table.score(iR,iC);
table.status(iR,iC) = true;
table.score(iR,iC) = score;
table.vR(iR,iC) = viR;
table.vC(iR,iC) = viC;
table.thrustRow{iR,iC} = [ table.thrustRow{fR,fC} vfR];
table.thrustCol{iR,iC} = [ table.thrustCol{fR,fC} vfC];
dB = (iR-bRow)^2 + (iC-bCol)^2;
if dB < table.dB(fR,fC)
table.dB(iR,iC) = dB;
else
table.dB(iR,iC) = table.dB(fR,fC);
end
end
end
%% =======================================================[ get_best_path ]
function [thrustRow thrustCol mR mC vR vC] = get_best_path(table,nR,nC)
% Initialize score table
score_table = Inf * ones(nR,nC);
[listR listC]=find(table.score < Inf);
% Calculat final score for all points
for n=1:length(listR)
iR=listR(n);
iC=listC(n);
dB = table.dB(iR,iC);
score_table(iR,iC) = dB + ...
sum(abs(table.thrustRow{iR,iC})) + ...
sum(abs(table.thrustCol{iR,iC}));
end
[mR, mC] = get_minimal_point(score_table);
% Return the best parh
thrustRow = table.thrustRow{mR,mC};
thrustCol = table.thrustCol{mR,mC};
vR = table.vR(mR,mC);
vC = table.vC(mR,mC);
end
function [thrustRow, thrustCol] = my_solver(chart, aIndex, bIndex, maxThrottle)
vec_length = 4;
best_score = Inf;
for a = 1:1000
if ~mod(a,40)
vec_length = vec_length+1;
end
[current_thrustRow, current_thrustCol] = my_algorithm(maxThrottle, vec_length);
[dA, dB] = runsolution(current_thrustRow, current_thrustCol, chart, aIndex, bIndex);
score = dB + dA + sum(abs(current_thrustRow)) + sum(abs(current_thrustCol));
if score<best_score
best_score = score;
thrustRow = current_thrustRow;
thrustCol = current_thrustCol;
end
end
end
function [thrustRow, thrustCol] = my_algorithm(maxThrottle, vec_length)
thrustRow = randi([ceil(-maxThrottle/2) floor(maxThrottle/2)], vec_length,1);
thrustCol = randi([ceil(-maxThrottle/2) floor(maxThrottle/2)], vec_length,1);
end
function [dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);
[AR,AC] = ind2sub([nR,nC],aIndex);
[BR,BC] = ind2sub([nR,nC],bIndex);
% 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;
if iR>nR || iR<1 || iC>nC || iC<1
break % out of bounds
end
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
if ((fR-BR)^2 + (fC-BC)^2) < dB
dB = (fR-BR)^2 + (fC-BC)^2;
end
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
end
|
