% Cálculo da cinemática directa a partir dos ângulos das juntas (não dos
% servos) e do comprimento dos elos.
%
% Syntax: xy=cinematdir(theta,L)
%
% Parâmetros de entrada:
%   theta -> [ângulo da junta ortogonal ao pé,
%             ângulo da junta paralela ao pé,
%             ângulo do joelho]
%   L     -> [Comprimento (cm) da parte inferior do tornozelo,
%             Comprimento (cm) da parte superior do tornozelo,
%             Comprimento do elo inferior da perna,
%             Comprimento do elo superior da perna]
%
% Parâmetros de saída:
%   xy -> Linhas definem o índice de theta (igual n. de linhas)
%         As 3 colunas referem-se as coordenadas cartesianas (x,y,z)
%         Dois conjuntos 3 colunas (3a dimensão) indicando as
%         ...coordenadas cartesianas do joelho e da anca respectivamente.

function xy=cinematdir(theta,L)

% Comprimento dos elos
Hi=L(1);            % Parte inferior do tornozelo
Hs=L(2);            % Parte superior do tornozelo
L1=L(3);            % Elo inferior da perna
L2=L(4);            % Elo superior da perna

theta=theta*pi/180;

N=size(theta,1);
xy=zeros(N,3,2);

for k=1:N
    
    theta0=theta(k,1);
    theta1=theta(k,2);
    theta2=theta(k,3);

    % Cálculo da cinemática directa

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

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

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

    % Retorno do resultado
    xy(k,:,1)=[x1 y1 z1];
    xy(k,:,2)=[x2 y2 z2];
end
