function P = DirKin3dof( q )

 %-- Definition of constants & variables ----------------------------------
 DIM = size(q,2);
 l0 = 47; l1 = 140; l2 = 146;
 Gain1 = (32/8);
 Gain2 = (32/8);
 Gain3 = (30/22);
 
 q1 = -(q(1)+0);
 q2 = q(2)-20;
 q3 = -(q(3)+90);

 q1 = (q1/Gain1)*pi/180;
 q2 = (q2/Gain2)*pi/180;
 q3 = (q3/Gain3)*pi/180;
 
 %-- Hip ------------------------------------------------------------------
  x3= l1*sin(q2)+l2*sin(q2+q3);
  y3= -l1*sin(q1)*cos(q2)-l2*sin(q1)*cos(q2+q3);
  z3= l0+l1*cos(q1)*cos(q2)+l2*cos(q1)*cos(q2+q3);
 %-------------------------------------------------------------------------
  P = [x3;y3;z3];
 %----------------------------------------------------------------------end
 
%  %-- Foot -----------------------------------------------------------------
%   xr=0*q1;
%   yr=0*q1;
%   zr=0*q1;
%  %-- Ankle ----------------------------------------------------------------
%   x1=xr;
%   y1=yr;
%   z1=l0*ones(1,DIM);
%  %-- Knee -----------------------------------------------------------------
%   x2=x1+l1*sin(q2);
%   y2=y1-l1*sin(q1).*cos(q2);
%   z2=z1+l1*cos(q1).*cos(q2);
%  %-- Hip ------------------------------------------------------------------
%   x3=x2+l2*sin(q2+q3);
%   y3=y2-l2*sin(q1).*cos(q2+q3);
%   z3=z2+l2*cos(q1).*cos(q2+q3);
%  %-------------------------------------------------------------------------
%   P = [xr x1 x2 x3; yr y1 y2 y3; zr z1 z2 z3];
  
end