function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
[aRow, aCol] = ind2sub(size(chart(:,:,1)), aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)), bIndex);
aCoord = [aRow aCol];
bCoord = [bRow bCol];
thrustRow = zeros(1, 1);
thrustCol = zeros(1, 1);
vel = [0 0];
turn = 1;
pos = aCoord;
des = bCoord;
while( sum(abs(des - pos)) > 0 )
vel = vel + [chart(pos(1), pos(2), 1) chart(pos(1), pos(2), 2)];
motor = [0 0];
for i = 1:2
motor(i) = ( (pos(i) > des(i)) && ((pos(i) + vel(i) < des(i)) || (pos(i) + vel(i) >= pos(i))) ) * (-vel(i) - 1) + ...
( (pos(i) < des(i)) && ((pos(i) + vel(i) > des(i)) || (pos(i) + vel(i) <= pos(i))) ) * (-vel(i) + 1) + ...
( pos(i) == des(i) ) * (-vel(i) );
end
thrustRow(turn) = motor(1);
thrustCol(turn) = motor(2);
vel = vel + motor;
pos = pos + vel;
turn = turn + 1;
end
pos = bCoord;
des = aCoord;
while( sum(abs(des - pos)) > 0 )
vel = vel + [chart(pos(1), pos(2), 1) chart(pos(1), pos(2), 2)];
motor = [0 0];
for i = 1:2
motor(i) = ( (pos(i) > des(i)) && ((pos(i) + vel(i) < des(i)) || (pos(i) + vel(i) >= pos(i))) ) * (-vel(i) - 1) + ...
( (pos(i) < des(i)) && ((pos(i) + vel(i) > des(i)) || (pos(i) + vel(i) <= pos(i))) ) * (-vel(i) + 1) + ...
( pos(i) == des(i) ) * (-vel(i) );
end
thrustRow(turn) = motor(1);
thrustCol(turn) = motor(2);
vel = vel + motor;
pos = pos + vel;
turn = turn + 1;
end
|