%% PROGRAMA PARA A SIMULACAO DO MOVIMENTO DO TRONCO
  close all
  clear all
  clc
  
  AAi=30;           %Altura Inicial da Anca
  AAM=85;           %Altura Anca Maxima
  EAA=5;            %EncrementoAlturaAnca
  AngTronco = 45;   %AnguloMaximoJoelho
  NPS = 10;         %NumeroPontosSimulacao
  
  j=1;
  AA=AAi;  
  fim=0;
  
  while fim<1
    %% ConfiguracaoFiguras
    figura1 = figure(1);
        set(figura1,'position',[1350 100 560 900])
        hold on; grid on;axis equal
        Title('Animação - Inclinação Tronco de 90º')
        %axis([-500 500 0 700]);
    figura2 = figure(2);
        hold on; grid on;
        set(figura2,'position',[810 760 530 240])
        axis([0 100 AAi AAM]);
        Title('ComprimentoElastico em funcao do AnguloJoelho')
    %figura3 = figure(3);
    %    hold on; grid on;
    %    set(figura3,'position',[810 430 530 240])
    %    axis([0 135 0 100]);
    %    Title('ForcaElastico em funcao do AnguloJoelho')
    %figura4 = figure(4);
    %    hold on; grid on;
    %    set(figura4,'position',[810 100 530 240])
    %    axis([0 135 0 2]);
    %    Title('BinarioElastico em funcao do AnguloJoelho')
    %% ParametrosEstrutura
    AT    = 45;              %AlturaTornozelo
    ACa   = 139;             %AlturaCanela
    AJ    = AT+ACa;          %AlturaJoelho
    ACo   = 142;             %AlturaCoxa
    AATr  = 68+250;          %AlturaAnca+Tronco
    AA    = AA;              %AltraAnca
    ATr   = AATr-AA;         %AlturaTronco
    ACMTr = 2*ATr/3;             %AlturaCentroMassaTronco(em relacao a ATr)
    ACMPA = 180;             %AlturaCentroMassaPernasAnca(em relacao ao solo)
    %% Constantes
    ConvDeg2Rad = (2*pi)/360;   %ConversaoGrausParaRadianos
    ConvRad2Deg = 360/(2*pi);
    Vermelho = [1 0 0];
    Verde    = [0 1 0];
    Azul     = [0 0 1];
    Preto    = [0 0 0];
    Amarelo  = [1 1 0];
    TS = 10;                    %TempoSimulacao
    NPS = NPS;                  %NumeroPontosSimulacao
    AngTronco = AngTronco;      %AnguloMaximoJoelho
    VTS = linspace(0,TS,NPS);   %VectorTempoSimulacao(s)
    VAngTronco = linspace(0,AngTronco,NPS); %AnguloJoelho(deg)
    ConvMm2M=1/1000;
    %% DesenhoPe
    figure(1)
      line([0 95],[0 0],'LineWidth',4,'Color',Azul);
      plot(55,0,'+r')
    %% DesenhoTornozelo
    figure(1)
      line([0 0],[0 AT],'LineWidth',4,'Color',Verde);
      plot(0,AT,'*r')
    %% DesenhosCanela
    figure(1)
      line([0 0],[AT AJ],'LineWidth',4,'Color',Vermelho);
      plot(0,AJ,'*r')
    %% DesenhosCoxa
    figure(1)
      line([0 0],[AJ ACo+AJ],'LineWidth',4,'Color',Azul);
      plot(0,ACo+AJ,'*r')
    %% DesenhoAnca
    figure(1)
    line([0 0],[ACo+AJ ACo+AJ+AA],'LineWidth',4,'Color',Verde);
    plot(0,ACo+AJ+AA,'*r')
    %% DesenhoTronco
    figure(1)
    h(1,1)=line([0 0],[ACo+AJ+AA ACo+AJ+AA+ATr],'LineWidth',4,'Color',Amarelo);
    plot(0,ACo+AJ+AA+ATr,'*r')
    %% DesenhoCentroMassaTronco
    figure(1)
    h(1,2)=plot(0,ACo+AJ+AA+ACMTr,'ok');
    %% DesenhoCentroMassaPernasAnca
    figure(1)
    plot(0,ACMPA,'ok');  
    %% DesenhoCentroMassaEstrutura
    figure(1)
    h(1,3)=plot(0,(ACo+AJ+AA+ACMTr+ACMPA)/2,'ok');
    %% DesenhoProjeccaoCentroMassaEstrutura
    figure(1)
    h(1,4)=plot(0,0,'ok');
    %% CICLO ACTUALIZACAO IMAGEM %%
    for i=1:1:NPS
    XDataCabeca = ATr*sin(VAngTronco(1,i)*ConvDeg2Rad); %NovaPosicaoXExtremidadeTronco
    YDataCabeca = ATr*cos(VAngTronco(1,i)*ConvDeg2Rad); %NovaPosicaoYExtremidadeTronco
    set(h(1,1),'XData',[0 XDataCabeca]);
    set(h(1,1),'YData',[ACo+AJ+AA ACo+AJ+AA+YDataCabeca]);
    plot(XDataCabeca,ACo+AJ+AA+YDataCabeca,'.r')
    XDataCMTronco = ACMTr*sin(VAngTronco(1,i)*ConvDeg2Rad); %NovaPosicaoXCMTronco
    YDataCMTronco = ACMTr*cos(VAngTronco(1,i)*ConvDeg2Rad); %NovaPosicaoYCMTronco
    set(h(1,2),'XData',XDataCMTronco);
    set(h(1,2),'YData',ACo+AJ+AA+YDataCMTronco);
    XDataCMEstrutura = (0+XDataCMTronco)/2;                %NovaPosicaoXCMEstrutura
    YDataCMEstrutura = (ACMPA+ACo+AJ+AA+YDataCMTronco)/2;  %NovaPosicaoYCMEstrutura
    set(h(1,3),'XData',XDataCMEstrutura);
    set(h(1,3),'YData',YDataCMEstrutura);
    plot(XDataCMEstrutura,YDataCMEstrutura,'.r')
    XDataPCMEstrutura = XDataCMEstrutura; %NovaPosicaoXPCMEstrutura
    YDataPCMEstrutura = 0;                %NovaPosicaoYPCMEstrutura
    set(h(1,4),'XData',XDataPCMEstrutura);
    set(h(1,4),'YData',0);
    pause(0.001)
    end
    AA=AA+EAA;
    if AA>=AAM
        fim=1;
    end
    PCMEstrutura(j,1)=AA;
    PCMEstrutura(j,2)=XDataPCMEstrutura;
    j=j+1;
    figure(2)
    plot(PCMEstrutura(:,2),PCMEstrutura(:,1))
    plot(PCMEstrutura(:,2),PCMEstrutura(:,1),'r*')
  end

PCMEstrutura
Maximo = max(PCMEstrutura(:,2));
Minimo = min(PCMEstrutura(:,2));
Diferenca = PCMEstrutura(1,2)-PCMEstrutura(j-1,2);
Resultados= ['     Max     ' '     Min     ' '     Dif     ']
Resultados= [Maximo Minimo Diferenca]




