function handles = CoordJuntas( Ai,DM,handles )

q       = DM.q(DM.fase,:);
L       = DM.L;
O       = DM.O;
XYZRef  = DM.XYZRef(DM.fase,:);
pe      = DM.pe(DM.fase,:);
tetaPe  = DM.tetaPe(DM.fase);

%----------------------------------------------------------------------------------
%-- Coordenadas dos elos mecanicos: endoesqueleto formado por linhas
%----------------------------------------------------------------------------------


rotPe = makehgtform('xrotate', tetaPe);

Ai(:,:,1) = Ai(:,:,1)*rotPe;
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];    %-- DE perna direita ATE esquerda
 j = 1; P(j,:) = [XYZRef(1) XYZRef(2) XYZRef(3)]; j=j+1; %-- Referencia: P[1] = (X1,Y1,Z1) 
 T_ind = 1:14;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     if i ~= 7, P(j,:) = T(1:3,4)'; j = j+1; end %-- Anca: 2X P(7) ----------------
     if T_ind(i) == 7, T_aux = T; end           %-- Guardar T=A1*A2*...A6 ---------
     
 end
  
 T = T_aux;                                    %-- DE perna direita ATE cabeça
 T_ind = 15:22;                                %-- Indices Ai: T_aux*A15*...A17
 for i=1:length(T_ind);
     T = T*Ai(:,:,T_ind(i));
     if T_ind(i) ~= 20, P(j,:) = T(1:3,4)'; j=j+1; end %-- Pescoço: 2X P(19)-------
     if T_ind(i) == 17, T_aux = T; end           %-- Guardar T=A1*A2*...A6 ---------
 end
  
 T = T_aux;                                    %-- DE tronco ATE braço direito
 T_ind = 23:25;                                %-- Indices Ai: T_aux*A15*...A17
 for i=1:length(T_ind);
     T = T*Ai(:,:,T_ind(i));
     P(j,:) = T(1:3,4)';                        %-- Braço direito: ---------------
     j=j+1; 
 end

 T = T_aux;                                    %-- DE tronco ATE braço esquerdo
 T_ind = 26:28;                                %-- Indices Ai: T_aux*A15*...A17
 for i=1:length(T_ind);
     T = T*Ai(:,:,T_ind(i));
     P(j,:) = T(1:3,4)';                        %-- Braço esquerdo: ---------------
     j=j+1; 
 end
 
 
 
%----------------------------------------------------------------------------------
%-- Coordenadas do pe de suporte e pe livre: 2 faces paralelas
%----------------------------------------------------------------------------------
 lf_l = O(1,1);  lf_w = O(1,2); lf_h = O(1,3);
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];    %-- Matriz identidade
 T_ind = 1:14;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 F1 = [  0       0       0       0      %-- Top XY
         lf_l/2 -lf_l/2 -lf_l/2  lf_l/2
         lf_w/2  lf_w/2 -lf_w/2 -lf_w/2     
         1       1       1       1      ]; F1 = T*F1;
         
 F2 = [ -lf_h   -lf_h   -lf_h   -lf_h %-- Top XY
         lf_l/2 -lf_l/2 -lf_l/2  lf_l/2
         lf_w/2  lf_w/2 -lf_w/2 -lf_w/2     
         1       1       1       1      ]; F2 = T*F2;         

T = Ai(:,:,1)*makehgtform('yrotate', pi/2, 'zrotate', pi, 'translate', 0, 0, -L(1)); %-- Pe de suporte
%  T = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 0 ; 0 0 0 1]*Ai(:,:,1); %-- Pe de suporte
 F3 = [  -lf_w/2  lf_w/2  lf_w/2 -lf_w/2 %-- Top XY
         -lf_l/2 -lf_l/2  lf_l/2  lf_l/2
          0      0      0      0  
          1      1      1      1     ]; F3 = T*F3;

 F4 = [  -lf_w/2  lf_w/2  lf_w/2 -lf_w/2 %-- Top XY
         -lf_l/2 -lf_l/2  lf_l/2  lf_l/2
          0.01  0.01  0.01  0.01  
         1      1      1      1     ]; F4 = T*F4;
 
 if(strcmp(pe,'dir'))
     F(1:3,:,1) = F1(1:3,:);  F(1:3,:,2) = F2(1:3,:);   %-- Pe livre
     F(1:3,:,3) = F3(1:3,:);  F(1:3,:,4) = F4(1:3,:);   %-- Pe de suporte
 else
     F(1:3,:,1) = F3(1:3,:);  F(1:3,:,2) = F4(1:3,:);   %-- Pe de suporte
     F(1:3,:,3) = F1(1:3,:);  F(1:3,:,4) = F2(1:3,:);   %-- Pe livre
 end
 
 handles.F = F;
 
