% Cinematica inversa do pendulo simples.
%
% theta=cinematInv2(xe,ye,L1,L2)
%
% Entrada: coordenadas da extremidade do pendulo (xe,ye) e dimensoes do
% elo (L).
% Saida: Angulos da junta (theta).

function theta=cinematInv(xe,ye,L)

% Verificaçao dos argumentos
if (nargin<2)
    error('MATLAB:badarg','Insufficient arguments.');
end
if (nargin<3)
    L=1;
end

% Distancia (ao quadrado) da origem ao ponto (xe,ye)
dist2=xe.^2+ye.^2;

% Existe soluçao para as coordenadas pedidas?
if (sum(dist2>L^2))
    error('MATLAB:nosolution','Impossibility to reach point.');
end

% Calculo da soluçao
theta=atan2(ye,xe);