function [thrustRow, thrustCol,loc,min_score,scores] = improved(chart, aIndex, bIndex, maxThrottle)
% if nargin<5
% extra_input = [];
% end
% allows structure input
if nargin<3
ts = chart;
if nargin < 2
index = 1;
else
index = aIndex;
end
ts = ts(index);
chart = ts.chart;
aIndex = ts.aIndex;
bIndex = ts.bIndex;
maxThrottle = ts.maxThrottle;
end
% Improved
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];
% Set up default parameter values
target_speed = maxThrottle*.5;
theta_thresh = 30; %degrees
too_fast_thresh = maxThrottle;
neighborhood_factor = 0.8;
failCount =0;
failCountMax = 10;
nRollbackSteps = 1;
slowFactor = .5;
loop_retry_count = 0;
max_loop_retry = 4;
keepgoing = true;
reached_B = false;
fail_msg = [];
score = [];
while keepgoing
[fail_msg,cur_pos,curV,curT] = next_step(cur_pos,curV,cur_goal);
% Update Results
loc(iter,:) = cur_pos;
v(iter+1,:) = curV;
thrustRow(iter) = curT(1);
thrustCol(iter) = curT(2);
if ~isempty(fail_msg)
failCount = failCount+1;
if failCount>failCountMax
keepgoing = false;
rollItBack(1); % to get back to before the failure
continue
end
switch fail_msg{1}
case {'off side','off corner','off differently'}
%keyboard
rollItBack(nRollbackSteps+failCount);
%keyboard
target_speed = slowFactor*target_speed;
too_fast_thresh = too_fast_thresh*slowFactor;
% Start from here on the next iteration
continue
otherwise
%%
%keyboard
%%
end
end
% 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?
[we_are_looping_tf,dup_inds] = loop_check;
if we_are_looping_tf
store_current_results;
% Trim it back to the best place
inds_to_consider = dup_inds(1):size(loc,1)-1;
locs_to_consider = loc(inds_to_consider,:);
dist2_to_goal = (locs_to_consider(:,1)-cur_goal(1)).^2+(locs_to_consider(:,2)-cur_goal(2)).^2;
[dum,min_ind] = min(dist2_to_goal);
closest_loc_ind = inds_to_consider(min_ind);
% declare that one as close as we'll ever get to the goal, and change
% goals or stop
if reached_B
keepgoing = false;
else
keepgoing = true;
reached_B = true;
reached_B_iter = closest_loc_ind;
cur_goal = [br,bc];
end
% 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_pos = loc(end,:);
cur_vr = v(end,1);
cur_vc = v(end,2);
curV = v(end,:);
store_current_results;
if we_are_looping_tf && loop_retry_count< max_loop_retry
loop_retry_count = loop_retry_count+1;
% try rolling things back, taking one different move, and
% trying again
rollItBack(nRollbackSteps+loop_retry_count);
% Get ordinary next step
%debug_cur_pos2 = cur_pos;
[fail_msg,cur_pos,curV,curT] = next_step(cur_pos,curV,cur_goal);
% debug_curT = curT;
% debug_curV = curV;
% debug_cur_pos = cur_pos;
% if any(cur_pos<[1,1]) || any(cur_pos>[nr,nc])
% keyboard
% end
% Adjust!
curT_old = curT;
if rand > rand
curT_change(1) = -1;
curT_change(2) = -round(rand);
else
curT_change(2) = -1;
curT_change(1) = -round(rand);
end
curT = sign(curT).*(abs(curT)+curT_change);
delta_curT = curT-curT_old;
% fix others as well
curV = curV+delta_curT;
cur_pos = cur_pos+delta_curT;
if any(cur_pos<[1,1]) || any(cur_pos>[nr,nc])
% off the board, have to drop the changes
cur_pos = cur_pos-delta_curT;
curV = curV-delta_curT;
curT = curT_old; %sign(curT).*(abs(curT)-curT_change);
end
% if any(cur_pos<[1,1]) || any(cur_pos>[nr,nc])
% keyboard
% end
% Update Results
loc(iter,:) = cur_pos;
v(iter+1,:) = curV;
thrustRow(iter) = curT(1);
thrustCol(iter) = curT(2);
% Don't kill it this time!
keepgoing = true;
end
end
% Increment counter
iter = iter+1;
end
%loc(iter,:) = [cur_r,cur_c];
%disp('-------------------------------------')
store_current_results
%score
if min(score)>300
% we have work to do!!
end
% Pick the result with the best score
[dum,ind] = min(score);
thrustRow = sTR{ind};
thrustCol = sTC{ind};
loc = sL{ind};
loc = [ar,ac;loc]; %add back in starting location
v = sV{ind};
min_score = min(score);
scores = score;
%keyboard
%% %%%%%%%%%%% NESTED FUNCTIONS %%%%%%%%%%%%%%
%% store_results
function store_current_results
l = length(score)+1;
score(l) = scoreme(thrustRow,thrustCol,loc,ar,ac,br,bc,maxThrottle);
sTR{l} = thrustRow;
sTC{l} = thrustCol;
sL{l} = loc;
sV{l} = v;
end
%% next_step
function [fail_msg,cur_pos,curV,curT] = next_step(cur_pos,curV,cur_goal)
% uses, but doesn't change, rwind,cwind,
fail_msg = [];
cur_r = cur_pos(1);
cur_c = cur_pos(2);
cur_vr = curV(1);
cur_vc = curV(2);
% 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?
%neighborhood_factor = 0.8; %getting this number wrong has HUGE ill effects. The right region appears to be ~0.5-1
% I don't really understand why this is yet. My first hypothesis
% is now that it is because I regulate speed when we're not getting
% close, but don't once we're close enough.
if sum(abs(d))< maxThrottle*neighborhood_factor
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))>too_fast_thresh
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]));
old_cur_tc = cur_tc;
old_cur_tr = cur_tr;
% 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
fail_msg = {'off side',off_board_dirs{off_board_flags}};
cur_tc = old_cur_tc;
cur_tr = old_cur_tr;
%disp('GIVING UP NOW');
%keyboard
%break
% keyboard;
end
else
% we've sailed off a corner
%disp('OFF A CORNER');
fail_msg = {'off 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 DIFFERENTLY NOW');
fail_msg = {'off differently'};
%keyboard
%break
end
end
% Set up outputs
cur_pos = [next_r,next_c];
curV = [cur_vr+cur_tr,cur_vc+cur_tc];
curT = [cur_tr,cur_tc];
end %eof for next_step
%% rollItBack
function rollItBack(nRollbackSteps)
store_current_results;
if iter==1
% can't rollback from the first step
return
end
iter = iter-nRollbackSteps; % this is the iteration I'm about to rerun
iter = max(iter,2); % can't rollback to before 1, so iter must be at least two
cur_pos = loc(iter-1,:);
loc(iter:end,:) = [];
curV = v(iter,:);
v(iter+1:end,:) = [];
thrustRow(iter:end) = [];
thrustCol(iter:end) = [];
if reached_B && iter-1<reached_B_iter
reached_B = false;
reached_B_iter = [];
cur_goal = [br,bc];
end
end % eof for rollItBack
%% loop_check
function [tf,dup_inds] = loop_check
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);
dup_inds = dup_inds+reached_B_iter-1;
else
% consider all locations up to now
dup_inds = find(loc(1:end-1,1)==cur_r & loc(1:end-1,2)==cur_c);
end
if length(dup_inds)>=2
tf = true;
else
tf = false;
return
end
end % eof for loop_check
end % eof for solver.m
%% %%%%%%%%%%%%%% SUBFUNCTIONS %%%%%%%%%%%%%%%%%%
%% scoreme
function score = scoreme(thrustRow,thrustCol,loc,ar,ac,br,bc,maxThrottle)
[thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle);
dA = sum((loc(end,:)-[ar,ac]).^2);
dBs = (loc(:,1)-br).^2+(loc(:,2)-bc).^2;
dB = min(dBs);
score = dA+dB+sum(abs(thrustRow))+sum(abs(thrustCol));
end
%% validate?
function [thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle)
% VALIDATESOLUTION Validates the solution vectors given by the solver
% just copied from runcontest
% Ensure column vectors and reals
thrustRow = real(thrustRow(:));
thrustCol = real(thrustCol(:));
% Check maximum number of moves
mnmoves = 1000;
if numel(thrustRow) > mnmoves
thrustRow = thrustRow(1:mnmoves);
end
if numel(thrustCol) > mnmoves
thrustCol = thrustCol(1:mnmoves);
end
% Ensure integers
if any(rem(thrustRow,1))
thrustRow = round(thrustRow);
end
if any(rem(thrustCol,1))
thrustCol = round(thrustCol);
end
% Ensure same length
ntR = numel(thrustRow);
ntC = numel(thrustCol);
if ntR~=ntC
n = min(ntR,ntC);
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
end
% Check maximum throttle
h = (abs(thrustRow) + abs(thrustCol)) > maxThrottle;
if any(h)
% Motor is blown for every turn the throttle exceeds the motor capacity!
thrustRow(h) = 0;
thrustCol(h) = 0;
end
end
|
