function CM = centro_massa_3R(TETA, M, L, R)

%CENTRO_MASSA   devolve o centro de massa de um corpo
%   CM = centro_massa(TETA, M, L, R)
%   PARAMETROS DE ENTRADA:
%       TETA = [teta1, teta2, teta3]
%           teta1, teta2, teta3 -> posição angular das juntas
%       M -> massas dos braços
%       L -> comprimentos dos braços
%       R -> distancias das juntas ao centro de massa de cada braço
%   ------------------------
%   PARAMETROS DE SAIDA:
%       CM -> posição cartesiana do centro de massa

l1 = L(1); l2 = L(2); l3 = L(3);
r1 = R(1); r2 = R(2); r3 = R(3);
m1 = M(1); m2 = M(2); m3 = M(3);
Mt = m1+m2+m3;

teta1 = TETA(1,:); teta2 = TETA(2,:); teta3 = TETA(3,:);


% centro de massa no segmento 1
x1 = r1.*cos(teta1);    
y1 = r1.*sin(teta1);    

% centro de massa no segmento 2
x2 = l1.*cos(teta1)+r2.*cos(teta1+teta2);
y2 = l1.*sin(teta1)+r2.*sin(teta1+teta2);

% centro de massa no segmento 3
x3 = l1.*cos(teta1)+l2.*cos(teta1+teta2)+r3.*cos(teta1+teta2+teta3);
y3 = l1.*sin(teta1)+l2.*sin(teta1+teta2)+r3.*sin(teta1+teta2+teta3);

% calculo do centro de massa do conjunto de segmentos
CM = (x1.*m1 + x2.*m2 + x3.*m3)./Mt;        % abcissa do centro de massa
CM(2,:) = (y1.*m1 + y2.*m2 + y3.*m3)./Mt;   % ordenada do centro de massa