%----------------------------------------------------------------------------------
%-- Coordenadas do exoesqueleto (pernas e braços): 2 faces
%----------------------------------------------------------------------------------
 ll_l = O(2,1);  ll_w = O(2,2); ll_h = L(3);  %-- Elo tornozelo-joelho
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];   %-- Matriz identidade
 T_ind = 1:3;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E1 = [  0        0       -ll_h   -ll_h   %-- XX
         ll_w/2  -ll_w/2  -ll_w/2  ll_w/2 %-- YY
         ll_l/2   ll_l/2   ll_l/2  ll_l/2 %-- ZZ
         1       1       1         1      ]; E1 = T*E1; E(1:3,:,1) = E1(1:3,:);
 E2 = [  0        0       -ll_h   -ll_h   %-- XX
        -ll_w/2   ll_w/2   ll_w/2 -ll_w/2 %-- YY
        -ll_l/2  -ll_l/2  -ll_l/2 -ll_l/2 %-- ZZ
         1       1       1         1      ]; E2 = T*E2; E(1:3,:,2) = E2(1:3,:);     

 ll_h = L(4);                               %-- Elo joelho-anca
 T_ind = 4;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E3 = [  0        0       -ll_h   -ll_h   %-- XX
         ll_w/2  -ll_w/2  -ll_w/2  ll_w/2 %-- YY
         ll_l/2   ll_l/2   ll_l/2  ll_l/2 %-- ZZ
         1       1       1         1      ]; E3 = T*E3; E(1:3,:,3) = E3(1:3,:);
 E4 = [  0        0       -ll_h   -ll_h   %-- XX
        -ll_w/2   ll_w/2   ll_w/2 -ll_w/2 %-- YY
        -ll_l/2  -ll_l/2  -ll_l/2 -ll_l/2 %-- ZZ
         1       1       1         1      ]; E4 = T*E4; E(1:3,:,4) = E4(1:3,:);

 T_ind = 5:11;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E5 = [ -ll_h    -ll_h     0       0   %-- XX
         ll_w/2  -ll_w/2  -ll_w/2  ll_w/2 %-- YY
        -ll_l/2  -ll_l/2  -ll_l/2 -ll_l/2 %-- ZZ
         1        1        1       1      ]; E5 = T*E5; E(1:3,:,5) = E5(1:3,:);
 E6 = [ -ll_h    -ll_h     0       0   %-- XX
        -ll_w/2   ll_w/2   ll_w/2 -ll_w/2 %-- YY
         ll_l/2   ll_l/2   ll_l/2  ll_l/2 %-- ZZ
         1        1        1       1      ]; E6 = T*E6; E(1:3,:,6) = E6(1:3,:);     

 ll_h = L(3);                                %-- Elo tornozelo-joelho
 T_ind = 12;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E7 = [ -ll_h    -ll_h     0       0   %-- XX
         ll_w/2  -ll_w/2  -ll_w/2  ll_w/2 %-- YY
        -ll_l/2  -ll_l/2  -ll_l/2 -ll_l/2 %-- ZZ
         1        1        1       1      ]; E7 = T*E7; E(1:3,:,7) = E7(1:3,:);
 E8 = [ -ll_h    -ll_h     0       0   %-- XX
        -ll_w/2   ll_w/2   ll_w/2 -ll_w/2 %-- YY
         ll_l/2   ll_l/2   ll_l/2  ll_l/2 %-- ZZ
         1        1        1       1      ]; E8 = T*E8; E(1:3,:,8) = E8(1:3,:);     

 la_l = O(5,1);  la_w = O(5,2); la_h = O(5,3);
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];    %-- Matriz identidade
 T_ind = [1:7 15:17 23:25];                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E9 = [  0        0       -la_h   -la_h   %-- XX
         la_l/2   la_l/2   la_l/2  la_l/2 %-- YY
         la_w/2  -la_w/2  -la_w/2  la_w/2 %-- ZZ
         1       1       1         1      ]; E9 = T*E9; E(1:3,:,9) = E9(1:3,:);
 E10= [  0        0       -la_h   -la_h   %-- XX
        -la_l/2  -la_l/2  -la_l/2 -la_l/2 %-- YY
        -la_w/2   la_w/2   la_w/2 -la_w/2 %-- ZZ        
         1       1       1         1      ]; E10 = T*E10; E(1:3,:,10) = E10(1:3,:);    
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];    %-- Matriz identidade
 T_ind = [1:7 15:17 26:28];                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E11= [  0        0       -la_h   -la_h   %-- XX
         la_l/2   la_l/2   la_l/2  la_l/2 %-- YY
         la_w/2  -la_w/2  -la_w/2  la_w/2 %-- ZZ
         1       1       1         1      ]; E11 = T*E11; E(1:3,:,11) = E11(1:3,:);
 E12= [  0        0       -la_h   -la_h   %-- XX
        -la_l/2  -la_l/2  -la_l/2 -la_l/2 %-- YY
        -la_w/2   la_w/2   la_w/2 -la_w/2 %-- ZZ        
         1       1       1         1      ]; E12 = T*E12; E(1:3,:,12) = E12(1:3,:);        
     
 handles.E = E;
 
