%função que vai desenhar o robo

% Objectos para representar sistemas de coordenadas
% de forma não ambígua. Possibilidade de usar um 
% objecto único para os 3 eixos.  V. Santos, Oct 2007.

close all;
clear all;
clc;

% Definição parcial de cada sub-objecto (eixo)
Xpart = [
 0  0.05   0.5   0.4  0.5  0.6   0.7  0.8  0.7   1     1    1.5 1   1     0.7  0.8  0.7  0.6  0.5  0.4  0.5  0.05
 0 -0.05  -0.05 -0.2 -0.2 -0.05 -0.2 -0.2 -0.05 -0.05 -0.2  0   0.2 0.05  0.05 0.2  0.2  0.05 0.2  0.2  0.05 0.05
 0  0      0     0    0    0     0     0   0     0      0   0   0   0     0      0    0    0   0    0    0    0
 ]*20;

Ypart = [
0 0.05 0.05 0.2 0.2 0.05 0.05 0.2 0   -0.2 -0.05 -0.05 -0.2 -0.2 -0.05 -0.05
0 0.05 0.6  0.7 0.8 0.7  1    1   1.5 1     1     0.7   0.8  0.7  0.6   0.05
0 0    0    0   0   0    0    0   0   0     0     0     0    0    0     0 
 ]*20;

Zpart = [
     0   0     0      0     0    0    0     0     0     0   0   0   0    0    0   0    0    0    0     0    0
     0  -0.05 -0.05  -0.2  -0.2 -0.1 -0.1  -0.05 -0.05 -0.2 0   0.2 0.05 0.05 0.2 0.2  0.1  0.1  0.05  0.05 0.05
     0   0.05  0.55   0.45  0.8  0.8  0.62  0.66  1     1   1.5 1   1    0.7  0.8 0.45 0.45 0.62 0.58  0.5  0.05
]*20; 
 
% Concatenação dos três objectos num só.
X = [ Xpart(1,:) Ypart(1,:) Zpart(1,:) ];
Y = [ Xpart(2,:) Ypart(2,:) Zpart(2,:) ];
Z = [ Xpart(3,:) Ypart(3,:) Zpart(3,:) ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Ilustração do desenho do 1º objecto (azul)
f = figure(1);
set(f,'Position',[1920/2 100 950 900],'CurrentPoint',[31 467]);
%fill3(X+70,Y,Z, 'b');
title('HUMANOIDE');xlabel('X'); ylabel('Y'); zlabel('Z');
grid on; hold on;
% Transformações:
P0 = [X;Y;Z; ones(1,size(X,2))]; %ones(1,size(X,2)) - matriz de 1's com tantos quanto numeros existem numa linha

t1=0;t2=0;t3=0;
TabelaI=[%%  t   l   d   a
    0 70 0 0
%
    0 -12.538 38.719 0
    0 0 0 90
    0 0 36.510 0 
    0 0 0 -90
    0 0 6.281 90
    90 0 0 0
    0 0 0 90
%
    t1 66.596 0 0
    90 0 0 0
    0 1.493 -0.058 0
    0 -1.493 0.058 0
    -90 0 0 0
    0 27.404 0 0
%
    t2 108 0 0
    90 0 0 0
    0 6.207 -1.561 0
    0 -6.207 1.561 0
    -90 0 0 0
    0 33.617 0 0
%
    t3 162.602 0 0
    90 0 0 0
    0 -6.222 -57.462 0
    -90 0 0 0
    0 100 0 0
    ];

%%%%%%%%%%%%%%%

t1=-20;t2=135;t3=-135;
TabelaF=[%%  t   l   d   a
    0 70 0 0
    0 -12.538 38.719 0
    0 0 0 90
    0 0 36.510 0
    0 0 0 -90
    0 0 6.281 90
    90 0 0 0
    0 0 0 90
%
    t1 66.596 0 0
    90 0 0 0
    0 1.493 -0.058 0
    0 -1.493 0.058 0
    -90 0 0 0
    0 27.404 0 0
%
    t2 108 0 0
    90 0 0 0
    0 6.207 -1.561 0
    0 -6.207 1.561 0
    -90 0 0 0
    0 33.617 0 0    
%
    t3 162.602 0 0
    90 0 0 0
    0 -6.222 -57.462 0
    -90 0 0 0
    0 100 0 0
    ];

%%%%%%%%%%%%%%%

K=20;

for t=1:size(TabelaI,1)
    Qi(t,1)=TabelaI(t,1);
    Qf(t,1)=TabelaF(t,1);
end

MQ=VectLinSpace(Qi,Qf,K);
DH=TabelaI;
MDH=GeraMultiDH(DH,MQ);
SHM=CalculaMovimentoRobo(MDH);

%%%%%%%%%%%%%%%

T=SHM(:,:,1,1);
P = T * P0;
%h(1,1) = fill3(P(1,:),P(2,:),P(3,:),'g');
Zeros = PZeros(SHM);
l(1,1) = line([70 Zeros(1,1,1)],[0 Zeros(1,2,1)],[0 Zeros(1,3,1)]);
set(l(1,1),'Color',[0 0 0]) %%[R G B]
set(l(1,1),'LineWidth',10)
for i=2:1:size(SHM,4)
   Tr=SHM(:,:,1,i);
   T = T * Tr;
   P = T * P0;
   %h(1,i)=fill3(P(1,:),P(2,:),P(3,:), 'g');
   axis equal;
   axis([-100 100 -120 120 0 500])
   l(1,i)=line([Zeros(i-1,1,1) Zeros(i,1,1)],[Zeros(i-1,2,1) Zeros(i,2,1)],[Zeros(i-1,3,1) Zeros(i,3,1)]);
   set(l(1,i),'Color',[0 0 0]) %%[R G B]
   set(l(1,i),'LineWidth',1)
end
%plot3(Zeros(size(Zeros,1),1,1),Zeros(size(Zeros,1),2,1),Zeros(size(Zeros,1),3,1),'*r')

%%%%%%%%%%%%%%%
%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];

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;


