rotation3dToEulerAngles

ROTATION3DTOEULERANGLES Extract Euler angles from a rotation matrix.

   [PHI, THETA, PSI] = rotation3dToEulerAngles(MAT)
   Computes Euler angles PHI, THETA and PSI (in degrees) from a 3D 4-by-4
   or 3-by-3 rotation matrix.

   ANGLES = rotation3dToEulerAngles(MAT)
   Concatenates results in a single 1-by-3 row vector. This format is used
   for representing some 3D shapes like ellipsoids.

   ... = rotation3dToEulerAngles(MAT, CONVENTION)
   CONVENTION specifies the axis rotation sequence. Default is 'ZYX'.
   Supported conventions are: 
       'ZYX','ZXY','YXZ','YZX','XYZ','XZY'
       'ZYZ','ZXZ','YZY','YXY','XZX','XYX'

   Example
   rotation3dToEulerAngles

   References
   Code from '1994 - Shoemake - Graphics Gems IV: Euler Angle Conversion:
   http://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
   (see also rotm2eul, that is part of MATLAB's Robotics System Toolbox)
   Modified using explanations in:
   http://www.gregslabaugh.net/publications/euler.pdf
   https://www.geometrictools.com/Documentation/EulerAngles.pdf

   See also
   transforms3d, rotation3dAxisAndAngle, createRotation3dLineAngle,
   eulerAnglesToRotation3d


 ------
 Authors: David Legland, oqilipo
 e-mail: david.legland@grignon.inra.fr
 Created: 2010-08-11,    using Matlab 7.9.0.529 (R2009b)
 Copyright 2010 INRA - Cepia Software Platform.

0001 function varargout = rotation3dToEulerAngles(mat, varargin)
0002 %ROTATION3DTOEULERANGLES Extract Euler angles from a rotation matrix.
0003 %
0004 %   [PHI, THETA, PSI] = rotation3dToEulerAngles(MAT)
0005 %   Computes Euler angles PHI, THETA and PSI (in degrees) from a 3D 4-by-4
0006 %   or 3-by-3 rotation matrix.
0007 %
0008 %   ANGLES = rotation3dToEulerAngles(MAT)
0009 %   Concatenates results in a single 1-by-3 row vector. This format is used
0010 %   for representing some 3D shapes like ellipsoids.
0011 %
0012 %   ... = rotation3dToEulerAngles(MAT, CONVENTION)
0013 %   CONVENTION specifies the axis rotation sequence. Default is 'ZYX'.
0014 %   Supported conventions are:
0015 %       'ZYX','ZXY','YXZ','YZX','XYZ','XZY'
0016 %       'ZYZ','ZXZ','YZY','YXY','XZX','XYX'
0017 %
0018 %   Example
0019 %   rotation3dToEulerAngles
0020 %
0021 %   References
0022 %   Code from '1994 - Shoemake - Graphics Gems IV: Euler Angle Conversion:
0023 %   http://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
0024 %   (see also rotm2eul, that is part of MATLAB's Robotics System Toolbox)
0025 %   Modified using explanations in:
0026 %   http://www.gregslabaugh.net/publications/euler.pdf
0027 %   https://www.geometrictools.com/Documentation/EulerAngles.pdf
0028 %
0029 %   See also
0030 %   transforms3d, rotation3dAxisAndAngle, createRotation3dLineAngle,
0031 %   eulerAnglesToRotation3d
0032 %
0033 %
0034 % ------
0035 % Authors: David Legland, oqilipo
0036 % e-mail: david.legland@grignon.inra.fr
0037 % Created: 2010-08-11,    using Matlab 7.9.0.529 (R2009b)
0038 % Copyright 2010 INRA - Cepia Software Platform.
0039 
0040 p = inputParser;
0041 validStrings = {...
0042     'ZYX','ZXY','YXZ','YZX','XYZ','XZY',...
0043     'ZYZ','ZXZ','YZY','YXY','XZX','XYX'};
0044 addOptional(p,'convention','ZYX',@(x) any(validatestring(x,validStrings)));
0045 logParValidFunc = @(x) (islogical(x) || isequal(x,1) || isequal(x,0));
0046 addParameter(p,'IsRotation', 1, logParValidFunc);
0047 valTol = @(x) validateattributes(x,{'numeric'},{'scalar', '>=',eps(class(mat)), '<=',1});
0048 addParameter(p,'tolerance', 1e-8, valTol);
0049 parse(p,varargin{:});
0050 convention=p.Results.convention;
0051 isRotation = p.Results.IsRotation;
0052 tolerance = p.Results.tolerance;
0053 
0054 if isRotation
0055     if ~isTransform3d(mat(1:3,1:3), 'rotation', 1, 'tolerance', tolerance)
0056         warning(['Rotation matrix contains reflection or scaling ' ...
0057             'tested with a tolerance of ' num2str(tolerance) '.' newline ...
0058             'Calculation of euler angles might be incorrect.'])
0059     end
0060 end
0061 
0062 switch convention
0063     case 'ZYX'
0064         % extract |cos(theta)|
0065         cy = hypot(mat(1,1), mat(2,1));
0066         % avoid dividing by 0
0067         if cy > 16*eps
0068             % normal case: theta <> 0
0069             phi   = atan2( mat(2,1), mat(1,1));
0070             theta = atan2(-mat(3,1), cy);
0071             psi   = atan2( mat(3,2), mat(3,3));
0072         else
0073             phi   = 0;
0074             theta = atan2(-mat(3,1), cy);
0075             psi   = atan2(-mat(2,3), mat(2,2));
0076         end
0077     case 'ZXY'
0078         cy = hypot(mat(2,2), mat(1,2));
0079         if cy > 16*eps
0080             phi   = -atan2( mat(1,2), mat(2,2));
0081             theta = -atan2(-mat(3,2), cy);
0082             psi   = -atan2( mat(3,1), mat(3,3));
0083         else
0084             phi   = 0;
0085             theta = -atan2(-mat(3,2), cy);
0086             psi   = -atan2(-mat(1,3), mat(1,1));
0087         end
0088     case 'YXZ'
0089         cy = hypot(mat(3,3), mat(1,3));
0090         if cy > 16*eps
0091             phi   = atan2( mat(1,3), mat(3,3));
0092             theta = atan2(-mat(2,3), cy);
0093             psi   = atan2( mat(2,1), mat(2,2));
0094         else
0095             phi   = 0;
0096             theta = atan2(-mat(2,3), cy);
0097             psi   = atan2(-mat(1,2), mat(1,1));
0098         end
0099     case 'YZX'
0100         cy = hypot(mat(1,1), mat(3,1));
0101         if cy > 16*eps
0102             phi   = -atan2( mat(3,1), mat(1,1));
0103             theta = -atan2(-mat(2,1), cy);
0104             psi   = -atan2( mat(2,3), mat(2,2));
0105         else
0106             phi   = 0;
0107             theta = -atan2(-mat(2,1), cy);
0108             psi   = -atan2(-mat(3,2), mat(3,3));
0109         end
0110     case 'XYZ'
0111         cy = hypot(mat(3,3), mat(2,3));
0112         if cy > 16*eps
0113             phi   = -atan2( mat(2,3), mat(3,3));
0114             theta = -atan2(-mat(1,3), cy);
0115             psi   = -atan2( mat(1,2), mat(1,1));
0116         else
0117             phi   = 0;
0118             theta = -atan2(-mat(1,3), cy);
0119             psi   = -atan2(-mat(2,1), mat(2,2));
0120         end
0121     case 'XZY'
0122         cy = hypot(mat(2,2), mat(3,2));
0123         if cy > 16*eps
0124             phi   = atan2( mat(3,2), mat(2,2));
0125             theta = atan2(-mat(1,2), cy);
0126             psi   = atan2( mat(1,3), mat(1,1));
0127         else
0128             phi   = 0;
0129             theta = atan2(-mat(1,2), cy);
0130             psi   = atan2(-mat(3,1), mat(3,3));
0131         end
0132         
0133     case 'ZYZ'
0134         cy = hypot(mat(3,2), mat(3,1));
0135         if cy > 16*eps
0136             phi   = -atan2(mat(2,3), -mat(1,3));
0137             theta = -atan2(cy, mat(3,3));
0138             psi   = -atan2(mat(3,2), mat(3,1));
0139         else
0140             phi   = 0;
0141             theta = -atan2(cy, mat(3,3));
0142             psi   = -atan2(-mat(2,1), mat(2,2));
0143         end
0144     case 'ZXZ'
0145         cy = hypot(mat(3,2), mat(3,1));
0146         if cy > 16*eps
0147             phi   = atan2(mat(1,3), -mat(2,3));
0148             theta = atan2(cy, mat(3,3));
0149             psi   = atan2(mat(3,1), mat(3,2));
0150         else
0151             phi   = 0;
0152             theta = atan2(cy, mat(3,3));
0153             psi   = atan2(-mat(1,2), mat(1,1));
0154         end
0155     case 'YZY'
0156         cy = hypot(mat(2,3), mat(2,1));
0157         if cy > 16*eps
0158             phi   = atan2(mat(3,2), -mat(1,2));
0159             theta = atan2(cy, mat(2,2));
0160             psi   = atan2(mat(2,3), mat(2,1));
0161         else
0162             phi   = 0;
0163             theta = atan2(cy, mat(2,2));
0164             psi   = atan2(-mat(3,1), mat(3,3));
0165         end
0166     case 'YXY'
0167         cy = hypot(mat(2,3), mat(2,1));
0168         if cy > 16*eps
0169             phi   = -atan2(mat(1,2), -mat(3,2));
0170             theta = -atan2(cy, mat(2,2));
0171             psi   = -atan2(mat(2,1), mat(2,3));
0172         else
0173             phi   = 0;
0174             theta = -atan2(cy, mat(2,2));
0175             psi   = -atan2(-mat(1,3), mat(1,1));
0176         end
0177     case 'XZX'
0178         cy = hypot(mat(1,3), mat(1,2));
0179         if cy > 16*eps
0180             phi   = -atan2(mat(3,1), -mat(2,1));
0181             theta = -atan2(cy, mat(1,1));
0182             psi   = -atan2(mat(1,3), mat(1,2));
0183         else
0184             phi   = 0;
0185             theta = -atan2(cy, mat(1,1));
0186             psi   = -atan2(-mat(3,2), mat(3,3));
0187         end
0188     case 'XYX'
0189         cy = hypot(mat(1,2), mat(1,3));
0190         if cy > 16*eps
0191             phi   = atan2(mat(2,1), -mat(3,1));
0192             theta = atan2(cy, mat(1,1));
0193             psi   = atan2(mat(1,2), mat(1,3));
0194         else
0195             phi   = 0;
0196             theta = atan2(cy, mat(1,1));
0197             psi   = atan2(-mat(2,3), mat(2,2));
0198         end
0199 end
0200 
0201 % format output arguments
0202 if nargout <= 1
0203     % one array
0204     varargout{1} = rad2deg([phi theta psi]);
0205 else
0206     % three separate arrays
0207     varargout = cellfun(@rad2deg, {phi theta psi},'uni',0);
0208 end