% GeneratoreTraiettoriaCorridoio.m

%genera traiettorie rettilinee da una parte all'altra di un corridoio
%orizzontale (non cambia niente se sono spezzate per il nostro scopo).
%versione "senza ripensamenti" cio� il corridoio � percorso per intero.

%passo uomo 4km/h, telecamere 25frame/s
d_uomo = 4 / 3.6 / 25;  %=0.0444m/frame

close
%clc
%clear

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%    PARAMETRI    %%%%%     %(lunghezze in metri)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%nota: modificare solo quelli con il segno di = tra spaziature
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
L1 = 50;                   %asse x, misura a
L2 = 25;
C1 = 7;                    %corridoio
C2 = 17;
campioni = 3500;           %numero di campioni di traiettoria
proporzione = 0.6;        %non mettere 0 !!
%maxlengthspezzate=min(L1,L2)*proporzione; %lunghezza massima dei segmenti di cammino
%maxlengthspezzate = 10;     %manuale
d = 0.05;                   %lunghezza dei passi tra ogni frame
d = 1;
prossimita=d+0.01;          %inseguo il punto random fino a tale distanza
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

a0 = 0;
b0 = 10;
%genero il primo punto
randx  = L1;
randy  = C1 + (C2-C1)*rand;
%calcolo il coefficiente angolare della retta passante per i due punti
m = (randy-b0)/(randx-a0);
%trovo la coordinata x corrispondente a uno spostamento di d lungo tale retta 
a1 = a0 + d*cos(atan(m));
%trovo la corrispondente coordinata y
b1 = m * a1 + b0;
%memorizzo i punti proposti
Randx = [randx];
Randy = [randy];
%memorizzo i punti scelti
A = zeros(1,campioni);
B = zeros(1,campioni);
A(1) = a0;
A(2) = a1;
B(1) = b0;
B(2) = b1;

%ciclo

a=a1;
b=b1;
segnox=1;
segnoy=1;
verso = -1;
entra=1;
iter1=2;

while B(length(B))==0
    anew = a + segnox*d*abs(cos(atan(m)));
    bnew = b + segnoy*d*abs(sin(atan(m)));
    %s = sqrt((anew-a)^2+(bnew-b)^2);
    dist = sqrt((anew-randx)^2+(bnew-randy)^2);
    if dist>prossimita && anew<L1 && bnew<C2 && anew>0 && bnew>C1     %rimango nel rettangolo e lontano dal punto random
        A(iter1) = anew;
        B(iter1) = bnew;
        a = anew;
        b = bnew;
        iter1 = iter1+1;
    else
        entra = 1;
%         while entra
        randx  = randx + verso*L1;
        verso = -verso;     %per il prossimo utilizzo
        randy  = C1 + (C2-C1)*rand;
%         distfrarandom = sqrt((Randx(length(Randx))-randx)^2+(Randy(length(Randy))-randy)^2);
%         if distfrarandom < maxlengthspezzate
            Randx = [Randx randx];
            Randy = [Randy randy];
            m = (randy-b)/(randx-a);
            if randy<b && randx<a 
               segnox=-1;
               segnoy=-1;
            elseif randy>b && randx<a
               segnox=-1;
               segnoy=1;
            elseif randy<b && randx>a
               segnox=1;
               segnoy=-1;
            else
               segnox=1;
               segnoy=1;
            end
%             entra=0;
%         end
%         end
    end
    
    %prova per fare un video
%     hold on
%     axis equal
%     axis([0 L1 0 L2])
%     plot(anew,bnew)
%     pause(0.005);
%     %drawnow
%     getframe;        %cosi' plotta sempre troppo lentamente
%     
end
hold off



%plot


plot(A,B,'-r.', Randx, Randy,'-b.')
axis equal
axis([0 L1 0 L2])















