%%Teste

clear all, close all, clc

syms('a','b','c','Xp', 'Yp', 'Zp','Zg')   
% 
% a=0/180*pi;    %rot ZZ
% b=0/180*pi;    %rot YY
% c=180/180*pi;   %rot XX
% 
% Xp=5;
% Yp=0;
% Zp=0;
% Zg=4;

rotZ=[cos(a)    -sin(a)    0   0
      sin(a)     cos(a)    0   0
         0          0      1   0
         0          0      0   1]
     
rotY=[cos(b)    0    sin(b)    0
         0      1      0       0
     -sin(b)    0    cos(b)    0
         0      0      0       1]     

rotX=[   1      0      0       0
         0   cos(c) -sin(c)    0
         0   sin(c)  cos(c)    0
         0      0      0       1]          
     
Trans1=[ 1      0      0      Xp
         0      1      0      Yp
         0      0      1      Zp
         0      0      0       1]

Trans2=[ 1      0      0       0
         0      1      0       0
         0      0      1       Zg
         0      0      0       1]    
    
A=Trans1 * rotZ * rotY * rotX * Trans2    
% rotZ * rotY * rotX    
%A=rotX


% disp('Rotação em torno de YY:')
% b = ( atan2( A(1,3), A(1,1)) )*180/pi 
% 
% disp('Rotação em torno de XX:')
% c = ( atan2(A(3,2), A(2,2)) )*180/pi
% 
% disp('Rotação em torno de ZZ:')
% a = ( atan2( -A(1,2)*sin(c), A(3,2) ))*180/pi