function [QQ,t] = PolyTrajV(Q0,Qf,Qv0,Qvf,N,t0,tf)
%Retorna trajet´orias polinomiais de terceira ordem entre duas posturas com velocidades iniciais e finais distintas
%QQ - matriz com N colunas onde cada coluna cont´em os valores das juntas nos instantes correspondentes.
%t - vetor com os tempos correspondentes aos N instantes de amostragem da trajet´oria entre o instante inicial (t0) e o final (tf).
%inputs: 
%Q0-vetor da posicao inicial das juntas (teta0)
%Qf-vetor da posicao final das juntas (tetaf)
%Qv0- vetor da velocidade inicial das juntas
%Qvf-vetor da velocidade final das juntas
%t0- instante inicial da trajetoria (segundos)
%tf-instate final da trajetoria (segundos)
%N-Numero de pontos a usar na definiao da trajetoria
t=linspace(t0,tf,N);


for n=1:numel(Q0) %para cada junta
    A3=(-2/((tf-t0)^3)*(Qf(n)-Q0(n)) + (1/((tf-t0)^2))*(Qvf(n)+Qv0(n)));
    A2=((3/((tf-t0)^2).*(Qf(n)-Q0(n)) - (2/(tf-t0)*Qv0(n)) - ((1/(tf-t0))*Qvf(n))));
    QQ(n,:)=Q0(n) + Qv0(n).*(t-t0) +A2.*((t-t0).^2) + A3.*((t-t0).^3);
end
% for n=1:size(Q0,1)
%     a0=Q0(n);
%     a1=velocidade0;
%     a2=( 3*(tetaf-teta0)/((tf-t0).^2) )- (2*teta0/(tf-t0)).*velocidade0 - (velocidadef/(tf-t0));
%     a3=( 2*(tetaf-teta0)/((tf-t0).^3) )-  ((velocidadef-velocidade0)/(tf-t0).^2);
%     teta=a0+a1.*(t-t0)+a2.*(t-t0)^2+a3.*(t-t0).^3;
%     
%     QQ(n,:)=teta;
% end




end

