T2quaternion

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   Q = T2quaternion(T)
   Returns the quaternion corresponding to an homogeneous transformation 
   matrix T. Only the 3x3 rotation matrix in T is used.

   See also QPROD, QUATERNION2T.

   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email:
   arturo.gil@umh.es date:   21/04/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %   Q = T2quaternion(T)
0003 %   Returns the quaternion corresponding to an homogeneous transformation
0004 %   matrix T. Only the 3x3 rotation matrix in T is used.
0005 %
0006 %   See also QPROD, QUATERNION2T.
0007 %
0008 %   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email:
0009 %   arturo.gil@umh.es date:   21/04/2012
0010 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0011 
0012 % Copyright (C) 2012, by Arturo Gil Aparicio
0013 %
0014 % This file is part of ARTE (A Robotics Toolbox for Education).
0015 %
0016 % ARTE is free software: you can redistribute it and/or modify
0017 % it under the terms of the GNU Lesser General Public License as published by
0018 % the Free Software Foundation, either version 3 of the License, or
0019 % (at your option) any later version.
0020 %
0021 % ARTE is distributed in the hope that it will be useful,
0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0024 % GNU Lesser General Public License for more details.
0025 %
0026 % You should have received a copy of the GNU Leser General Public License
0027 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0028 function Q = T2quaternion(T)
0029 
0030 Q = zeros(1,4);
0031 Q(1) = sqrt(trace(T))/2;
0032 Q(1) = real(Q(1));
0033 
0034 Lx = T(3,2) - T(2,3);    
0035 Ly = T(1,3) - T(3,1);    
0036 Lz = T(2,1) - T(1,2);    
0037 
0038 if (T(1,1) >= T(2,2)) && (T(1,1) >= T(3,3))
0039     Lx1 = T(1,1) - T(2,2) - T(3,3) + 1;    
0040     Ly1 = T(2,1) + T(1,2);            
0041     Lz1 = T(3,1) + T(1,3);
0042 elseif (T(2,2) >= T(3,3))
0043     Lx1 = T(2,1) + T(1,2);            
0044     Ly1 = T(2,2) - T(1,1) - T(3,3) + 1;    
0045     Lz1 = T(3,2) + T(2,3);        
0046 else
0047     Lx1 = T(3,1) + T(1,3);            
0048     Ly1 = T(3,2) + T(2,3);            
0049     Lz1 = T(3,3) - T(1,1) - T(2,2) + 1;    
0050 end
0051 
0052 if (Lx >= 0) || (Ly >= 0) || (Lz >= 0)
0053     Lx = Lx + Lx1;
0054     Ly = Ly + Ly1;
0055     Lz = Lz + Lz1;
0056 else
0057     Lx = Lx - Lx1;
0058     Ly = Ly - Ly1;
0059     Lz = Lz - Lz1;
0060 end
0061 
0062 if norm([Lx Ly Lz]) == 0
0063     Q = [1 0 0 0];
0064 else
0065     s = sqrt(1-Q(1)^2)/norm([Lx Ly Lz]);
0066     Q(2:4) = s*[Lx Ly Lz];
0067 end
0068 
0069 
0070 
0071 
0072