Finish 2010-11-17 12:00:00 UTC

Gig Em Aggies 4.0

by Greg S

Status: Passed
Results: 8085 (cyc: 40, node: 2446)
CPU Time: 25.119
Score: 1649.54
Submitted at: 2010-11-11 18:41:30 UTC
Scored at: 2010-11-11 18:42:15 UTC

Current Rank: 2036th (Highest: 1st )

Comments
Greg S
11 Nov 2010
really good on the test suite...
Please login or create a profile.
Code
function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)

nruns = 15;
Pa_array = [2.0 1.0 2.5 1.5 0.5 2.0 1.0 2.0 1.0 2.0 1.0 0.5 2.0 1.0 0.5];
Pb_array = [3   3   3   3   3   1.5 1.5  6   6  3    3  3   5   5   5 ];
V_adj =    [0   0   0   0   0   0   0   0   0   -2  -2  -2  3   3   3  ];

Score = 99999*(ones(nruns,1));
V1 = [6 8];
V2 = [4 8];
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,1)),bIndex);
thrustRow = zeros(800,1);
thrustCol = zeros(800,1);

temp = -10:10;
penalty1 = [temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp; temp];
penalty2 = penalty1';
       

chart1 = chart(:,:,1);
chart2 = chart(:,:,2);
CHART(1,:,:) = chart1;
CHART(2,:,:) = chart2;

[xlimit, ylimit] = size(chart(:,:,1));
pos(1)=aRow; pos(2)=aCol; pos_old = pos; START = pos;
goal(1) = bRow; goal(2) = bCol;

%Move the goal if it's in a bad place_____________
xrange = max(goal(1)-5,1):min(goal(1)+5,xlimit);
yrange = max(goal(2)-5,1):min(goal(2)+5,ylimit);
Xrange = xrange-goal(1)+11;
Yrange = yrange-goal(2)+11;

newX = chart(xrange,yrange,1) + goal(1);
newY = chart(xrange,yrange,2) + goal(2);
OUCH  = (newX<1).*(1-newX) + (newX>xlimit).*(newX-xlimit) + (newY<1).*(1-newY) + (newY>ylimit).*(newY-ylimit) +...
             (penalty1(Xrange,Yrange).^2+penalty2(Xrange,Yrange).^2);
[temp, ix] = min(OUCH);
[temp, iy] = min(temp);
ix = ix(iy);
goal(1) = xrange(ix);
goal(2) = yrange(iy);
%  end move goal_________________________________         
GOAL = goal;

goal2 = pos;


for RUN = 1:nruns,

vel = zeros(1,2); 
turns = 0; Complete1 = 0; force = 0;
pos = START; goal = GOAL; GoalVect = goal - pos;
ErrorFlag=0; stuck = 0;

Pa = Pa_array(RUN);
Pb = Pb_array(RUN);
Vadj = V_adj(RUN);