%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');

line([60 -60],[2 2],[0 0],'LineWidth',2,'Color',[1.0 1.0 0]);
line([60 -60],[-86 -86],[0 0],'LineWidth',2,'Color',[1.0 1.0 0]);
%%%%%%%%%%%%%%%

for j=2:1:size(SHM,3)
    T=SHM(:,:,j,1);
    P = T * P0;
    %set(h(1,1),'XData',P(1,:))
    %set(h(1,1),'YData',P(2,:))
    %set(h(1,1),'ZData',P(3,:))
    for i=2:1:size(SHM,4)
        Tr=SHM(:,:,j,i);
        T = T * Tr;
        P = T * P0;
        %set(h(1,i),'XData',P(1,:))
        %set(h(1,i),'YData',P(2,:))
        %set(h(1,i),'ZData',P(3,:))
        axis equal;
        axis([-100 100 -120 120 0 500])
        set(l(1,i),'Color',[0 0 0]) %%[R G B]
        set(l(1,i),'LineWidth',1)
        set(l(1,i),'XData',[Zeros(i-1,1,j) Zeros(i,1,j)])
        set(l(1,i),'YData',[Zeros(i-1,2,j) Zeros(i,2,j)])
        set(l(1,i),'ZData',[Zeros(i-1,3,j) Zeros(i,3,j)])
    end
    
    plot3(Zeros(size(Zeros,1)-14,1,j),...
        Zeros(size(Zeros,1)-14,2,j),...
        Zeros(size(Zeros,1)-14,3,j),'*r')
    
    plot3(Zeros(size(Zeros,1)-8,1,j),...
        Zeros(size(Zeros,1)-8,2,j),...
        Zeros(size(Zeros,1)-8,3,j),'*r')
    
    plot3(Zeros(size(Zeros,1)-2,1,j),...
        Zeros(size(Zeros,1)-2,2,j),...
        Zeros(size(Zeros,1)-2,3,j),'*r')
    
    %CG
    CentroMassa = [...
    Zeros(size(Zeros,1)-14,1,j)*PesoPonderadoCanela + ...
    Zeros(size(Zeros,1)-8,1,j)*PesoPonderadoCoxa + ...
    Zeros(size(Zeros,1)-2,1,j)*PesoPonderadoBaciaTronco ...
    ...
    Zeros(size(Zeros,1)-14,2,j)*PesoPonderadoCanela + ...
    Zeros(size(Zeros,1)-8,2,j)*PesoPonderadoCoxa + ...
    Zeros(size(Zeros,1)-2,2,j)*PesoPonderadoBaciaTronco ...
    ...
    Zeros(size(Zeros,1)-14,3,j)*PesoPonderadoCanela + ...
    Zeros(size(Zeros,1)-8,3,j)*PesoPonderadoCoxa + ...
    Zeros(size(Zeros,1)-2,3,j)*PesoPonderadoBaciaTronco ...
    ];

    plot3(CentroMassa(1,1),CentroMassa(1,2),CentroMassa(1,3),'ob');
    plot3(CentroMassa(1,1),CentroMassa(1,2),0,'og')
    pause(0.1)
    
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
