function [thrustRow, thrustCol,loc,v] = solver(chart, aIndex, bIndex, maxThrottle)
% First try
rwind = chart(:,:,1);
cwind = chart(:,:,2);
[nr,nc] = size(rwind);
[ar,ac] = ind2sub([nr,nc],aIndex);
[br,bc] = ind2sub([nr,nc],bIndex);
thrustRow = [];
thrustCol = [];
loc = [ar,ac];
v = [0,0];
iter = 1;
cur_r = ar;
cur_c = ac;
cur_pos = [ar,ac];
cur_vr = 0; %rwind(aIndex);
cur_vc = 0; %cwind(aIndex);
curV = [cur_vr,cur_vc];
cur_goal = [br,bc];
% Target cruising speed
target_speed = maxThrottle*.5;
keepgoing = true;
reached_B = false;
while keepgoing
% Find the direction to current goal
d = cur_goal-cur_pos;
% Update current velocity with local winds
cur_vr = cur_vr+rwind(cur_r,cur_c);
cur_vc = cur_vc+cwind(cur_r,cur_c);
curV =[cur_vr,cur_vc];
% Are we within striking distance?
if sum(abs(d))<maxThrottle
getting_close = true;
else
getting_close = false;
end
% Decide on thrust inputs
% If current velocity is good enough, then don't do anything
% Find angle between current velocity and desired direction
thetad = acosd(dot(d,curV)/norm(d)/norm(curV));
theta_thresh = 30; % within 30 degrees is good enough
if sum(abs(curV))>maxThrottle
too_fast = true;
else
too_fast = false;
end
if thetad<theta_thresh && ~getting_close && ~too_fast
% don't do anything
cur_tr = 0;
cur_tc = 0;
else
% Adjust direction by thrusting more towards where we want to go
d_u = d/norm(d); % unit vector in the direction we want to go
if ~getting_close
% Multiply by desired target speed
perfect_v = round(d_u*target_speed);
elseif getting_close
% perfect speed gets us there exactly
perfect_v = d;
end
% Can we get there?
Vdiff = perfect_v-curV;
if sum(abs(Vdiff))<maxThrottle
% Just do it!
cur_tr = Vdiff(1);
cur_tc = Vdiff(2);
else
% Come as close as you can
Vdiff_scaled = round(Vdiff/sum(abs(Vdiff))*maxThrottle);
if sum(abs(Vdiff_scaled))>maxThrottle
% this happens when two .5s both get rounded up
% randomly reduce one of these by 1
[dum,ind] = max(rand(1,2));
Vdiff_scaled(ind) = sign(Vdiff_scaled(ind))*(abs(Vdiff_scaled(ind))-1);
end
cur_tr = Vdiff_scaled(1);
cur_tc = Vdiff_scaled(2);
end
end
% if abs(cur_tr)+abs(cur_tc)>maxThrottle
% keyboard;
% end
%it's almost certainly worth it to look around the local area we'll
%end up in to see if there's a 'best' landing spot. Future versions
% update position
next_r = cur_r + cur_vr + cur_tr; % local wind already included in cur_vr
next_c = cur_c + cur_vc + cur_tc; % local wind already included in cur_vc
%fprintf(' %4d %4d %4d %4d %4d %4d %4d %4d\n',rwind(cur_r,cur_c),cwind(cur_r,cur_c),cur_tr,cur_tc,cur_vr+cur_tr,cur_vc+cur_tc,next_r,next_c);
off_board_flags = [next_r<1, next_r>nr, next_c<1, next_c>nc];
off_board_dirs = {'top','bot','left','right'};
% Are we off the board?
if any(off_board_flags)
% Uh-oh, we're going off the board, can we save it?
if sum(off_board_flags)==1
% we're only off one border
available_thrust = maxThrottle-sum(abs([cur_tr,cur_tc]));
% how far off are we?
switch off_board_dirs{off_board_flags}
case 'top'
% need more positive row thrust to stay on
dist_off = 1-next_r;
cur_tr = cur_tr+dist_off;
% change column thrust if necessary
thrustCol_change = max(dist_off-available_thrust,0);
cur_tc = sign(cur_tc)*(abs(cur_tc)-thrustCol_change);
case 'bot'
% need more negative row thrust to stay on
dist_off = next_r-nr;
cur_tr = cur_tr-dist_off;
thrustCol_change = max(dist_off-available_thrust,0);
cur_tc = sign(cur_tc)*(abs(cur_tc)-thrustCol_change);
case 'left'
dist_off = 1-next_c;
cur_tc = cur_tc+dist_off;
thrustRow_change = max(dist_off-available_thrust,0);
cur_tr = sign(cur_tr)*(abs(cur_tr)-thrustRow_change);
case 'right'
dist_off = next_c-nc;
cur_tc = cur_tc-dist_off;
thrustRow_change = max(dist_off-available_thrust,0);
cur_tr = sign(cur_tr)*(abs(cur_tr)-thrustRow_change);
end
if abs(cur_tr)+abs(cur_tc)>maxThrottle
% This means we're NOT going to make it
%%% disp('GIVING UP NOW');
break
% keyboard;
end
else
% we've sailed off a corner
%%% disp('OFF A CORNER');
%keyboard
end
next_r = cur_r + cur_vr + cur_tr;
next_c = cur_c + cur_vc + cur_tc; % local wind already included in cur_vc
new_off_board_flags = [next_r<1, next_r>nr, next_c<1, next_c>nc];
%keyboard
if any(new_off_board_flags)
% we're pretty screwed, just give up, we're probably pretty
% close to the goal anyway :)
%%% disp('GIVING UP NOW');
break
end
end
loc(iter,:) = [cur_r,cur_c];
% update position
cur_r = next_r;
cur_c = next_c;
cur_pos = [cur_r,cur_c];
% Update velocity
cur_vr = cur_vr + cur_tr; %local wind already included
cur_vc = cur_vc + cur_tc;
curV = [cur_vr,cur_vc];
v(iter+1,:) = curV;
thrustRow(iter) = cur_tr;
thrustCol(iter) = cur_tc;
% are we to the current goal?
if isequal(cur_pos,cur_goal)
% are we back to point A
if isequal(cur_pos,[ar,ac])
%then we're done
keepgoing = false;
else %we must be at B
% change the current goal to point A
cur_goal = [ar,ac];
reached_B = true;
reached_B_iter = iter;
end
end
% Are we in a loop?
if reached_B
% only consider locations in iterations since then
dup_inds = find(loc(reached_B_iter:end-1,1)==cur_r & loc(reached_B_iter:end-1,2)==cur_c);
if length(dup_inds)>=2
% identify the cycle of locations
inds_to_consider = (dup_inds(1)+reached_B_iter-1):(size(loc,1)-1);
locs_to_consider = loc(inds_to_consider,:);
% find the one closest to point A
dist2_to_A = (locs_to_consider(:,1)-ar).^2+(locs_to_consider(:,2)-bc).^2;
[dum,min_ind] = min(dist2_to_A);
closest_loc_ind = inds_to_consider(min_ind);
% declare that as close as we'll ever get to A and quit moving
% truncate everything back to that point
%keyboard
iter = closest_loc_ind;
loc(iter+1:end,:) = [];
thrustRow(iter:end) = [];
thrustCol(iter:end) = [];
v(iter+2:end,:) = [];
break
end
else
% consider all locations up to now
dup_inds = find(loc(1:end-1,1)==cur_r & loc(1:end-1,2)==cur_c);
% If we've been here twice before, this is probably a loop
if length(dup_inds)>=2
% identify the cycle of locations
inds_to_consider = dup_inds(1):(size(loc,1)-1);
locs_to_consider = loc(inds_to_consider,:);
% find the one closest to point B
dist2_to_B = (locs_to_consider(:,1)-br).^2+(locs_to_consider(:,2)-bc).^2;
[dum,min_ind] = min(dist2_to_B);
closest_loc_ind = inds_to_consider(min_ind);
% declare that one as close as we'll ever get to B, and change
% goals
reached_B = true;
reached_B_iter = closest_loc_ind;
% truncate everything back to this point in time, adjust iter,
% and continue from there
%keyboard
iter = closest_loc_ind;
loc(iter+1:end,:) = [];
thrustRow(iter+1:end) = [];
thrustCol(iter+1:end) = [];
v(iter+2:end,:) = [];
cur_r = loc(end,1);
cur_c = loc(end,2);
cur_vr = v(end,1);
cur_vc = v(end,2);
end
end
iter = iter+1;
%disp(cur_pos)
end
%disp('-------------------------------------')
end
|
