function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
xlim = size(chart(:,:,1),2);
ylim = size(chart(:,:,1),1);
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);
thrustRow = zeros(70,1);
thrustCol = zeros(70,1);
posRow = aRow;
posCol = aCol;
velRow = 0;
velCol = 0;
ct = 0;
while( 1 )
delta_Pos = abs(posRow - bRow) + abs(posCol - bCol);
if(delta_Pos == 0)
break
end
br = 0;
for sum_w = 0:maxThrottle
for wy = -sum_w:sum_w
evRow = velRow + chart(posRow,posCol,1) + wy;
esRow = posRow + evRow;
if( 0 >= esRow || esRow > ylim )
continue;
end
for wx = -(sum_w - abs(wy)):(sum_w - abs(wy))
evCol = velCol + chart(posRow,posCol,2) + wx;
esCol = posCol + evCol;
if( 0 >= esCol || esCol > xlim )
continue;
end
ev2Row = evRow + chart(esRow,esCol,1);
es2Row = esRow + ev2Row;
ev2Col = evCol + chart(esRow,esCol,2);
es2Col = esCol + ev2Col;
esdelta_Pos = abs(esRow - bRow) + abs(esCol - bCol);
es2delta_Pos = abs(es2Row - bRow) + abs(es2Col - bCol);
if esdelta_Pos < delta_Pos && es2delta_Pos < delta_Pos && 0 < es2Row && es2Row <= ylim && 0 < es2Col && es2Col <= xlim
ct = ct + 1;
br = 1;
thrustRow(ct) = wy;
thrustCol(ct) = wx;
posRow = esRow;
posCol = esCol;
velRow = evRow;
velCol = evCol;
if(esdelta_Pos == 0)
ct = ct+1;
posRow = es2Row;
posCol = es2Col;
velRow = ev2Row;
velCol = ev2Col;
end
break
end
end
if(br)
break
end
end
if(br)
break
end
end
if(~br)
break
end
end
while( 1 )
delta_Pos = abs(posRow - aRow) + abs(posCol - aCol);
if(delta_Pos == 0)
break
end
br = 0;
for sum_w = 0:maxThrottle
for wy = -sum_w:sum_w
evRow = velRow + chart(posRow,posCol,1) + wy;
esRow = posRow + evRow;
if( 0 >= esRow || esRow > ylim )
continue;
end
for wx = -(sum_w - abs(wy)):(sum_w - abs(wy))
evCol = velCol + chart(posRow,posCol,2) + wx;
esCol = posCol + evCol;
if( 0 >= esCol || esCol > xlim )
continue;
end
ev2Row = evRow + chart(esRow,esCol,1);
es2Row = esRow + ev2Row;
ev2Col = evCol + chart(esRow,esCol,2);
es2Col = esCol + ev2Col;
esdelta_Pos = abs(esRow - aRow) + abs(esCol - aCol);
es2delta_Pos = abs(es2Row - aRow) + abs(es2Col - aCol);
if esdelta_Pos < delta_Pos && es2delta_Pos < delta_Pos && 0 < es2Row && es2Row <= ylim && 0 < es2Col && es2Col <= xlim
br = 1;
ct = ct + 1;
thrustRow(ct) = wy;
thrustCol(ct) = wx;
posRow = esRow;
posCol = esCol;
velRow = evRow;
velCol = evCol;
if(esdelta_Pos == 0)
ct = ct+1;
posRow = es2Row;
posCol = es2Col;
velRow = ev2Row;
velCol = ev2Col;
end
break
end
end
if(br)
break
end
end
if(br)
break
end
end
if(~br)
break
end
end
vec_thrustRow = thrustRow;
vec_thrustCol = thrustCol;
score = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex);
[best_score,i] = min(score);
thrustRow = vec_thrustRow(1:i);
thrustCol = vec_thrustCol(1:i);
for ct=1:850
vec_thrustRowT = vec_thrustRow + fix(randn(70,1));
vec_thrustColT = vec_thrustCol + fix(randn(70,1));
score = runsolution(vec_thrustRowT, vec_thrustColT, chart, aIndex, bIndex);
[y,i] = min(score);
if(y < best_score)
thrustRow = vec_thrustRowT(1:i);
thrustCol = vec_thrustColT(1:i);
vec_thrustRow = vec_thrustRowT;
vec_thrustCol = vec_thrustColT;
best_score = y;
end
end
end
function score = 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;
score = zeros(1,numel(thrustRow));
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
if(i == 1)
score = dB;
else
score = score(1:i-1);
end
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
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
score(i) = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end
end
|
