robocup_graph.m

function robocup_graph()
    % Turn off warnings due to lineintersect
    % producing warnings about possible float errors for
    % values near zero
    warning('off','all');
    l = 10;
    w = 5;
    num_l = 100;
    num_w = 50;
    num_graphs = 4;

    rng(5)
    bots = rand(6,2) .* repmat([l, w], 6, 1) - repmat([0, w/2], 6, 1);
    pts = zeros(num_graphs, num_l + 1, num_w + 1);
    start_time = cputime;
    
    for y_ind = 1:num_l + 1
        for x_ind = 1:num_w + 1
            y = l/num_l * (y_ind - 1);
            x = w/num_w * (x_ind - 1) - w / 2;

            pts(1,y_ind, x_ind) = field_pos(x, y, w, l, 0.2, 1, 1);
            pts(2,y_ind, x_ind) = 1-space(x, y, w, l, bots);
            pts(3,y_ind, x_ind) = block_percent(x, y, bots);
            pts(4,y_ind, x_ind) = win_eval(x, y, w, l, bots);
        end
    end
    
    end_time = cputime - start_time
    
    graph_names = {'Field Position', 'Space', 'Block Chance', 'Win Eval'};
    for i = 1:num_graphs 
        subplot(1,num_graphs,i);
        g = reshape(pts(i,:,:), [num_l+1, num_w+1]);
        surf(g, 'EdgeColor', 'None', 'facecolor', 'interp')
        axis equal
        axis off
        title(graph_names(i));
        view(2)
    end
end

function score = field_pos(x, y, w, l, c, d, a)
    centerValue = 1 - abs(x / w / 2);
    distValue = abs(y / l);
    angleValue = 1 - abs(atan2(x, l - y) / (pi / 2));
    score = centerValue*c + distValue*d + angleValue*a;
    score = score / (c + d + a);
end

function score = space(x, y, w, l, bots)
    max_dist = sqrt(w*w/4 + l*l);
    
    total = 0;
    sens = 8;
    
    for i = 1:6
        nx = bots(i, 2) - x;
        ny = bots(i, 1) - y;
        u = sqrt(nx*nx + ny*ny) * sens / max_dist;
        
        total = total + max((35/32)*power((1-power(u,2)), 3), 0);
    end
    
    score = min(total, 1);
end

function score = block_percent(x, y, bots)
    % Kick x/y
    kx = 0;
    ky = 0;
    
    % Kick->Goal Vector
    kdx = kx - x;
    kdy = ky - y;
    % Kick->Goal Angle
    ka = atan2(kdy, kdx);
    
    % Bot Positions in Polar Form
    b = zeros(6,2);
    
    for i = 1:6
        bx = bots(i,2) - x;
        by = bots(i,1) - y;
        
        dist = sqrt(bx*bx + by*by);
        angle = atan2(by,bx) - ka;
        % Force between -Pi and Pi
        angle = atan2(sin(angle), cos(angle));
        
        b(i,2) = dist;
        b(i,1) = angle;
        
        % Remove any that are behind the robot
        if (abs(angle) > pi/2)
            % Use -10 as Null Value
            b(i,1) = -10;
            b(i,2) = -10;
        end
    end
    
    % If no robots are in front, just return 1
    if (sum(b) == [-60, -60])
        score = 1;
        return
    end
    
    % Half Angle of Standard Deviation
    hkick = .1*pi;
    % Number of rays
    num_est = 32;
    % Robot position kernal sensitivity
    sens = 2;
    % Distance sensitivity
    dsens = -1/7;
    % Lowest max value of ray after scaling
    min_offset = .1;
    
    total = 0;
    max_total = 0;
    line_offset = -1 * hkick;
    inc = 2* hkick / num_est;
    
    % Quick and dirty to deal with float math
    while (line_offset <= hkick * 1.01)
        % List of scores
        % Again, -10 is used as an invalid score
        s = ones(6,1)*-10;
        
        for i = 1:6
            if (b(i,1) ~= -10 && b(i,2) ~= -10)
                % Get angle off of ray
                dangle = b(i,1) - line_offset;

                % Calculate straight line distance
                u = sin(dangle) * b(i,2) * sens;
                % Add in distance sensitivity
                u = u * exp(b(i,2) * dsens);

                % Limit to bounds of kernal function
                u = min(1,u);
                u = max(-1,u);
                
                % Inverted Triweight Kernal Distribution
                s(i) = 1 - max(power((1-power(u,2)),3), 0);
            end
        end
        
        % Calculate ray scale value
        line_offset_scale = 1 - (abs(line_offset) / ((1+min_offset) * hkick));
        
        % Get min score
        m = 0;
        if length(min(s(s ~= -10))) ~= 0
            m = min(s(s ~= -10));
        end
        
        total = total + m * line_offset_scale;
        max_total = max_total + line_offset_scale;
        line_offset = line_offset + inc;
    end
    
    score = total / max_total;
