function [cm_Cima,Centros]=DH(manipulador,alteracoes,cmassa,mi,Pfinal);          %para animar o manipulador
t=0.01;                                                         %tempo de cada frame
n=50;                                                           %numero de frames
n_elos=size(manipulador,1);                                     %primeiro e necessario saber o numero de elos
for j=1:n_elos;                                                 %para cada elo (movimenta-los todos ao mesmo tempo)
    Oi(j)=manipulador(j,1);                                     %valor inicial de theta
	Of(j)=Oi(j)+alteracoes(j,1);                                %posiçao final de theta
	passoO(j)=(Of(j)-Oi(j))/n;                                  %incremento/passo a theta
end;                                                            %
if passoO(:)==0;                                                %verificar se existe "passo"
    inc=n;                                                      %
end;                                                            %  
inc=0;                                                          %numero de incremento
while inc<=n;                                                   %repete-se este ciclo n vezes (passos intermedios)
        inc=inc+1;                                              %faz-se incremento
        axis equal;                                             %todos eixos iguais
        axis([-50 200 -150 100 0 500]);                         %se nao, define-se os eixos
        hold on;                                                %fixa-se o grafico
		grid on;                                                %coloca-se grelha
        Ref(:,:,1)=eye(4,4);                                    %matriz identidade 4*4 (especie de referencial para cada elo)
        junta(:,1)=[0;0;0;1];                                   %primeira junta
        centromassa(1:4,1:n_elos)=0;                            %centros de massa locais
        Rcm(1:3,1)=0;                                           %centro de massa global
        for i=1:n_elos;                                         %determino DH para todos os elos (i->numero do elo)
		    manipulador(i,1)=Oi(i);                             %actualiza-se o manipulador
	        theta=manipulador(i,1);                             %   "
	        alfa=manipulador(i,2);                              %   "
	        l=manipulador(i,3);                                 %   "
	        d=manipulador(i,4);                                 %   "
	        DH(1,1,i)=cos(theta);                               %   "
        	DH(1,2,i)=-sin(theta)*cos(alfa);                    %   "
        	DH(1,3,i)=sin(theta)*sin(alfa);                     %   "
        	DH(1,4,i)=l*cos(theta);                             %   "
        	DH(2,1,i)=sin(theta);                               %   "
        	DH(2,2,i)=cos(theta)*cos(alfa);                     %   "
        	DH(2,3,i)=-cos(theta)*sin(alfa);                    %   "
        	DH(2,4,i)=l*sin(theta);                             %   "
        	DH(3,1,i)=0;                                        %   "
        	DH(3,2,i)=sin(alfa);                                %   "
        	DH(3,3,i)=cos(alfa);                                %   "
        	DH(3,4,i)=d;                                        %   "
        	DH(4,1,i)=0;                                        %   "
        	DH(4,2,i)=0;                                        %   "
        	DH(4,3,i)=0;                                        %   "
        	DH(4,4,i)=1;                                        %   "
            Ref(:,:,i+1)=Ref(:,:,i)*DH(:,:,i);                  %   "
			junta(:,i+1)=Ref(:,:,i+1)*junta(:,1);               %determino nova junta (acrescento-a a matriz junta)            
            centromassa(:,i)=Ref(:,:,i+1)*cmassa(:,i);          %centros de massas locais
            Rcm=Rcm+centromassa(1:3,i)*mi(i);                   %centro de massa global
        end;                                                                        %
        Pe=[-45 30 30 -45;-105 -105 95 95;0 0 0 0;1 1 1 1];                         %PE de apoio
        fill3(Pe(1,:),Pe(2,:),Pe(3,:),'y');                                         %desenha PE de apoio
        PeE=Ref(:,:,i+1)*Pe;                                                        %PE solto (nova posicao)
        fill3(PeE(1,:),PeE(2,:),PeE(3,:),'y');                                      %desenha PE solto
        plot3(junta(1,:),junta(2,:),junta(3,:));                                    %desenho os elos
        plot3(junta(1,2:n_elos-1),junta(2,2:n_elos-1),junta(3,2:n_elos-1),'bo');    %desenha juntas
        plot3(centromassa(1,2:n_elos),centromassa(2,2:n_elos),centromassa(3,2:n_elos),'rx'); %desenha centros de massa (locais)
        Oi=Oi+passoO;                                                               %incremento o passo a posiçao anterior (theta)
		if inc<=n;                                                                  %caso ainda nao tenha acabado
			pause(t);                                                               %pausa para visualizaçao
			clf;                                                                    %apaga passos anteriores
		end;                                                                        %
end;                                                                                %
[cm_Cima,Ref]=Parte_superior(Ref,cmassa,mi,Pfinal);             %contribuiçao da parte superior
[cm_Direita,Ref]=Parte_direita(Ref,cmassa,mi,Pfinal);           %contribuiçao da parte superior direita (braço, ombro...)
[cm_Esquerda,Ref]=Parte_esquerda(Ref,cmassa,mi,Pfinal);         %contribuiçao da parte superior esquerda (braço, ombro...)
cm_estrutura_sup=cm_Cima*(sum(mi(18:23)))+cm_Direita*(sum(mi(24:27)))+cm_Esquerda*(sum(mi(28:31)));
                                                                %
Rcm=(Rcm+cm_estrutura_sup)/(sum(mi(1:31)));                     %
plot3(Rcm(1),Rcm(2),Rcm(3),'ko');                               %desenha centro de massa (global)
plot3(Rcm(1),Rcm(2),0,'go','linewidth',7);                      %projecçao do centro de massa no chao
                                                                %
                                                                %                                                                
%VALORES DE SAIDA                                               %
Centros(1:4,1:n_elos)=centromassa(1:4,1:n_elos);                %atribuiçao de valores de saida (centros de massa)
Centros(5:8,1:n_elos)=junta(1:4,1:n_elos);                      %atribuiçao de valores de saida (posiçao dos elos)
cm_Cima=cm_estrutura_sup/(sum(mi(18:31)));                      %centro de massa da estrutura superior
