function pFinal=animate(h,P,pinicial,D,N)

%h=handle grafico
%P= matriz pontos objectos
%pinicial= matriz transformaçao da posiçao inicial
%D - vector [x y z phi theta psi] do incremento da posiçao
%N=nº de passos na animaçao
%pfinal - 
X=linspace(0,D(1),N);
Y=linspace(0,D(2),N);
Z=linspace(0,D(3),N);
Phi=linspace(0,D(4),N);
Theta=linspace(0,D(5),N);
Psi=linspace(0,D(6),N);

for n=1:N;
T=trans(X(n),Y(n),Z(n))*rotx(X(n))*roty(Y(n))*rotz(Z(n));
Q=pinicial*T*P;
set(h,'Xdata',Q(1,:),'Ydata',Q(2,:),'Zdata',Q(3,:))
pause(0.1)
end