function [pos,vel,ace] = PolyTrajGenerator(t,tf,Pi,Pf,N)
%__________________________________________________________________________
% in_args: t, time vector
%          Pi, initial position Pi
%          Pf, final position Pi
%          N, order of the polynomial function N @{3,5}
% out_args: position, velocity & acceleration poly trajectories [1xDIM(t)]
%
% Polynomial trajectories
 switch N,
     case 3, % 3rd-order polynomial: ZERO initial and final velocities
             pos = -2*(Pf-Pi)*t.^3/tf^3+3*(Pf-Pi)*t.^2/tf^2+Pi;
             vel = -6*(Pf-Pi)*t.^2/tf^3+6*(Pf-Pi)*t/tf^2;
             ace = -12*(Pf-Pi)*t/tf^3+6*(Pf-Pi)/tf^2;
     case 5, % 5th-order polynomial: ZERO initial/final vel & acceleration
             pos = 6*(Pf-Pi)*t.^5/tf^5-15*(Pf-Pi)*t.^4/tf^4+10*(Pf-Pi)*t.^3/tf^3+Pi;
             vel = 30*(Pf-Pi)/tf^5*t.^4-60*(Pf-Pi)/tf^4*t.^3+30*(Pf-Pi)/tf^3*t.^2;
             ace = 120*(Pf-Pi)/tf^5*t.^3-180*(Pf-Pi)/tf^4*t.^2+60*(Pf-Pi)/tf^3*t;
     otherwise, 
               disp('Unknown order!!!')
               pos = NaN, vel = NaN; ace = NaN;
 end
%__________________________________________________________________________
end