function [P,C,F,E] = CoordJuntas( Ai,XYZRef,L,O )

%----------------------------------------------------------------------------------
%-- Coordenadas dos elos mecanicos: endoesqueleto formado por linhas
%----------------------------------------------------------------------------------
 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:4;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
     T = T*Ai(:,:,T_ind(i)); 
     P(j,:) = T(1:3,4)'; j = j+1;
     
 end
  
%----------------------------------------------------------------------------------
%-- Coordenadas do pe de suporte: 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 = [1 0 0 XYZRef(1) ; 0 1 0 XYZRef(2) ; 0 0 1 XYZRef(3) ; 0 0 0 1]; %-- Pe de suporte
 F1 = [  -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     ]; F1 = T*F1;

 F2 = [  -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     ]; F2 = T*F2;

 F(1:3,:,3) = F1(1:3,:);  F(1:3,:,4) = F2(1:3,:);   %-- Pe de suporte
 
%----------------------------------------------------------------------------------
%-- Coordenadas do exoesqueleto (perna): 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,:);
     
%  keyboard
 ll_h = L(4);                               %-- Elo anca-anca
 T_ind = 5;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind), T = T*Ai(:,:,T_ind(i)); end
 E5 = [  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      ]; E5 = T*E5; E(1:3,:,5) = E5(1:3,:);
 E6 = [  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      ]; E6 = T*E6; E(1:3,:,6) = E6(1:3,:);

%----------------------------------------------------------------------------------
%-- 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:4;                                 %-- Indices Ai: A1*A2*...A14
 for i=1:length(T_ind);
     
    T = T*Ai(:,:,T_ind(i)); 
     
    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
 
%-------------------------------------------------------------------------------end