%----------------------------------------------------------------------------------
%-- Coordenadas da secçao da anca: 3 faces
%----------------------------------------------------------------------------------
 lh_l = O(3,1);  lh_w = O(3,2); lh_h = O(3,3);
 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];    %-- Matriz identidade
 T_ind = [1:7 15];                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 H1 = [  lh_l/2  lh_l/2  lh_l/14  -lh_l/14  -lh_l/2  -lh_l/2   %-- XX
         lh_h    lh_h/2  0         0         lh_h/2   lh_h     %-- YY
         lh_w/2  lh_w/2  lh_w/2    lh_w/2    lh_w/2   lh_w/2   %-- ZZ
         1       1       1         1         1        1        ]; H1 = T*H1;                

 H2 = [  lh_l/2  lh_l/2  lh_l/14  -lh_l/14  -lh_l/2  -lh_l/2   %-- XX
         lh_h    lh_h/2  0         0         lh_h/2   lh_h     %-- YY
        -lh_w/2 -lh_w/2 -lh_w/2   -lh_w/2   -lh_w/2  -lh_w/2   %-- ZZ
         1       1       1         1         1        1        ]; H2 = T*H2;

 H3 = [  lh_l/2 -lh_l/2 -lh_l/2    lh_l/2    lh_l/2  -lh_l/2   %-- XX
         lh_h    lh_h    lh_h      lh_h      lh_h     lh_h     %-- YY
         lh_w/2  lh_w/2 -lh_w/2   -lh_w/2    lh_w/2   lh_w/2   %-- ZZ
         1       1       1         1         1        1        ]; H3 = T*H3;                

 H4 = [  lh_l/14 -lh_l/14 -lh_l/14  lh_l/14  lh_l/14  -lh_l/14   %-- XX
         0        0        0        0        0         0     %-- YY
         lh_w/2  lh_w/2 -lh_w/2   -lh_w/2    lh_w/2   lh_w/2   %-- ZZ
         1       1       1         1         1        1        ]; H4 = T*H4;  

 H(1:3,:,1) = H1(1:3,:); H(1:3,:,2) = H2(1:3,:); 
 H(1:3,:,3) = H3(1:3,:); H(1:3,:,4) = H4(1:3,:);
 
 handles.H = H;
 
%----------------------------------------------------------------------------------
%-- Coordenadas da secçao do tronco: 4 faces
%----------------------------------------------------------------------------------
 lt_l = O(4,1);  lt_w = O(4,2); lt_h = O(4,3);
 T_ind = 16:18;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 W1 = [  lt_h     lt_h     lt_h    lt_h   %-- XX
         lt_w/2  -lt_w/2  -lt_w/2  lt_w/2 %-- YY
        -lt_l/2  -lt_l/2   lt_l/2  lt_l/2 %-- ZZ
         1       1       1         1      ]; W1 = T*W1;                

 W2 = [  0        0        0       0      %-- XX
         lt_w/2  -lt_w/2  -lt_w/2  lt_w/2 %-- YY
        -lt_l/2  -lt_l/2   lt_l/2  lt_l/2 %-- ZZ
         1       1       1         1      ]; W2 = T*W2;

 W3 = [  lt_h     lt_h     0        0      %-- XX
        -lt_w/2   lt_w/2   lt_w/2  -lt_w/2 %-- YY
        -lt_l/2  -lt_l/2  -lt_l/2  -lt_l/2 %-- ZZ
         1       1       1         1      ]; W3 = T*W3;                

 W4 = [  lt_h     lt_h     0        0      %-- XX
        -lt_w/2   lt_w/2   lt_w/2  -lt_w/2 %-- YY
         lt_l/2   lt_l/2   lt_l/2   lt_l/2 %-- ZZ
         1       1       1         1      ]; W4 = T*W4;  

 W(1:3,:,1) = W1(1:3,:); W(1:3,:,2) = W2(1:3,:); 
 W(1:3,:,3) = W3(1:3,:); W(1:3,:,4) = W4(1:3,:);
 
 handles.W = W;
 
