% Cinematica inversa do duplo pendulo.
%
% [theta1,theta2]=cinematInv2(xe,ye,L1,L2)
%
% Entrada: coordenadas da extremidade do pendulo (xe,ye) e dimensoes dos
% elos (L1 e L2).
% Saida: Angulos das juntas [theta1,theta2].
%
% Esta funçao esta preparada para receber listas de pontos.
% Exemplo: cinematInv2([x1,x2,...],[y1,y2,...],L1,L2)
%          -> theta1=[theta1(x1,y1) theta1(x2,y2) ...]
%          -> theta2=[theta2(x1,y1) theta2(x2,y2) ...]

function [theta1,theta2]=cinematInv2(xe,ye,L1,L2)

% Verificaçao dos argumentos
if (nargin<2)
    error('MATLAB:badarg','Insufficient arguments.');
end
if (nargin<4)
    L2=1;
end
if (nargin<3)
    L1=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<(L1-L2)^2) || sum(dist2>(L1+L2)^2))
    error('MATLAB:nosolution','Impossibility to reach point.');
end

% Calculo da soluçao
k1=2*ye*L1;
k2=2*xe*L1;
k3=dist2+L1^2-L2^2;

kk1=0;
kk2=2*L1*L2;
kk3=dist2-L1^2-L2^2;

% Considera-se theta2 sempre positivo ---------------------------------\
theta1=atan2(k1,k2)-atan2(sqrt(k1.^2+k2.^2-k3.^2),k3);          % Sinal -
theta2=atan2(kk1,kk2)+atan2(sqrt(kk1.^2+kk2.^2-kk3.^2),kk3);    % Sinal +
