% FUNCTION DINSCAL2(Xo,Xf,tf,proc) Vezba za dinamicko skaliranje trajektorija
%                                  po sinusoidalnom profilu intenziteta brzine
%                                  vrha robota

function dinscal2(A,B,tf);

%A=[0.2 0.2];
%B=[1 1];
%tf=3;
%primer poziva: dinscal2([0.2 0.2],[1 1],3)

dt=0.025;
t=[0:dt:tf];

Xo=A(1);
Xf=B(1);
Yo=A(2);
Yf=B(2);

k=(Yf-Yo)/(Xf-Xo);
n=Yf-k*Xf;

w=pi/tf;
a=pi*sqrt(1+k*k)*(Xf-Xo)/(2*tf);
X=Xo*ones(size(t))+(a/(w*sqrt(1+k*k))*(ones(size(t))-cos(w*t)));
Xd=a*sin(w*t)/sqrt(1+k*k);
Xdd=a*w*cos(w*t)/sqrt(1+k*k);

Y=k*X+n*ones(size(t));
Yd=k*Xd;
Ydd=k*Xdd;

v=sqrt(Xd.*Xd+Yd.*Yd);
ac=sqrt(Xdd.*Xdd+Ydd.*Ydd);

kindin;

