function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
thrustRow = [];
thrustCol = [];
[a.x a.y] = ind2sub(size(chart(:,:,1)), aIndex);
[b.x b.y] = ind2sub(size(chart(:,:,2)), bIndex);
chrtSize = size(chart(:,:,1));
MAX_ITER = 500;
pos = [a.x a.y];
goal = [b.x b.y];
t = [0 0]; % thrust velocity at current position
isDone = false;
iter = 0;
searchFwdDone = false;
while ~isDone
% wind velocity at current position
w = [chart(pos(1),pos(2),1) chart(pos(1),pos(2),2)];
v = w + t; % current velocity
[moves npos] = getAllPossibleMotorMoves(maxThrottle, pos, chrtSize, v);
if isempty(moves), return, end
num = size(moves,1);
switch getChoiceProbabilisticly([0.9 0.07 0.025 0.005])
case 1
% distance from goal + cost of motor moves
cost = sum(bsxfun(@minus, goal, npos).^2,2) + sum(abs(moves),2);
[cost,ord] = sort(cost); % sort smaller to larger
case 2
% distance from goal + cost of motor moves
cost = sum(bsxfun(@minus, goal, npos).^2,2) + sum(abs(moves),2);
[cost,ord] = sort(cost); % sort smaller to larger
N = min(randi([2 5]),num); % top-N
ord(1:N) = ord( randperm(N) ); % shuffle top-N
case 3
% find the move with the least cost (only motor cost)
cost = sum(abs(moves),2);
[cost,ord] = sort(cost);
case 4
% random order
ord = randperm(num);
end
moves = moves(ord,:);
npos = npos(ord,:);
% take the first of possible moves (sorted according to above)
pos = npos(1,:);
t = moves(1,:);
thrustRow(end+1) = t(1);
thrustCol(end+1) = t(2);
iter = iter + 1;
% ending conditions: max iterations
if iter>=MAX_ITER
isDone = true;
if ~searchFwdDone, iter = 0; end
end
% ending conditions: goal reached
if all(pos==goal)
isDone = true;
end
% ending conditions: no change in position
if iter>=2 && ...
( thrustRow(end-1)==thrustRow(end) && ...
thrustCol(end-1)==thrustCol(end) );
isDone = true;
end
% forward-backward
if isDone && ~searchFwdDone
isDone = false;
searchFwdDone = true;
goal = [a.x a.y];
end
end
end
function [moves npos] = getAllPossibleMotorMoves(maxThrottle, pos, chrtSize, v)
% generate all combinations of motor velocities
[X Y] = meshgrid(-maxThrottle:maxThrottle,-maxThrottle:maxThrottle);
moves = [X(:) Y(:)];
%clear X Y
% corresponding ending locations
%npos = bsxfun(@plus, moves, pos+v);
npos = moves + repmat(pos+v, [size(moves,1) 1]);
% keep only valid moves in terms of throttle
idx = ( sum(abs(moves),2) <= maxThrottle );
moves = moves(idx,:);
npos = npos(idx,:);
% keep only valid moves in terms of grid position
idx = ( 1<=npos(:,1) & npos(:,1)<=chrtSize(1) & ...
1<=npos(:,2) & npos(:,2)<=chrtSize(2) );
moves = moves(idx,:);
npos = npos(idx,:);
end
function choice = getChoiceProbabilisticly(probs, N)
if nargin < 2, N=1; end
%choice = randsample(numel(probs), N, true, probs);
[~,choice] = histc(rand(1,N), cumsum([0;probs(:)./sum(probs)]));
choice = choice(:);
end
|