%                   Universidade de Aveiro
%                               -
%       Departamento de Electrónica e Telecomunicações
%
% Projecto 5ºAno - Desenvolviento de Algoritmos de Controle para um Robô
% Humanoide
%
% Pedro Miguel Ferreira     
% Nº Mec - 27593
%
% Data - 29-01-2007
%
%%DIRECT_CINE_3D     Cinemática directa 3D
%   [r1,r2] = direct_cine_3D(t1,t2,t3,l1,l2), devolve a posição cartesiana das juntas
%   dada a posição angular em que elas se encontram. (caso particular)
%   PARAMETROS DE ENTRADA:
%       teta1 é o angulo da junta 1 na direcção do eixo x
%       teta2 é o angulo da junta 1 na direcção do eixo y
%       teta3 é o angulo da junta 2 em relação ao junta 1
%       l1 é o comprimento do 1º segmento
%       l2 é o comprimento do 2º segmento
%   ------------------------
%   PARAMETROS DE SAIDA:
%       r1 é o vector com posição cartesiana da junta 1
%       r2 é o vector com posição cartesiana da junta 2


function [r1, r2] = direct_cine_3D(teta1, teta2, teta3, l1, l2)
    

    r1(1,:) = l1.*sin(teta1).*cos(teta2);   % x1
    r1(2,:) = l1.*sin(teta2).*cos(teta1);   % y1
    r1(3,:) = l1.*cos(teta1).*cos(teta2);   % z1
          
    
    r2(1,:) = l1.*sin(teta1).*cos(teta2) + l2.*sin(teta1+teta3);  % x2
    r2(2,:) = l1.*sin(teta2).*cos(teta1) + l2.*cos(teta1+teta3).*sin(teta2);  % y2
    r2(3,:) = l1.*cos(teta1).*cos(teta2) + l2.*cos(teta1+teta3).*cos(teta2);  % z2
    
    
%     r1(1,:) = l1.*cos(teta1).*sin(teta2);   % y1
%     r1(2,:) = l1.*cos(teta1).*cos(teta2);   % x1
%     r1(3,:) = l1.*sin(teta1);   % z1
%     
%         
%     r2(1,:) = l1.*cos(teta1).*sin(teta2) + l2.*cos(teta1+teta3).*sin(teta2);  % y2
%     r2(2,:) = l1.*cos(teta1).*cos(teta2) + l2.*cos(teta1+teta3).*cos(teta2);  % x2
%     r2(3,:) = l1.*sin(teta1) + l2.*sin(teta1+teta3);  % z2