%----------------------------------------------------------------------------------
%-- Coordenadas da camara video: sphere
%----------------------------------------------------------------------------------
 T_ind = 19:22;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 R = 0.025; Trans = [1 0 0 R ; 0 1 0 0 ; 0 0 1 0 ; 0 0 0 1];
 T = T*Trans;
 P(28,:) = T(1:3,4)';                       %-- Centro da camara video ------------
 
 handles.P = P;
 
%----------------------------------------------------------------------------------
%-- Coordenadas dos cilindros: 22 juntas rotacionais
%----------------------------------------------------------------------------------
%-- Definiçao do cilindro original ------------------------------------------------
 [X,Y,Z] = cylinder(1,100);
 X = 0.01*X; Y = 0.01*Y; Z = 0.07*(Z-0.5);
 X1 = X(1,:); Y1 = Y(1,:); Z1 = Z(1,:);
 X2 = X(2,:); Y2 = Y(2,:); Z2 = Z(2,:);
 Pcyl = [X1;X2;Y1;Y2;Z1;Z2;ones(1,length(X1))];

 T = [ 1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
 j = 1; T_ind = 1:13;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     if i ~= 6 && i ~= 7 && i ~= 8, 
        Cj([1 3 5 7],:) = T*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
     if i == 7 || i == 8, 
        Trans = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 lf_l/2 ; 0 0 0 1]; %-- elevar a junta da anca
        Cj([1 3 5 7],:) = T*Trans*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*Trans*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj;  
        j=j+1;     
     end
     if T_ind(i) == 7, T_aux = T; end           %-- Guardar T=A1*A2*...A6 ---------
     
 end
 
 [X,Y,Z] = cylinder(1,100);
 X = 0.005*X; Y = 0.005*Y; Z = 0.05*(Z-0.5);
 X1 = X(1,:); Y1 = Y(1,:); Z1 = Z(1,:);
 X2 = X(2,:); Y2 = Y(2,:); Z2 = Z(2,:);
 Pcyl = [X1;X2;Y1;Y2;Z1;Z2;ones(1,length(X1))];

 T = T_aux;
 T_ind = 15:21;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     if T_ind(i) == 16 || T_ind(i) == 17 || T_ind(i) == 21, 
        Cj([1 3 5 7],:) = T*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
     if T_ind(i) == 20, 
        Trans = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 -0.05/2 ; 0 0 0 1]; %-- elevar a junta da anca
        Cj([1 3 5 7],:) = T*Trans*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*Trans*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
    end
    if T_ind(i) == 17, T_aux = T; end           %-- Guardar T=A1*A2*...A6 ---------
     
 end
 
 T = T_aux;
 T_ind = 23:24;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     if T_ind(i) == 24, 
        Cj([1 3 5 7],:) = T*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
     if T_ind(i) == 23, 
        Trans = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 -0.05/2 ; 0 0 0 1]; %-- elevar a junta da anca
        Cj([1 3 5 7],:) = T*Trans*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*Trans*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
      
 end
 T = T_aux;
 T_ind = 26:27;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     if T_ind(i) == 27, 
        Cj([1 3 5 7],:) = T*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
     if T_ind(i) == 26, 
        Trans = [1 0 0 0 ; 0 1 0 0 ; 0 0 1 -0.05/2 ; 0 0 0 1]; %-- elevar a junta da anca
        Cj([1 3 5 7],:) = T*Trans*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ones(1,length(Pcyl(1,:)))];
        Cj([2 4 6 7],:) = T*Trans*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ones(1,length(Pcyl(1,:)))];
        C(:,:,j) = Cj; j=j+1;     
     end
      
 end 
 handles.C = C;
%-------------------------------------------------------------------------------end
