%                   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
%
%%INV_CINE_3D   Cinemática inversa 2R_3D especial
%   [teta1, teta2, teta3] = inv_cine_3D(x,y,z,l1,l2,c), devolve a posição angular 
%   das juntas no sentido de atingir a posição (x,y,z).
%   PARAMETROS DE ENTRADA:
%       x é a abcissa da posição pretendida
%       y é a ordenada da posição pretendida
%       z é a ordenada da posição pretendida
%       l1 é l2 é o comprimento do braço respectivo
%       c é a indicação do cotovelo para cima ou para baixo
%   ------------------------
%   PARAMETROS DE SAIDA:
%       teta1 é o angulo vertical da junta 1 em relação à referencia
%       teta2 é o angulo horizontal da junta 1 em relação à referencia
%       teta3 é o angulo da junta 2 em relação ao junta 1


function [teta1, teta2, teta3] = inv_cine_3D(x,y,z,l1, l2, c)

     if(abs(x)<1e-4 && abs(y)<1e-4)
         x1 = 0;
     else
         x1 = sqrt(x.^2+y.^2);   % comprimento do braço projectado no plano x0y
     end
     
    % para teta2
    if(x1 < 1e-4)
        teta2 = pi/2;
    elseif(y>=0)
        teta2 = asin(y/x1);
    else
        teta2 = -asin(y/x1);
    end
    % para teta1 e teta3
    [teta1, teta3] = inv_cine(x1, z, l1, l2, c);