GenerateDataUniformControlCollisionCheck

function GenerateDataUniformControlCollisionCheck
% generates data for Figure 8: CollisionCheck
% this data is stored as 'CollisionCheck.mat' and transformed into a figure
% by ParseGenerateDataUniformControlCollisionCheck.m
%
%CAPTION: Probability of a collision as a function of robot radius for different 
% numbers of robots, $n$.  The setup in Fig.~\ref{fig:SimSetup} is used. 
% The probability of collision increases nonlinearly with number of robots and  robot radius.

clc

targetRange = 1;
tx = 2;
ty = 0;
NUMsofROBOTs = [2,5,10,20];
dTHETAs = linspace(pi/4,3*pi/4,10);
NUMSofMOVES = 1:10;
RobRadii = [0.0001,0.001,0.002,0.003,0.005,0.0075,0.01,0.015,0.02,0.025,0.03,0.035,0.04,0.05,0.06,0.07, 0.08,0.09,0.1];

% for n = 2,5,10,20,  and robot radii
% (1/20 = 0.0500), 0.02, 0.01,0.004
probSucc = zeros(numel(RobRadii),numel(NUMsofROBOTs));

savefilename = 'CollisionCheck.mat';
for r = 1:numel(RobRadii)
    rad = RobRadii(r);
    for n = 1:numel(NUMsofROBOTs)
        numsuccess = 0;
        NUMROBOTS = NUMsofROBOTs(n);
        offset = 0;
        for dTHETAp = dTHETAs 
            dTHETA = dTHETAp;
            for mv = 1:numel(NUMSofMOVES)
                
                xp = zeros(NUMROBOTS,1);
                yp = linspace(-1,1,NUMROBOTS)';
                
                delta = 1/2;
                epsilon = linspace(1-delta, 1+delta, NUMROBOTS);
                
                % Goal position: +pi turns them inside, +pi/2 makes them tangent, +0 makes
                % them face out.
                
                NUMMOVES = NUMSofMOVES(mv) + NUMROBOTS*8 + offset;
                tg = NUMMOVES*dTHETA*epsilon;
                xg = tx+ targetRange*cos(tg+pi)';
                yg = ty+ targetRange*sin(tg+pi)';
                %Create C matrix:
                angs = dTHETA*repmat(epsilon', 1,NUMMOVES).*repmat((1:NUMMOVES),NUMROBOTS,1);
                C = [cos(angs);sin(angs)];
                dq = [xg;yg]-[xp;yp];
                % X = A\B is the solution in the least squares sense to the
                % under- or overdetermined system of equations AX = B
                %C.u = dq
                u = C\dq;
                while sum(abs(u)) > 50
                    display(['Offset = ',num2str(offset),' for radii ', num2str(rad),' and numrobots ', num2str(NUMROBOTS),'dtheta=', num2str(dTHETA),'nummoves = ',num2str(NUMMOVES) ])
                    offset = offset+1;
                    dTHETA = dTHETA+randn;
                    NUMMOVES = NUMSofMOVES(mv) + NUMROBOTS*8 + offset;
                    tg = NUMMOVES*dTHETA*epsilon;
                    xg = tx+ targetRange*cos(tg+pi)';
                    yg = ty+ targetRange*sin(tg+pi)';
                    %Create C matrix:
                    angs = dTHETA*repmat(epsilon', 1,NUMMOVES).*repmat((1:NUMMOVES),NUMROBOTS,1);
                    C = [cos(angs);sin(angs)];
                    dq = [xg;yg]-[xp;yp];
                    % X = A\B is the solution in the least squares sense to the
                    % under- or overdetermined system of equations AX = B
                    %C.u = dq
                    u = C\dq;
                end
                
  
                Xs = xp;
                Ys = yp;
                allsafe = true;
                for i = 1:numel(u)
                    Xe = Xs + cos(i*dTHETA*epsilon')*u(i);
                    Ye = Ys + sin(i*dTHETA*epsilon')*u(i);
                    IsSafe = pathSafe(Xs,Ys,Xe,Ye,mindist(Xs,Ys),mindist(Xe,Ye),rad);
                    if IsSafe == false
                        allsafe = false;
                        break
                    end
                    %display(['segment ',num2str(i),' is ',num2str(IsSafe)])
                    Xs = Xe;
                    Ys = Ye;
                end
                if allsafe == true
                    numsuccess = numsuccess+1;
                end
                
            end
        end
        pSucc = numsuccess/(numel(dTHETAs)*numel(NUMSofMOVES));
        probSucc(r,n) = pSucc;
        display(['Prob success = ',num2str(pSucc),' for radii ', num2str(rad),' and numrobots ', num2str(NUMROBOTS)])
        save(savefilename, 'probSucc',  'RobRadii','NUMsofROBOTs','dTHETAs','NUMSofMOVES')
    end
end
end


function md = mindist(x,y)
%% Rob's solution
n= numel(x);
if n ==2
    md = sqrt((x(1)-x(2))^2 + (y(1)-y(2))^2);
else
    pairs = nchoosek(1:n,2);
    dist = sqrt((diff(x(pairs),1,2)).^2 + (diff(y(pairs),1,2)).^2);
    [B,IX] = sort(dist); %#ok<NASGU>
    %rad = 0.361/2;
    %pts = scatter(x,y);
    %set(pts,'SizeData',700);
    %hold on;
    %plot([x(pairs(IX(1),:))],[y(pairs(IX(1),:))],'-+r'); %#ok<NBRAK>
    md = B(1);
end
%http://blogs.mathworks.com/pick/2008/06/02/matlab-puzzler-finding-the-two-closest-points/
%http://www.mathworks.com/matlabcentral/newsreader/view_thread/173627
end

function IsSafe = pathSafe(Xs,Ys,Xe,Ye, minStart,minEnd,rad)
% Xs = robots 1 to NUMROBOTS x pos, then ypos
% robot size = 11cm diam = 0.361ft

len = (Xe(1)-Xs(1))^2+(Ye(1)-Ys(1))^2;

if ( minStart > 2*len+2*rad && minEnd > 2*len+2*rad )
    IsSafe = true;
elseif len < 0.0001/2 %0.0001 is min rad
    IsSafe = false;
    %display(['COLLISION!, min dist = ',num2str(min(minStart,minEnd))])
else
    Xm = (Xs+Xe)/2;
    Ym = (Ys+Ye)/2;
    minMid = mindist(Xm,Ym);
    IsSafe = pathSafe(Xs,Ys,Xm,Ym, minStart,minMid,rad) & pathSafe(Xm,Ym,Xe,Ye, minMid,minEnd,rad);
end
end