% Cálculo da cinemática directa

function xy=cinematdir(theta,L)

% Comprimento dos elos
L1=L(1);
L2=L(2);

theta=theta*pi/180;

theta0=theta(1);
theta1=theta(2)+pi/2;
theta2=theta(3);

% Cálculo da cinemática directa

% Componentes zz sem considerar a inclinação de theta0
h1=L1*sin(theta1);
h2=h1+L2*sin(theta1+theta2);

% Coordenadas (x,y,z) do joelho
x1=h1*sin(theta0);
y1=L1*cos(theta1);
z1=h1*cos(theta0);

% Coordenadas (x,y,z) da anca
x2=h2*sin(theta0);
y2=y1+L2*cos(theta1+theta2);
z2=h2*cos(theta0);

% Retorno do resultado
xy=[x1 y1 z1; x2 y2 z2];