function [pFinal] = Animate(h,P,Prev_M,end_pos,N)
% h-handle grafico do objeto a animar (movimentar);
% P-matriz de pontos do objeto (no formato homogeneo);
% Prev_M=pInicial-Matriz de transformacao da posicao inicial do objeto;
% end_pos=D-vetor [x; y; z; a; b; c] (a,b,c em rad) com os incrementos totais em posicao e
% orientacao vistos do referencial local do objeto; este vetor servira para
% criar a matriz de transformacao que sera usada em pos-multiplicacao.
% N-numero de passos da animacao da etapa (duracao" de movimento);
% pFinal-matriz com a posic~ao do objeto no final do movimento.
%   Detailed explanation goes here
    
x=linspace(0,end_pos(1), N);
y=linspace(0,end_pos(2), N);
z=linspace(0,end_pos(3), N);
a=linspace(0,end_pos(4), N);
b=linspace(0,end_pos(5), N);
c=linspace(0,end_pos(6), N);

for n=1:N
    M_trans=trans(x(n),y(n),z(n))*rotx(a(n))*roty(b(n))*rotz(c(n));
    M=Prev_M*M_trans;% Ref local ! se invertemos a ordem de multiplicacao utilizamos o Ref Global!
    P1=M*P;
    set(h,'Xdata',P1(1,:),'Ydata',P1(2,:),'Zdata',P1(3,:));
    %fill3(P1(1,:),P1(2,:),P3(3,:),'g');
    plot3(P1(1,1),P1(2,1),P1(3,1),'.b');
    pause(0.05);
end
pFinal=M; %ATENCAO !! nao é igual ao P1 , nao é o objeto!
end
