function P = DirKin3dof( q,L )

 %-- Definition of constants & variables ----------------------------------
 DIM = size(q,2);
 q1 = q(1,:); q2 = q(2,:); q3 = q(3,:);
 l0 = L(1); l1 = L(2); l2 = L(3);
 %-- 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;yr;zr; x1;y1;z1; x2;y2;z2; x3;y3;z3];
 %----------------------------------------------------------------------end
