function [C_joint,C_tool] = BodyCoordinates( q,L,warm )
%__________________________________________________________________________
% in_args: q, joint vector
%          L, arm's link lengths
% out_args: coordinates of the joint cylinders (C_joint) and tool (C_tool)
%
% Decodes input arguments  
 lt = L(1); lt_l = L(2); lt_w = L(3); lt_h = L(4); 
 lr = L(5); l0 = L(6); 
 l1 = L(7); l1_1 = L(8); l1_2 = L(9); l2 = L(10); 
 ang1 = q(1); ang2 = q(2); ang3 = q(3); ang4 = q(4);
% Decodes the desired arm
 if warm == +1, FLAG_sign =  1;     %Left arm
 else           FLAG_sign = -1; end % Right arm   
% Original cylinder
 [X,Y,Z] = cylinder(1,100);
 X = 1.1*lr*X; Y = 1.1*lr*Y; Z = 8*lr*(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))];
% Joint#1: homogeneous transformation 
 T1 = [ 0   FLAG_sign   0           0
        0   0          -FLAG_sign   FLAG_sign*(lt_l/2-lr)
       -1   0           0           lt+lt_h-lr
        0   0           0           1           ];
 C1([1 3 5 7],:) = T1*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                               ones(1,length(Pcyl(1,:)))];
 C1([2 4 6 7],:) = T1*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                               ones(1,length(Pcyl(1,:)))];
% Joint#2: homogeneous transformation
 T2 = T1*[ cos(ang1)  0          -FLAG_sign*sin(ang1) 0
           sin(ang1)  0           FLAG_sign*cos(ang1) 0
           0         -FLAG_sign   0                 -l0-lr
           0          0           0                  1           ];
 C2([1 3 5 7],:) = T2*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                               ones(1,length(Pcyl(1,:)))];
 C2([2 4 6 7],:) = T2*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                               ones(1,length(Pcyl(1,:)))];                                           
% Joint#3: homogeneous transformation
 T3 = T2*[ -sin(ang2)  0   cos(ang2) l1_1*cos(ang2)
            cos(ang2)  0   sin(ang2) l1_1*sin(ang2)
            0          1   0         0
            0          0   0         1           ];
 C3([1 3 5 7],:) = T3*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                               ones(1,length(Pcyl(1,:)))];
 C3([2 4 6 7],:) = T3*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                               ones(1,length(Pcyl(1,:)))];
% Joint#4: homogeneous transformation
 T4 = T3*[ -sin(ang3)  0          -FLAG_sign*cos(ang3) 0
            cos(ang3)  0          -FLAG_sign*sin(ang3) 0
            0         -FLAG_sign   0                   l1_2
            0          0           0                   1           ];
 C4([1 3 5 7],:) = T4*[Pcyl(1,:); Pcyl(3,:); Pcyl(5,:); ...
                                               ones(1,length(Pcyl(1,:)))];
 C4([2 4 6 7],:) = T4*[Pcyl(2,:); Pcyl(4,:); Pcyl(6,:); ...
                                               ones(1,length(Pcyl(1,:)))];                                           
% Output matrix: joint cylinders
 C_joint(:,:,1) = C1;
 C_joint(:,:,2) = C2;
 C_joint(:,:,3) = C3;
 C_joint(:,:,4) = C4;
% Output matrix: tool tip
 T5 = T4*[  FLAG_sign*sin(ang4)  FLAG_sign*cos(ang4) 0  FLAG_sign*l2*sin(ang4)
           -FLAG_sign*cos(ang4)  FLAG_sign*sin(ang4) 0 -FLAG_sign*l2*cos(ang4)
            0                    0                   1  0
            0                    0                   0  1           ];
 Ptool = [0 -0.03 -0.03 0; 0 0 0 0; 0.015 0.015 -0.015 -0.015; 1 1 1 1];
 C_tool = T5*Ptool;
 %__________________________________________________________________________
end