% Calcula a cinemática inversa de uma perna, a partir das coordenadas (x,y)
% da base do pé
% As coordenadas (x,y) usam como origem o solo.
% Assim a coordenada x define o deslocamento na horizontal e a
% coordenada y representa a altura do pé relativamente ao solo.

function [theta,error,errorstr]=cinematinv(xy,L)

% Validação do argumento
if (size(xy,2)~=2)
    error('Argumento inválido!');
end

% Inicialização dos parametros de saída
theta=[0 0];
error=0;
errorstr='OK';

% Número de linhas de (x,y)
len=size(xy,1);

% Propriedades da perna
L1=L(3);                  % Junta inferior ao joelho
L2=L(4);                  % Joelho a anca

% Conversão das coordenadas usando como origem o topo da anca
x=xy(:,1);              % Coordenada x
y=-L1-L2+xy(:,2);       % Coordenada y

% Distancia ao quadrado da distancia a origem das coordenadas (x,y)
d2=x.^2+y.^2;

% Validação das coordenadas solicitadas
if (d2>(L1+L2)^2)
    error=1;
    errorstr='Coordenadas inválidas!';
    return;
end

% Parametros para cálculo dos angulos
% Cada linha diz respeito a um conjunto de coordenadas (x,y),
% e cada coluna diz respeito a uma junta (angulo)
K1=2*y*L1;
K2=2*x*L1;
K3=d2+L1^2-L2^2;

k1=zeros(len,1);
k2=ones(len,1)*2*L1*L2;
k3=d2-L1^2-L2^2;

% Angulo entre a anca e o elo superior
theta1=atan2(K1,K2)+atan2(sqrt(K1.^2+K2.^2-K3.^2),K3);
% Angulo entre o elo superior e o inferior
theta2=atan2(k1,k2)-atan2(sqrt(k1.^2+k2.^2-k3.^2),k3);

% Angulo de retorno
theta=[theta1 theta2]*180/pi;
