%                   Universidade de Aveiro
%                               -
%       Departamento de Electrónica e Telecomunicações
%
% Projecto 5ºAno - Desenvolviento de Algoritmos de Controle para um Robô
% Humanoide
%
% Pedro Miguel Ferreira     
% Nº Mec - 27593
%
% Data - 29-01-2007
%
%%DESENHAR_2R_3D   Faz o desenho do robô com 1 perna - Caso particular
%   desenhar_2R_3D(x, l1, l2)
%   PARAMETROS DE ENTRADA:
%       x é a matriz com a posição cartesiana das 2 juntas
%       l1 é o comprimento do 1º segmento
%       l2 é o comprimento do 2º segmento

function desenhar_2R_3D(x, l1, l2)

projeccao_2R_2D(x, l1, l2);

subplot(2, 2, 4);
HP = x(3,3);        % altura do pé
CP = (l1+l2)/1.4;   % metade do comprimento do pé
LP = CP/2;          % metade da largura do pé

% x contêm uma matriz 3xN, em que N é o número de segmentos e cada linha
% representa a coordenada x, y, z, respectivamente.

pe(:,1) = [x(1,end)-LP; x(2,end)-CP; x(3,end)];   % canto superior esquerdo
pe(:,end+1) = [x(1,end)-LP; x(2,end)+CP; x(3,end)];   %   canto superior esquerdo
pe(:,end+1) = [x(1,end)+LP; x(2,end)+CP; x(3,end)];   %   canto superior direito
pe(:,end+1) = [x(1,end)+LP; x(2,end)-CP; x(3,end)];   %   canto superior direito
pe(:,end+1) = [x(1,end)-LP; x(2,end)-CP; x(3,end)];   %   superficie superior 
pe(:,end+1) = [x(1,end)-LP; x(2,end)-CP; x(3,end)-HP];    % canto inferior direito
pe(:,end+1) = [x(1,end)-LP; x(2,end)+CP; x(3,end)-HP];    %   canto inferior esquerdo
pe(:,end+1) = [x(1,end)+LP; x(2,end)+CP; x(3,end)-HP];    %   canto inferior direito
pe(:,end+1) = [x(1,end)+LP; x(2,end)-CP; x(3,end)-HP];    %   canto inferior esquerdo
pe(:,end+1) = [x(1,end)-LP; x(2,end)-CP; x(3,end)-HP];    %% plataforma do pé

plot3(x(1,:), x(2,:), x(3,:)); 
hold on
plot3(pe(1,:), pe(2,:), pe(3,:)); 
plot3(x(1,:), x(2,:), x(3,:), 'bo');         % faz o plot do robo

maxi = l1+l2+(l1+l2)/2;
plot3([-maxi+x(1,3) maxi+x(1,3) maxi+x(1,3) -maxi+x(1,3) -maxi+x(1,3)], [-maxi+x(2,3) -maxi+x(2,3) maxi+x(2,3) maxi+x(2,3) -maxi+x(2,3)], [0 0 0 0 0], 'k');   % cria a linha de superficie
axis equal
axis([-maxi+x(1,3) maxi+x(1,3) -maxi+x(2,3) maxi+x(2,3) 0 maxi+x(3,3)]);  % estabelece os eixos
xlabel('X');
ylabel('Y');
zlabel('Z');
hold off