% Calcula uma trajectória polinomial de quinto grau, considerando
% velocidades inicial e final nulas.
%
% Syntax: traj=calctrajectory_poly3(t,pos0,posF,T)
%
% Parametros de entrada:
%   t    -> Array de instantes de tempo a calcular as posições previstas
%   pos0 -> Posição inicial da trajectória (x,y,z)
%   posF -> Posição final da trajectória (x,y,z)
%   T    -> Duração da trajectória
%
% Parâmetros de saída:
%   traj -> Array de posições da trajectória calculada para as três
%           dimensões (x,y,z)

function traj=calctrajectory_poly5(t,pos0,posF,T)

% Inicializar a variável de saída
traj=[];

% Percorrer todas as dimensões
for i=1:min(length(pos0),length(posF))
    
    % Variação de posição
    delta_pos=posF(i)-pos0(i);
    
    % Coeficientes do polinómio de terceira ordem
    c0= pos0(i);
    c1= 0;
    c2= 0;
    c3= 10*delta_pos/T^3;
    c4=-15*delta_pos/T^4;
    c5=  6*delta_pos/T^5;
    
    % Trajectória prevista para esta dimensão
    pos=c0+c1*t+c2*t.^2+c3*t.^3+c4*t.^4+c5*t.^5;
    pos(find(t>T))=posF(i);
    
    % Actualização da variável de saída
    traj=[traj pos];
end