% 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 xyz=cinematdir2(theta,ref)

if (nargin<2)
    ref=[0 0 0];
end

footdata;

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

theta=theta*pi/180;

N=size(theta,1);
xyz=zeros(N,3,5);

T=zeros(4,4,5);

for k=1:N
    
    theta1=theta(k,1);
    theta2=theta(k,2);
    theta3=theta(k,3);
    
    % Matrizes de transformação geométrica

    % Elo superior L3
    T(:,:,5)=[ 1  0            0            0             ;
               0  cos(theta3) -sin(theta3) -L3*sin(theta3);
               0  sin(theta3)  cos(theta3)  L3*cos(theta3);
               0  0            0            1             ];
    
    % Elo inferior L2
    T(:,:,4)=[ 1  0            0            0             ;
               0  cos(theta2) -sin(theta2) -L2*sin(theta2);
               0  sin(theta2)  cos(theta2)  L2*cos(theta2);
               0  0            0            1             ];
           
    % Elo de inclinação lateral L1
    T(:,:,3)=[ cos(theta1) 0  sin(theta1)  L1*sin(theta1);
               0           1  0            0             ;
              -sin(theta1) 0  cos(theta1)  L1*cos(theta1);
               0           0  0            1             ];
           
    % Elo L0
    T(:,:,2)=[1 0 0 0 ;
              0 1 0 0 ;
              0 0 1 L0;
              0 0 0 1 ];

    % MAtriz identidade
    T(:,:,1)=eye(4);
    
    M=eye(4);
    for j=1:5
        
        % Matriz de transformação geom+etrica
        M=M*T(:,:,j);
        
        % Cálculo da transformação geométrica
        res=M*[ref'; 1];
        % Normalização das coordenadas
        res=res(1:3)/res(4);
        
        % Armazenamento do resultado
        xyz(k,:,j)=res';
    end
end