% Desenho do duplo pendulo
%
% pendulo2_draw(x1,y1,x2,y2,win_width)
%
% Entradas:
% (x1,y1) - Coordenadas da segunda junta
% (x2,y2) - Coordenadas da extremidade do pendulo
% win_width - Dimensao da janela grafica

function leg_draw(theta)

footdata;

% Largura da janela gráfica
win_width=sum(L);

% Cálculo das coordenadas das juntas
xyz=cinematdir2(theta);

% Translação das coordenadas, de modo a ter a anca na origem
for i=1:size(xyz,3)
    xyz(1,:,i)=xyz(1,:,i)-xyz(1,:,5);
end

% Junta do Joelho
y1=xyz(1,2,4);
z1=xyz(1,3,4);

% Junta superior do pé
y2=xyz(1,2,3);
z2=xyz(1,3,3);

% Junta inferior do pé
y3=xyz(1,2,2);
z3=xyz(1,3,2);

% Base do pé
y4=xyz(1,2,1);
z4=xyz(1,3,1);

% Desenho dos elos da perna
plot([0,y1],[0,z1],'r',[y1,y2],[z1,z2],'b',[y2,y3],[z2,z3],'k', ...
     0,0,'ro', y1,z1,'bo', y2,z2,'bs','linewidth',3);
hold on

% Desenho do pé
plot([y3,y4],[z3,z4],'k', [y4-1.0*L(3),y4+1.0*L(3)],[z4,z4],'k','linewidth',3);

% Desenho do solo
plot([-win_width,win_width],-sum(L)*[1,1],'g','linewidth',1);

hold off

% Definiçao da gama dos eixos
axis('equal'), axis(win_width*[-1.1 1.1 -1.1 0.1]);
grid on