for DIR = 1:2,

    while ~Complete1

        wind = CHART(:,pos(1),pos(2))';
        newvel1 = vel + wind;
        newpos1 = pos + newvel1;
        if sum(abs(GoalVect)) > 12
            GoalVel = min(V1(DIR)+Vadj,floor(maxThrottle*.75)) * GoalVect / sqrt(sum(GoalVect.^2));
            GoalVel(1) = GoalVel(1) + max(0,10-newpos1(1))/Pb;
            GoalVel(1) = GoalVel(1) - max(0,9+newpos1(1)-xlimit)/Pb;
            GoalVel(2) = GoalVel(2) + max(0,10-newpos1(2))/Pb;
            GoalVel(2) = GoalVel(2) - max(0,9+newpos1(2)-xlimit)/Pb;
        else
            GoalVel = min(V2(DIR)+Vadj,floor(maxThrottle*.4)) * GoalVect / sqrt(sum(GoalVect.^2));
            force = 1;
        end

        %check if approaching the goal
        if sum(abs(goal-newpos1)) < 6 && ( sum(abs(newvel1)) < 5 || DIR==2 ) && ~(sum(newpos1 < [1 1]) || sum(newpos1 > [xlimit ylimit]))
            vel_old = vel; pos_old2 = pos_old; pos_old=pos;
            vel = newvel1; pos=newpos1;
            turns = turns+1;
            xThr=0;  yThr=0;
            GoalVect = goal - pos;
            break
        end

        minT = zeros(1,2);
        if newpos1(1) < 1,
            minT(1) = 1-newpos1(1);
        elseif newpos1(1) > xlimit;
            minT(1) = xlimit - newpos1(1);
        end
        if newpos1(2) < 1,
            minT(2) = 1-newpos1(2);
        elseif newpos1(2) > ylimit;
            minT(2) = ylimit - newpos1(2);
        end

        if sum(abs(minT)) > maxThrottle
            %disp('Into the abyss...')
            ErrorFlag=1; break
        end

        newvel2 = vel + minT + wind;

        Verror = newvel2 - GoalVel;
        Vadd = -1*fix(Verror*.5);
        newvel2 = vel + minT + Vadd + wind;
        newpos2 = pos + newvel2;

        xrange = max(newpos2(1)-6,1):min(newpos2(1)+6,xlimit);
        yrange = max(newpos2(2)-6,1):min(newpos2(2)+6,ylimit);
        Xrange = xrange-newpos2(1)+11;
        Yrange = yrange-newpos2(2)+11;
        p2 = penalty2(Xrange,Yrange);
        p1 = penalty1(Xrange,Yrange);

        if force,
            E1 = abs(0.5*chart(xrange,yrange,1)+p2+Verror(1)+Vadd(1));
            E2 = abs(0.5*chart(xrange,yrange,2)+p1+Verror(2)+Vadd(2));
            temp = (abs(p2+Vadd(1)+minT(1))+abs(p1+Vadd(2)+minT(2)));
            windhelp = E1 + E2 + 0.2*temp + 999*(temp>maxThrottle);
        else
            E1 = abs(chart(xrange,yrange,1)+p2+Verror(1)+Vadd(1));
            E2 = abs(chart(xrange,yrange,2)+p1+Verror(2)+Vadd(2));
            temp = (abs(p2+Vadd(1)+minT(1))+abs(p1+Vadd(2)+minT(2)));
            windhelp = E1 + E2 + (~stuck)*Pa*temp + 999*(temp>maxThrottle);
        end
        [temp, ix] = min(windhelp);
        [temp, iy] = min(temp);
        ix = ix(iy);
        xThr = minT(1) + Vadd(1) + p2(ix,iy);
        yThr = minT(2) + Vadd(2) + p1(ix,iy);

        if sum(abs(xThr)+abs(yThr)) > maxThrottle,
            temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
            xThr = xThr - sign(xThr) * floor(temp/2);
            yThr = yThr - sign(yThr) * ceil(temp/2);
        end

        vel_old = vel; pos_old2 = pos_old; pos_old=pos;
        vel = vel + [xThr yThr] + wind;
        pos_temp = pos+vel;
        % Break any loops we might get caught in
        if sum(abs(vel)) == 0 || sum(pos_temp == pos_old2)==2 %|| sum(pos_temp == pos_old)==2           %%&& sum(abs(vel_old))==0
            stuck = stuck+1;
            if (stuck >2)
               THR = maxThrottle * GoalVect / sum(abs(GoalVect));
               xThr = fix(THR(1));
               yThr = fix(THR(2));
            end
            vel = vel_old + [xThr yThr] + wind;
        else
            stuck = 0;
        end
        pos=pos + vel;
        turns = turns+1;
        thrustRow(turns) = xThr;
        thrustCol(turns) = yThr;

        if sum(pos < [1 1]) || sum(pos > [xlimit ylimit]) || turns>200
            %disp('OOOOOPS!!')
            ErrorFlag=1; break
        end
        %disp([pos vel xThr yThr force])
        GoalVect = goal - pos;
        if sum(abs(GoalVect)) < 5,
            Complete1 = 1;
        end
    end

    if DIR==1 && ~ErrorFlag,
        xThr = xThr+GoalVect(1);
        yThr = yThr+GoalVect(2);
        if sum(abs(xThr)+abs(yThr)) > maxThrottle,
            temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
            xThr = xThr - sign(xThr) * floor(temp/2);
            yThr = yThr - sign(yThr) * ceil(temp/2);
            %This is trouble!
        end
        vel_temp =  vel_old + [xThr yThr] + wind;
        vel_next =  vel_temp + CHART(:,goal(1),goal(2))';
        if sum(abs(vel_temp))>7 || sum(abs(vel_next))>maxThrottle
            wind = CHART(:,pos(1),pos(2))';
            xThr = GoalVect(1) - wind(1) - vel(1);
            yThr = GoalVect(2) - wind(2) - vel(2);
            %disp([pos vel xThr yThr turns Complete1])
            if sum(pos < [1 1]) || sum(pos > [xlimit ylimit])
                %disp('OOOOOPS!!')
                ErrorFlag=1;
            end

            if sum(abs(xThr)+abs(yThr)) > maxThrottle,
                temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
                xThr = xThr - sign(xThr) * floor(temp/2);
                yThr = yThr - sign(yThr) * ceil(temp/2);
                %This is trouble!
            end
            turns = turns +1;
            vel = vel + [xThr yThr] + wind;
            pos_old2 = pos_old; pos_old = pos;
            pos = pos + vel;
            thrustRow(turns) = xThr;
            thrustCol(turns) = yThr;
        else
            vel = vel_temp;
            pos = pos_old + vel;
            thrustRow(turns) = xThr;
            thrustCol(turns) = yThr;
        end
        if sum(pos < [1 1]) || sum(pos > [xlimit ylimit])
            %disp('OOOOOPS!!')
            ErrorFlag=1;
        end
        pos_old = [-1 -1]; pos_old2=[-1 -1];  
        %disp([pos vel xThr yThr turns Complete1 goal])
    end

    if ErrorFlag
        break
    end
    % Head Back! %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %disp(['Head Back!'])
    goal = goal2; GoalVect = goal -pos;
    Complete1 = 0; force = 0;