end

function score = win_eval(x, y, w, l, bots)
    kx = 0;
    ky = 0;
    ka = atan2(ky-y,kx-x);
    kd = sqrt(power(x-kx,2) + power(y-ky,2));
    hkick = .1*pi;
    num_est = 8;
    
    total = 2*hkick;
    line_offset = -1 * hkick;
    inc = 2* hkick / num_est;
    
    bot_rad = .09;
    ball_rad = .0225;
    r = bot_rad + ball_rad;
    
    while (line_offset <= hkick * 1.01)
        % Get normal of kick->goal
        dx = x-kx;
        dy = y-ky;
        
        norm1 = [-dy, dx];
        norm2 = [dy, -dx];
        
        % Apply normal to bot and check intersection
        for i = 1:6

            bx = bots(i,2) - x;
            by = bots(i,1) - y;
            angle = atan2(by,bx) - ka;
            angle = atan2(sin(angle), cos(angle));
            
            if abs(angle) > pi/2
                continue
            end
            
            l1x = bots(i,2) + norm1(1)*r;
            l1y = bots(i,1) + norm1(2)*r;
            l2x = bots(i,2) + norm2(1)*r;
            l2y = bots(i,1) + norm2(2)*r;
            
            [ix, iy] = lineintersect([l1x, l1y, l2x, l2y], [x, y, kx + cos(ka + line_offset), ky + sin(ka + line_offset)]);
        
            % Remove inc from total if intersection
            mag = sqrt(power(ix-bots(i,2),2) + power(iy-bots(i,1),2));
            if (mag < r)
                total = total - inc;
                break;
            end
        end
        
        line_offset = line_offset + inc;
    end
    
    % apply score and total / 2*hkick
    pc = .7;
    pd = 1-pc;
    
    max_d = sqrt(l*l + w*w/4);
    
    score = total / (2*hkick) * + (1-kd/max_d)*pd;
end

function [x,y]=lineintersect(l1,l2)
    %This function finds where two lines (2D) intersect
    %Each line must be defined in a vector by two points
    %[P1X P1Y P2X P2Y], you must provide two has arguments
    %Example:
    %l1=[93 388 120 354];
    %l2=[102 355 124 377];
    %[x,y]=lineintersect(l1,l2);
    %You can draw the lines to confirm if the solution is correct
    %figure
    %clf
    %hold on
    % line([l1(1) l1(3)],[l1(2) l1(4)])
    % line([l2(1) l2(3)],[l2(2) l2(4)])
    % plot(x,y,'ro') %this will mark the intersection point
    %
    %There is also included in another m file the Testlineintersect
    %That m file can be used to input the two lines interactively
    %
    %Made by Paulo Silva
    %22-02-2011

    %default values for x and y, in case of error these are the outputs
    x=nan;
    y=nan;

    %test if the user provided the correct number of arguments
    if nargin~=2
    disp('You need to provide two arguments')
    return
    end

    %get the information about each arguments
    l1info=whos('l1');
    l2info=whos('l2');

    %test if the arguments are numbers
    if (~((strcmp(l1info.class,'double') & strcmp(l2info.class,'double'))))
    disp('You need to provide two vectors')
    return
    end

    %test if the arguments have the correct size
    if (~all((size(l1)==[1 4]) & (size(l2)==[1 4])))
        disp('You need to provide vectors with one line and four columns')
        return
    end

    
    ml1=(l1(4)-l1(2))/(l1(3)-l1(1));
    ml2=(l2(4)-l2(2))/(l2(3)-l2(1));
    bl1=l1(2)-ml1*l1(1);
    bl2=l2(2)-ml2*l2(1);
    b=[bl1 bl2]';
    a=[1 -ml1; 1 -ml2];
    Pint=a\b;

    %when the lines are paralel there's x or y will be Inf
    if (any(Pint==Inf))
        disp('No solution found, probably the lines are paralel')
        return
    end

    %put the solution inside x and y
    x=Pint(2);y=Pint(1);
end