% Cálculo do centro de massa com massa nas posições das juntas e a
% inclinação da perna.
%
% [CoM,CoMproj,CoMfoot,theta,gama]=calcCoM(theta,gama)
%
% theta   -> Angulos das juntas (theta1,theta2,theta3)
% gama    -> Inclinação do plano (gama_x, gama_y)
% CoM     -> Centro de Massa real e projectado no pé segundo os mesmos
%            eixos (x,y,z) e projectado no pé considerando o plano xoy
%            coincidente com  o pé ([real; proj; projFoot])
%
% (c) Milton Ruas da Silva, 20/06/2006

function [CoM,theta,gama]=calccom(theta,gama)

theta1=theta(1)*pi/180;
theta2=pi/2-theta(2)*pi/180;
theta3=theta(3)*pi/180;

gama_x=gama(1)*pi/180;
gama_y=gama(2)*pi/180;

% Inclusão dos dados da perna
legdata;

% Alguns dados iniciais
h=footH;
l1=eloL;
l2=eloL;
r1=l1/2;
r2=l2/2;

% Cálculo dos Centros de massa na componente vertical e horizontal da perna
CoMvert=(m1*r1*sin(theta2-gama_x)+m2*(l1*sin(theta2-gama_x)+r2*sin(theta2+theta3-gama_x)))/(m1+m2);
CoMhorz=(m1*r1*cos(theta2-gama_x)+m2*(l1*cos(theta2-gama_x)+r2*cos(theta2+theta3-gama_x)))/(m1+m2);

% Centro de massa real
CoMreal=[ CoMvert*sin(theta1+gama_y)+h*sin(gama_y);
         -CoMhorz-h*sin(gama_x);
          CoMvert*cos(theta1+gama_y)+h*cos(gama_y)*cos(gama_x)];

% Cálculo do centro de massa projectado no pé (3D)
%   Vector perpendicular ao plano do pé
normal=[ sin(gama_y);
        -cos(gama_y)*sin(gama_x);
         cos(gama_y)*cos(gama_x)];
%   Sabendo que as coordenadas do ponto projectado sera da forma
%   (CoMx,CoMy,K*CoMz) determinemos K utilizando a equação do plano
%   a*x+b*y+c*z=0, com (a,b,c)=normal.
k=-(normal(1)*CoMreal(1)+normal(2)*CoMreal(2))/(normal(3)*CoMreal(3));
CoMproj=[CoMreal(1:2); k*CoMreal(3)];

% Cálculo do centro de massa no plano 2D do pé
Rx_=[1       0            0      ;          % Matriz de rotação inversa
     0 cos(-gama_x) -sin(-gama_x);          % no eixo xx
     0 sin(-gama_x)  cos(-gama_x)];
 
Ry_=[cos(-gama_y) 0  sin(-gama_y);          % Matriz de rotação inversa
         0        1       0     ;           % no eixo dos yy
     -sin(-gama_y) 0  cos(-gama_y)];

CoMfoot=Ry_*Rx_*CoMproj;            % Aplicação da rotação inversa
CoMfoot=-1.0*CoMfoot;               % Inversão dos eixos

% Matriz com as tres representações do centro de massa
CoM=[CoMreal CoMproj CoMfoot];