clc

%Determinacao de Algumas Variaveis
CGPeDireito=[57.462 -36.51 38.719 302];
CGPeEsquerdo=[-57.462  -36.51 38.719 302];
CGCanelaDireita=[57.404 -38.003 111.596 357];
CGCanelaEsquerda=[-57.404 -38.003 111.596 357];
CGCoxaDireita=[55.901 -42.717 292.383 352];
CGCoxaEsquerda=[-55.901 -42.717 292.383 352];
CGBaciaTronco=[0 -42.732 488.602 2493];

JuntaJoelhoDireito = [CGPeDireito(1,1) CGPeDireito(1,2) 45+139];
JuntaAncaDireita = [CGPeDireito(1,1) CGPeDireito(1,2) 45+139+142];

JuntaJoelhoEsquerdo = [CGPeEsquerdo(1,1) CGPeEsquerdo(1,2) 45+139];
JuntaAncaEsquerda = [CGPeEsquerdo(1,1) CGPeEsquerdo(1,2) 45+139+142];

PesoTotal = 302*2 + 357*2 + 352*2 + 2493;

PesoPonderadoPe = CGPeDireito(1,4) / PesoTotal;
PesoPonderadoCanela = CGCanelaDireita(1,4) / PesoTotal;
PesoPonderadoCoxa = CGCoxaDireita(1,4) / PesoTotal;
PesoPonderadoBaciaTronco = CGBaciaTronco(1,4) / PesoTotal;

CentroMassa = [...
    CGPeDireito(1,1)*PesoPonderadoPe + ...
    CGPeEsquerdo(1,1)*PesoPonderadoPe + ...
    CGCanelaDireita(1,1)*PesoPonderadoCanela + ...
    CGCanelaEsquerda(1,1)*PesoPonderadoCanela + ...
    CGCoxaDireita(1,1)*PesoPonderadoCoxa + ...
    CGCoxaEsquerda(1,1)*PesoPonderadoCoxa + ...
    CGBaciaTronco(1,1)*PesoPonderadoBaciaTronco ...
    ...
    CGPeDireito(1,2)*PesoPonderadoPe + ...
    CGPeEsquerdo(1,2)*PesoPonderadoPe + ...
    CGCanelaDireita(1,2)*PesoPonderadoCanela + ...
    CGCanelaEsquerda(1,2)*PesoPonderadoCanela + ...
    CGCoxaDireita(1,2)*PesoPonderadoCoxa + ...
    CGCoxaEsquerda(1,2)*PesoPonderadoCoxa + ...
    CGBaciaTronco(1,2)*PesoPonderadoBaciaTronco ...
    ...
    CGPeDireito(1,3)*PesoPonderadoPe + ...
    CGPeEsquerdo(1,3)*PesoPonderadoPe + ...
    CGCanelaDireita(1,3)*PesoPonderadoCanela + ...
    CGCanelaEsquerda(1,3)*PesoPonderadoCanela + ...
    CGCoxaDireita(1,3)*PesoPonderadoCoxa + ...
    CGCoxaEsquerda(1,3)*PesoPonderadoCoxa + ...
    CGBaciaTronco(1,3)*PesoPonderadoBaciaTronco ...
    ]

%DesenhoPernasHumanoide
figure(2)
hold on;grid on,
plot3(CentroMassa(1,1),CentroMassa(1,2),CentroMassa(1,3),'or')
plot3(CentroMassa(1,1),CentroMassa(1,2),CentroMassa(1,3),'*r')
    %PE
    plot3(60,2,0,'or');plot3(60,-86,0,'or');        line([60 60],[2 -86],[0 0],'LineWidth',2,'Color',[1.0 0 0]);
    plot3(-60,2,0,'or');plot3(-60,-86,0,'or');      line([-60 -60],[2 -86],[0 0],'LineWidth',2,'Color',[1.0 0 0]);
    plot3(60,-42,0,'*b');plot3(-60,-42,0,'*b');     line([60 60],[-42 -42],[0 45],'LineWidth',2);
    plot3(60,-42,45,'*b');plot3(-60,-42,45,'*b');   line([-60 -60],[-42 -42],[0 45],'LineWidth',2);
    plot3(CGPeDireito(1,1),CGPeDireito(1,2),CGPeDireito(1,3),'or');
    plot3(CGPeEsquerdo(1,1),CGPeEsquerdo(1,2),CGPeEsquerdo(1,3),'or');
    %CANELA
    plot3(60,-42,45+139,'*b');plot3(-60,-42,45+139,'*b');
    line([-60 -60],[-42 -42],[45 45+139],'LineWidth',2);
    line([60 60],[-42 -42],[45 45+139],'LineWidth',2);
    plot3(CGCanelaDireita(1,1),CGCanelaDireita(1,2),CGCanelaDireita(1,3),'or');
    plot3(CGCanelaEsquerda(1,1),CGCanelaEsquerda(1,2),CGCanelaEsquerda(1,3),'or');
    %COXA
    plot3(60,-42,45+139+142,'*b');plot3(-60,-42,45+139+142,'*b');
    line([-60 -60],[-42 -42],[45 45+139+142],'LineWidth',2);
    line([60 60],[-42 -42],[45 45+139+142],'LineWidth',2);
    plot3(CGCoxaDireita(1,1),CGCoxaDireita(1,2),CGCoxaDireita(1,3),'or');
    plot3(CGCoxaEsquerda(1,1),CGCoxaEsquerda(1,2),CGCoxaEsquerda(1,3),'or');
    %ANCA
    line([-60 60],[-42 -42],[45+139+142 45+139+142],'LineWidth',2)
    plot3(CGBaciaTronco(1,1),CGBaciaTronco(1,2),CGBaciaTronco(1,3),'or')