end


if ~ErrorFlag
    xThr = xThr+GoalVect(1);
    yThr = yThr+GoalVect(2);
    if sum(abs(xThr)+abs(yThr)) > maxThrottle,
        temp=sum(abs(xThr)+abs(yThr))-maxThrottle;
        xThr = xThr - sign(xThr) * floor(temp/2);
        yThr = yThr - sign(yThr) * ceil(temp/2);
    end
    thrustRow(turns) = xThr;
    thrustCol(turns) = yThr;

    vel =  vel_old + [xThr yThr] + wind;
    pos =  pos_old + vel;
    %disp([pos vel xThr yThr goal])

    thrustRow = thrustRow(1:turns);
    thrustCol = thrustCol(1:turns);
end
[tRow, tCol] = validatesolution(thrustRow, thrustCol, maxThrottle);

if sum(tRow - thrustRow) || sum(tCol - thrustCol),
    Score(RUN) = 99999;
else
    [dA,dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, 0);
    Score(RUN) = scoresolution(dA,dB,thrustRow,thrustCol);
    TROW{RUN} = thrustRow;
    TCOL{RUN} = thrustCol;
end

    
end   %end of RUNs loop

[junk, bestrun] = min(Score);
if junk < 99000
   thrustRow = TROW{bestrun};
   thrustCol = TCOL{bestrun};
end
end

function [thrustRow, thrustCol] = validatesolution(thrustRow, thrustCol, maxThrottle)
% VALIDATESOLUTION Validates the solution vectors given by the solver

% 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

function [dA, dB] = runsolution(thrustRow, thrustCol, chart, aIndex, bIndex, flagVisualize)
% 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;

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
        break % out of bounds
    end
    if flagVisualize
    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
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
end

function score = scoresolution(dA,dB,thrustRow,thrustCol)
% SCORESOLUTION Calculates the score for the solution.
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end