% Cinematica directa do duplo pendulo.
%
% [x1,x2,y1,y2]=cinematDir2(theta1,theta2,L1,L2)
%
% Entrada: Angulos de cada junta (theta1 e theta2) e dimensoes dos
% elos (L1 e L2).
% Saida: Coordenadas cartesianas de cada junta
% (junta1:(x1,y1), junta2:(x2,y2)).
%
% Esta funçao esta preparada para receber listas de pontos.
% Exemplo: cinematDir2([theta11;theta12;...],[theta21;theta22;...],L1,L2)
%          -> x1=[x1(theta11,theta21), x1(theta12,theta22), ...]
%          -> x2=[x2(theta11,theta21), x2(theta12,theta22), ...]
%          -> y1=[y1(theta11,theta21), y1(theta12,theta22), ...]
%          -> y2=[y2(theta11,theta21), y2(theta12,theta22), ...]

function [x1,x2,y1,y2]=cinematDir2(theta1,theta2,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

% Coordenadas cartesianas da extremidade de cada elo
x1=L1*cos(theta1);
x2=x1+L2*cos(theta1+theta2);
y1=L1*sin(theta1);
y2=y1+L2*sin(theta1+theta2);
