% Desenho de uma perna do robot
%
% drawleg(theta,gama)
%
% Parametros de entrada
%  theta: (theta1,theta2,theta3) -> Posição de cada uma das três juntas
%  gama:  (gama_x, gama_y) -> Inclinação do plano onde está assente o pé

function drawleg(theta,gama)

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

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

% Inclusão dos dados da perna
legdata;

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

hold on
% 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])

% 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],90.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],90.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],90.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',3);
rotate(h,[1 0 0],-theta3,[0,0,footH+eloL]);
rotate(h,[1 0 0],90.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',3);
rotate(h,[1 0 0],90.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',3);
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])

hold off

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

axis('off')
axis('equal')