%% **************************************************************************
%Robotica industrial - Aula 3 - 01/10/2009
%Luis Rodrigues - 36386
%**************************************************************************


%% Exercicio 3 
%Representar um manipulador com 3 eixos e seus sistemas de coordenadas

%% 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.
%%


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

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

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
]; 
 
%% 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,:) ];

P = [X;Y;Z; ones(1,size(X,2))];

%% Ilustração do gráfico 3D com a representação do manipulador e os
%% sistemas de coordenadas
close all

r=fill3(P(1,:),P(2,:),P(3,:), 'y');
hold on
%Ponto referencia para desenhar uma linha
pont_ref=[0 0 0 1]';
plot3(pont_ref(1,1),pont_ref(2,1),pont_ref(3,1),'*');

xlabel('X'); ylabel('Y'); zlabel('Z');
grid on; 
axis([-1 7 -1 14 -1 7]);
%axis equal;

%Definição de todas as transformações obtidas pelo objecto
T(:,:,1)=Trans3D(0,5,0);
T(:,:,2)=RotacaoXX((25/180)*pi);
T(:,:,3)=RotacaoXX((-30/180)*pi);
T(:,:,4)=Trans3D(0,4,0);
T(:,:,5)=RotacaoXX((20/180)*pi);
T(:,:,6)=Trans3D(0,3,0);

%0 -Transformação em relação ao eixo global
%1 -Transformação em relação ao eixo local
order = [0 0 1 1 1 1];

%Representação ou não do sistema de eixos
imag = [0 1 0 1 0 1];

%Cor do sistema de eixos
color = ['r' 'r' 'b' 'b' 'k' 'k'];

%Matriz de transformação inicial
MT=eye(4,4);

for n=1:6
    %Se a transformação for em relação ao eixo local
   if order(n)==1
       MT=MT*T(:,:,n);
   else
       MT=T(:,:,n)*MT;
   end
    PMod=MT*P;
    
    if (imag(n)==1)
        pause(0.5)
        r=fill3(PMod(1,:),PMod(2,:),PMod(3,:), color(n));
       
    end

     pont_ref2=MT*pont_ref
     if(n==2)
        line( [ pont_ref(1,1) pont_ref2(1,1)]', [pont_ref(2,1) pont_ref2(2,1)]', [pont_ref(3,1) pont_ref2(3,1)])
     end
    
     if(n==4 | n==6)
        line( [ pont_ref3(1,1) pont_ref2(1,1)]', [pont_ref3(2,1) pont_ref2(2,1)]', [pont_ref3(3,1) pont_ref2(3,1)])
     end
     
      ponto_ref3= double (pont_ref2)
      
      
      %Duvida!!!!!!!!!!!!!
    
end