% TRAJLIN       Generisanje diferencijalne pravolinijske trajektorije
%
%    [r rd rdd] = trajlin ([x1,y1,z1], [x2,y2,z2], T, dt)
%    [r rd rdd] = trajlin ([x1,y1,z1], [x2,y2,z2], T, dt, tap)
%
%    x1,y1,z1   koordinate pocetka trajektorije
%    x2,y2,z2   koordinate kraja trajektorije
%    T          vreme za koje treba da se izvrsi kretanje
%    dt         vremenski korak
%    tap        opcioni parametar (za trapezni profil vreme ubrzanja/usporenja u procentima

function [r, rd, rdd]= trajlin (r1, r2, T, dt, tap);

if nargin<5,
   tap=0.5;
end;   

x1=r1(1); y1=r1(2); z1=r1(3);
x2=r2(1); y2=r2(2); z2=r2(3);

n=fix(T/dt);

  
   T1=tap*T;  T2=T-T1;
   
   n1=fix(T1/dt);
   n2=n-n1;
   
   ax= (x2-x1)/(T*(T1+T2)-0.5*(T^2+T1^2+T2^2));
   ay= (y2-y1)/(T*(T1+T2)-0.5*(T^2+T1^2+T2^2));
   az= (z2-z1)/(T*(T1+T2)-0.5*(T^2+T1^2+T2^2));
   
   r  =[x1 y1 z1 0 0 0; zeros(n,6)];
   rd =[0  0  0  0 0 0; zeros(n,6)];
   rdd=[ax ay az 0 0 0; zeros(n,6)];
   
   for i=2:1:n+1,
       
       ri   = r(i-1,:) + rd(i-1,:)*dt + rdd(i-1,:)*dt^2;
       rdi  = rd(i-1,:) + rdd(i-1,:)*dt;
       
       if i<=n1,
          rddi = [ ax  ay  az 0 0 0];
       elseif i<=n2,
          rddi = [ 0   0   0  0 0 0];
       else 
          rddi = [-ax -ay -az 0 0 0];
       end;
       
       r(i,:)  =ri;
       rd(i,:) =rdi;
       rdd(i,:)=rddi;
   end; % for
   

