% Desenho de uma perna do robot
%
% fall=drawleg(theta,gama,CoM)
%
% Parametros de entrada
%  theta -> Posição de cada uma das três juntas (theta1,theta2,theta3)
%  gama  -> Inclinação do plano onde está assente o pé (gama_x, gama_y)
%  CoM   -> Centro de Massa real, projectado no pé e projectado no pé com o
%           plano xoy coincidente (CoMreal,CoMproj,CoMfoot)
%  fall  -> Flag indicativa de queda da perna (sempre que o centro de massa
%           projectado no pé ultrapassa o domínio do pé, a queda ocorre)
%
% (c) Milton Ruas da Silva, 20/06/2006

function fall=drawleg(theta,gama,CoM)

% Flag de queda inicializada a falso
fall=0;

CoMreal=CoM(:,1);       % Centro de massa real
CoMproj=CoM(:,2);       % Centro de massa projectado no pé
CoMfoot=CoM(:,3);       % Centro de massa projectado no plano do pé

theta1=theta(1);
theta2=theta(2);
theta3=theta(3);

gama_x=gama(1);
gama_y=gama(2);

% Inclusão dos dados da perna
legdata;

% Desenho do plano de apoio do pé
% [X,Y]=meshgrid([-0.7:0.1:0.7]*footL);
% Z=zeros(size(X))-footH/10;
% h=mesh(X,Y,Z);
% rotate(h,[1 0 0],gama_x,[0 0 0])
% rotate(h,[0 1 0],gama_y,[0 0 0])

% h=fill3([-1 1 1 -1]*0.7*footL,[1 1 -1 -1]*0.7*footL,[0 0 0 0],'w');
% rotate(h,[1 0 0],gama_x,[0 0 0])
% rotate(h,[0 1 0],gama_y,[0 0 0])
 

% Desenho da base da perna (pé)
h=fill3([-1/2 1/2 1/2 -1/2]*footW,[1/2 1/2 -1/2 -1/2]*footL,[0 0 0 0],'r');
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

hold on

% Desenho do topo da perna
h=fill3([-1/2 1/2 1/2 -1/2]*topW,[-1/2 -1/2 1/2 1/2]*topL,[0 0 0 0]+footH+2*eloL+topH,'w');
rotate(h,[1 0 0],-theta3,[0,0,footH+eloL]);
rotate(h,[1 0 0],theta2,[0,0,footH])
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

% Desenho da perna superior
h=fill3([-1/2 1/2 1/2 -1/2]*eloW,[0 0 0 0],[1 1 0 0]*eloL+eloL+footH,'y');
rotate(h,[1 0 0],-theta3,[0,0,footH+eloL]);
rotate(h,[1 0 0],theta2,[0,0,footH])
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

% Desenho da perna inferior
h=fill3([-1/2 1/2 1/2 -1/2]*eloW,[0 0 0 0],[1 1 0 0]*eloL+footH,'g');
rotate(h,[1 0 0],theta2,[0,0,footH])
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

% Desenho da junta da anca
h=line([-1/2 1/2]*eloW,[0 0],[0 0]+footH+2*eloL,'color','k','linewidth',2);
rotate(h,[1 0 0],-theta3,[0,0,footH+eloL]);
rotate(h,[1 0 0],theta2,[0,0,footH])
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

% Desenho da junta do joelho
h=line([-1/2 1/2]*eloW,[0 0],[0 0]+footH+eloL,'color','k','linewidth',2);
rotate(h,[1 0 0],theta2,[0,0,footH])
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

% desenho da junta do pé
h=line([-1/2 1/2]*eloW,[0 0],[0 0]+footH,'color','k','linewidth',2);
rotate(h,[0 1 0],theta1,[0,0,footH])
rotate(h,[1 0 0],gama_x,[0 0 0])
rotate(h,[0 1 0],gama_y,[0 0 0])

if (nargin>=3)
    plot3(CoMreal(1),CoMreal(2),CoM(3),'o')
    
    if (abs(CoMfoot(1))<=footW/2 && abs(CoMfoot(2))<=footL/2)
        plot3(CoMproj(1),CoMproj(2),CoMproj(3),'x')
        plot3([CoMreal(1) CoMproj(1)],[CoMreal(2) CoMproj(2)],[CoMreal(3) CoMproj(3)],'-.');
    else
        % Flag de queda
        fall=1;
    end
end

hold off

% Definição da vista
view(-37.5,10)

% Configurar eixos
axis('off')
axis('equal')