% Calcula uma trajectória polinomial de terceiro 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_poly3(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= 3*delta_pos/T^2;
    c3=-2*delta_pos/T^3;
    
    % Trajectória prevista para esta dimensão
    pos=c0+c1*t+c2*t.^2+c3*t.^3;
    
    % Actualização da variável de saída
    traj=[traj pos];
end