hold off;

%Posição do CG da Canela ao longo do tempo
figure(2);
hold on;
    %em x
    PosicaoCGCanelaDireita(:,1) = CGCanelaDireita(1,1);
    PosicaoJoelhoDireito(:,1) = JuntaJoelhoDireito(1,1);
    %em y
    for linha=1:size(AngTorDir.signals.values,1)
        PosicaoCGCanelaDireita(linha,2) = ...
            CGCanelaDireita(1,2) + ...
            CGCanelaDireita(1,3) * sin(-AngTorDir.signals.values(linha,1)*2*pi/360);
        PosicaoJoelhoDireito(linha,2) = ...
            JuntaJoelhoDireito(1,2) + ...
            JuntaJoelhoDireito(1,3) * sin(-AngTorDir.signals.values(linha,1)*2*pi/360);
    end
    %em z
    for linha=1:size(AngTorDir.signals.values,1)
        PosicaoCGCanelaDireita(linha,3) = ...
            CGCanelaDireita(1,3) * cos(AngTorDir.signals.values(linha,1)*2*pi/360);
        PosicaoJoelhoDireito(linha,3) = ...
            JuntaJoelhoDireito(1,3) * cos(AngTorDir.signals.values(linha,1)*2*pi/360);
    end
    
    plot3(PosicaoCGCanelaDireita(:,1),PosicaoCGCanelaDireita(:,2),PosicaoCGCanelaDireita(:,3),'-b','LineWidth',2);
    plot3(PosicaoJoelhoDireito(:,1),PosicaoJoelhoDireito(:,2),PosicaoJoelhoDireito(:,3),'-b','LineWidth',2);
    
%Posição do CG da Coxa ao longo do tempo
figure(2);
hold on;
    %em x
    PosicaoCGCoxaDireita(:,1) = CGCoxaDireita(1,1);
    PosicaoAncaDireita(:,1) = JuntaAncaDireita(1,1);
    %em y
    for linha=1:size(AngJoeDir.signals.values,1)
        PosicaoCGCoxaDireita(linha,2) = ...
            PosicaoJoelhoDireito(linha,2) + ...
            (CGCoxaDireita(1,3)-JuntaJoelhoDireito(1,3)) * sin(-AngJoeDir.signals.values(linha,1)*2*pi/360);
        PosicaoAncaDireita(linha,2) = ...
            PosicaoJoelhoDireito(linha,2) + ...
            (JuntaAncaDireita(1,3)-JuntaJoelhoDireito(1,3)) * sin(-AngJoeDir.signals.values(linha,1)*2*pi/360);
    end
    %em z
    for linha=1:size(AngTorDir.signals.values,1)
        PosicaoCGCoxaDireita(linha,3) = ...
            PosicaoJoelhoDireito(linha,3) + ...
            (CGCoxaDireita(1,3) - JuntaJoelhoDireito(1,3)) * cos(AngJoeDir.signals.values(linha,1)*2*pi/360);
        PosicaoAncaDireita(linha,3) = ...
            PosicaoJoelhoDireito(linha,3) + ...
            (JuntaAncaDireita(1,3) - JuntaJoelhoDireito(1,3)) * cos(AngJoeDir.signals.values(linha,1)*2*pi/360);
    end
    
    plot3(PosicaoCGCoxaDireita(:,1),PosicaoCGCoxaDireita(:,2),PosicaoCGCoxaDireita(:,3),'-b','LineWidth',2);
    plot3(PosicaoAncaDireita(:,1),PosicaoAncaDireita(:,2),PosicaoAncaDireita(:,3),'-b','LineWidth',2);

    
%Posição do CG da BaciaTronco ao longo do tempo
figure(2);
hold on;
    %em x
    PosicaoCGBaciaTronco(:,1) = CGBaciaTronco(1,1);
    %em y
    for linha=1:size(AngAncDir.signals.values,1)
        PosicaoCGBaciaTronco(linha,2) = ...
            PosicaoAncaDireita(linha,2) - ...
            (CGBaciaTronco(1,3)-JuntaAncaDireita(1,3)) * sin(+AngAncDir.signals.values(linha,1)*2*pi/360);
    end
    %em z
    for linha=1:size(AngAncDir.signals.values,1)
        PosicaoCGBaciaTronco(linha,3) = ...
            PosicaoAncaDireita(linha,3) - ... %este valor vai diminuindo smp positivo
            (CGBaciaTronco(1,3) - JuntaAncaDireita(1,3)) * cos(-AngAncDir.signals.values(linha,1)*2*pi/360);
    end
    
    plot3(PosicaoCGBaciaTronco(:,1),PosicaoCGBaciaTronco(:,2),PosicaoCGBaciaTronco(:,3),'-g','LineWidth',2);
    
    
    