function [r1, r2, r3] = direct_cine_3R(teta1, teta2, teta3, l1, l2, l3)

%DIRECT_CINE_3R   Cinemática directa 3R
%   [r1,r2,r3] = direct_cine_3R(t1,t2,t3,l1,l2,l3), devolve a posição cartesiana das juntas
%   dada a posição angular em que elas se encontram.
%   PARAMETROS DE ENTRADA:
%       teta1 é o angulo da junta 1 em relação à referencia
%       teta2 é o angulo da junta 2 em relação ao junta 1
%       teta3 é o angulo da junta 3 em relação ao junta 2
%       l1 é o comprimento do 1º segmento
%       l2 é o comprimento do 2º segmento
%       l3 é o comprimento do 3º segmento
%   ------------------------
%   PARAMETROS DE SAIDA:
%       r1 é o vector com posição cartesiana da junta 1
%       r2 é o vector com posição cartesiana da junta 2
%       r3 é o vector com posição cartesiana da junta 3


    r1(1,:) = l1.*cos(teta1);  % x1
    r1(2,:) = l1.*sin(teta1);  % y1
    
    r2(1,:) = l1.*cos(teta1)+l2.*cos(teta1+teta2);  % x2
    r2(2,:) = l1.*sin(teta1)+l2.*sin(teta1+teta2);  % y2
    
    r3(1,:) = l1.*cos(teta1)+l2.*cos(teta1+teta2)+l3*cos(teta1+teta2+teta3);  % x3
    r3(2,:) = l1.*sin(teta1)+l2.*sin(teta1+teta2)+l3*sin(teta1+teta2+teta3);  % y3