Code covered by the BSD License  

Highlights from
SpinConv

SpinConv

by

 

Conversion from a rotation representation type to another

SpinConv.m
function OUTPUT = SpinConv(TYPES, INPUT, tol, ichk)
%SpinConv   Conversion from a rotation representation type to another
%
%   OUT = SpinConv(TYPES, IN, TOL, ICHK) converts a rotation representation
%   type (IN) to another (OUT). Supported conversion input/output types are
%   as follows:
%      1) Q      Rotation quaternions
%      2) EV     Euler vector and rotation angle (degrees)
%      3) DCM    Direction cosine matrix (a.k.a. rotation matrix)
%      4) EA###  Euler angles (12 possible sets) (degrees)
%   All representation types accepted as input (IN) and returned as output
%   (OUT) by SpinConv are meant to represent the rotation of a 3D
%   coordinate system (CS) relative to a rigid body or vector space
%   ("alias" transformation), rather than vice-versa ("alibi"
%   transformation).
% 
%   OUT=SpinConv(TYPES,IN) is equivalent to OUT=SpinConv(TYPES,IN,10*eps,1) 
%   OUT=SpinConv(TYPES,IN,TOL) is equiv. to OUT=SpinConv(TYPES,IN,TOL,1) 
%
%   Input and output arguments:
%
%      TYPES - Single string value that specifies both the input type and
%            the desired output type. The allowed values are:
%
%               'DCMtoEA###'      'DCMtoEV'      'DCMtoQ'     
%               'EA###toDCM'      'EA###toEV'    'EA###toQ'     
%               'EVtoDCM'         'EVtoEA###'    'EVtoQ'         
%               'QtoDCM'          'QtoEA###'     'QtoEV'        
%               'EA###toEA###' 
%                  
%            For cases that involve Euler angles, ### should be
%            replaced with the proper order desired. E.g., EA321
%            would be Z(yaw)-Y(pitch)-X(roll).
%
%      IN  - Array of N matrices or N vectors (N>0) corresponding to the
%            first entry in the TYPES string, formatted as follows:
%
%            DCM - (33N) Array of rotation matrices. Each matrix R 
%                  contains in its rows the versors of the rotated CS
%                  represented in the original CS, and in its columns the
%                  versors of the original CS represented in the rotated
%                  CS. This format is typically used when the column-vector
%                  convention is adopted: point coordinates are arranged in
%                  column vectors Vi, and the desired rotation is applied
%                  by pre-multiplying Vi by R (rotated Vi = R * Vi).
%            EA### - [psi,theta,phi] (N3) row vector list containing, in 
%                  each row, three Euler angles or Tait-Bryan angles.
%                  (degrees).
%            EV  - [m1,m2,m3,MU] (N4) Row vector list containing, in each 
%                  row, the components (m1, m2, m3) of an Euler rotation
%                  vector (represented in the original CS) and the Euler
%                  rotation angle about that vector (MU, in degrees).
%            Q   - [q1,q2,q3,q4] (N4) Row vector list defining, in each
%                  row, a rotation quaternion. q4 = cos(MU/2), where MU is
%                  the Euler angle.
%
%      TOL - (Default value: TOL = 10 * eps) Tolerance value for deviations
%            from 1. Used to test determinant of rotation matrices or
%            length of unit vectors.
%      ICHK - (Default value: ICHK = 1) Flag controlling whether 
%            near-singularity warnings are issued or not. 
%            ICHK = 0 disables warnings.  
%            ICHK = 1 enables them.
%      OUT - Array of N matrices or N vectors (N > 0) corresponding to the
%            second entry in the TYPES string, formatted as shown
%            above.
%
%   See also SpinCalc, degtorad, rad2deg.

% Version 2.2
% 2013 April 3
%
% Based on:
%    SpinCalc, Version 1.3 (MATLAB Central file #20696)
%    2009 June 30 
% SpinCalc code by:
%    John Fuller
%    National Institute of Aerospace
%    Hampton, VA 23666
%    John.Fuller@nianet.org
% Debugged and optimized for speed by:
%    Paolo de Leva
%    University "Foro Italico" 
%    Rome, Italy
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

% Setting default values for missing input arguments
switch nargin
    case 2, tol = 10*eps; ichk = true;
    case 3, ichk = true;
    case 4
        if isequal(ichk, 0), ichk = false; 
        else                 ichk = true; 
        end
    otherwise, error(nargchk(2, 4, nargin)); % Allow 2 to 4 input arguments
end

% No TYPES string can be shorter than 4 or longer than 12 chars
    len = length(TYPES);
    if len>12 || len<4, error('Invalid entry for TYPES input string'); end

% Determine type of conversion from TYPES string
    TYPES = upper(TYPES);
    index = strfind(TYPES, 'TO');
    TYPE.INPUT  = TYPES(1 : index-1);
    TYPE.OUTPUT = TYPES(index+2 : len);
    fields = {'INPUT', 'OUTPUT'}; % 12 cell
    % Check validity of TYPES string, both for input and output
    for f = 1:2
       IO = fields{f};
       type = TYPE.(IO);
       switch type
           case {'Q' 'EV' 'DCM'} % Valid TYPE
           otherwise
               % Check that TYPE is 'EA###'
               if length(type)~=5 || ~strcmp(type(1:2), 'EA')
                   error('Invalid entry for TYPES input string')
               end
               TYPE.(IO) = 'EA';
               EAorder.(IO) = type(3:5);
               % Check that all characters in '###' are numbers between 1
               % and 3, and that no 2 consecutive characters are equal
               order31 = str2num(EAorder.(IO)'); % 31 double
               if isempty(order31) || any ([order31<1; order31>3]) || ...
                                            order31(1)==order31(2) || ...
                                            order31(2)==order31(3)
                   error('Invalid Euler angle order in TYPES string.')
               end
               % Type of EA sequence:
               %    1) Rotations about three distinct axes
               %    2) 1st and 3rd rotation about same axis
               if order31(1)==order31(3), EAtype.(IO) = 2; 
               else                       EAtype.(IO) = 1;
               end
        end
    end
    
% Set N (number of rotations) and check INPUT size
    [size1, size2, size3] = size(INPUT);
    switch TYPE.INPUT
       case 'DCM' % (33N) Direction cosine matrix
           N = size3;
           isnot_DCM = false;
           if ndims(INPUT)>3 || N==0 || size1~=3 || size2~=3
               error('Invalid INPUT size (INPUT must be 33N for DCM type)')
           end
        case 'EA', v_length=3; Isize='N3'; isnot_DCM=true;
        case 'Q',  v_length=4; Isize='N4'; isnot_DCM=true;
        case 'EV', v_length=4; Isize='N4'; isnot_DCM=true;
    end
    if isnot_DCM
        N = size1;
        if ndims(INPUT)~=2 || N==0 || size2~=v_length
            error(['Invalid INPUT size (INPUT must be ' ...
                    Isize ' for ' TYPE.INPUT ' type)'])
        end
    end

% Determine the quaternions that uniquely describe the rotation prescribed
% by INPUT. OUTPUT will be calculated in the second portion of the code
% from these quaternions.
switch TYPE.INPUT
    
    case 'DCM'
        % NOTE: Orthogonal matrixes may have determinant -1 or 1
        %       DCMs are special orthogonal matrices, with determinant 1
        improper  = false;
        DCM_not_1 = false;
        if N == 1
            % Computing deviation from orthogonality
            delta = INPUT * INPUT' - eye(3); % DCM*DCM' - I
            delta = delta(:); % 91 <-- 33
            % Checking determinant of DCM
            DET = det(INPUT);
            if DET<0, improper=true; end
            if ichk && abs(DET-1)>tol, DCM_not_1=true; end 
            % Permuting INPUT
            INPUT = reshape(INPUT, [1 3 3]); % 133
        else
            % Computing deviation from orthogonality
            delta = multiprod(INPUT, multitransp(INPUT), [1 2]); % DCM*DCM'
            delta = bsxfun(@minus, delta, eye(3)); % Subtracting I
            delta = delta(:); % 9N1 <-- 33N
            % Checking determinant of DCMs
            DET = INPUT(1,1,:).*INPUT(2,2,:).*INPUT(3,3,:) -INPUT(1,1,:).*INPUT(2,3,:).*INPUT(3,2,:)...
                 +INPUT(1,2,:).*INPUT(2,3,:).*INPUT(3,1,:) -INPUT(1,2,:).*INPUT(2,1,:).*INPUT(3,3,:)...
                 +INPUT(1,3,:).*INPUT(2,1,:).*INPUT(3,2,:) -INPUT(1,3,:).*INPUT(2,2,:).*INPUT(3,1,:); % 11N
            if any(DET<0), improper=true; end
            if ichk && any(abs(DET-1)>tol), DCM_not_1=true; end 
            % Permuting INPUT
            INPUT = permute(INPUT, [3 1 2]); % N33            
        end
        % Issuing error messages or warnings
        if ichk && any(abs(delta)>tol)
            errordlg('Warning: Input DCM is not orthogonal.')
        end
        if improper, error('Improper input DCM'); end
        if DCM_not_1
            errordlg('Warning: Input DCM determinant off from 1 by more than tolerance.');
        end
        % Denominators for 4 distinct types of equivalent Q equations
        denom = [1 + INPUT(:,1,1) - INPUT(:,2,2) - INPUT(:,3,3),...
                 1 - INPUT(:,1,1) + INPUT(:,2,2) - INPUT(:,3,3),...
                 1 - INPUT(:,1,1) - INPUT(:,2,2) + INPUT(:,3,3),...
                 1 + INPUT(:,1,1) + INPUT(:,2,2) + INPUT(:,3,3)];
        denom = 2 .* sqrt (denom); % N4
        % Choosing for each DCM the equation which uses largest denominator
        [maxdenom, index] = max(denom, [], 2); % N1
        clear delta DET denom
        Q = NaN(N,4); % N4
        % EQUATION 1
        ii = (index==1); % (Logical vector) MAXDENOM==DENOM(:,1)
        if any(ii)
            Q(ii,:) = [                         0.25 .* maxdenom(ii,1),... 
                       (INPUT(ii,1,2)+INPUT(ii,2,1)) ./ maxdenom(ii,1),...
                       (INPUT(ii,1,3)+INPUT(ii,3,1)) ./ maxdenom(ii,1),...
                       (INPUT(ii,2,3)-INPUT(ii,3,2)) ./ maxdenom(ii,1)];
        end
        % EQUATION 2
        ii = (index==2); % (Logical vector) MAXDENOM==DENOM(:,2)
        if any(ii)
            Q(ii,:) = [(INPUT(ii,1,2)+INPUT(ii,2,1)) ./ maxdenom(ii,1),...
                                                0.25 .* maxdenom(ii,1),...
                       (INPUT(ii,2,3)+INPUT(ii,3,2)) ./ maxdenom(ii,1),...
                       (INPUT(ii,3,1)-INPUT(ii,1,3)) ./ maxdenom(ii,1)];
        end
        % EQUATION 3
        ii = (index==3); % (Logical vector) MAXDENOM==DENOM(:,3)
        if any(ii)
            Q(ii,:) = [(INPUT(ii,1,3)+INPUT(ii,3,1)) ./ maxdenom(ii,1),...
                       (INPUT(ii,2,3)+INPUT(ii,3,2)) ./ maxdenom(ii,1),...
                                                0.25 .* maxdenom(ii,1),...
                       (INPUT(ii,1,2)-INPUT(ii,2,1)) ./ maxdenom(ii,1)];
        end
        % EQUATION 4
        ii = (index==4); % (Logical vector) MAXDENOM==DENOM(:,4)
        if any(ii)
            Q(ii,:) = [(INPUT(ii,2,3)-INPUT(ii,3,2)) ./ maxdenom(ii,1),...
                       (INPUT(ii,3,1)-INPUT(ii,1,3)) ./ maxdenom(ii,1),...
                       (INPUT(ii,1,2)-INPUT(ii,2,1)) ./ maxdenom(ii,1),...
                                                0.25 .* maxdenom(ii)];
        end
        clear INPUT maxdenom index ii
        
    case 'EV'
        % Euler vector (EV) and angle MU in degrees
        EV = INPUT(:,1:3); % N3
        halfMU = INPUT(:,4) * (pi/360); % (N1) MU/2 in radians
        % Check that input m's constitute unit vector
        delta = sqrt(sum(EV.*EV, 2)) - 1; % N1
        if any(abs(delta) > tol)
            error('(At least one of the) input Euler vector(s) is not a unit vector')            
        end
        % Quaternion
        SIN = sin(halfMU); % (N1)
        Q = [EV(:,1).*SIN, EV(:,2).*SIN, EV(:,3).*SIN, cos(halfMU)];
        clear EV delta halfMU SIN
        
    case 'EA'
        % Identify singularities (2nd Euler angle out of range)
        theta = INPUT(:, 2); % N1
        if EAtype.INPUT == 1
            % Type 1 rotation (rotations about three distinct axes)
            if any(abs(theta)>=90)
                error('Second input Euler angle(s) outside -90 to 90 degree range')
            elseif ichk && any(abs(theta)>88)
                errordlg(['Warning: Second input Euler angle(s) near a '...
                          'singularity (-90 or 90 degrees).'])
            end
        else 
            % Type 2 rotation (1st and 3rd rotation about same axis)
            if any(theta<=0 | theta>=180)
                error('Second input Euler angle(s) outside 0 to 180 degree range')
            elseif ichk && any(theta<2 | theta>178)
                errordlg(['Warning: Second input Euler angle(s) near a '...
                          'singularity (0 or 180 degrees).'])
            end
        end
        % Half angles in radians
        HALF = INPUT * (pi/360); % N3
        Hpsi   = HALF(:,1); % N1
        Htheta = HALF(:,2); % N1
        Hphi   = HALF(:,3); % N1
        % Pre-calculate cosines and sines of the half-angles for conversion.
        c1=cos(Hpsi); c2=cos(Htheta); c3=cos(Hphi);
        s1=sin(Hpsi); s2=sin(Htheta); s3=sin(Hphi);
        c13 =cos(Hpsi+Hphi);  s13 =sin(Hpsi+Hphi);
        c1_3=cos(Hpsi-Hphi);  s1_3=sin(Hpsi-Hphi);
        c3_1=cos(Hphi-Hpsi);  s3_1=sin(Hphi-Hpsi);
        clear HALF Hpsi Htheta Hphi
        switch EAorder.INPUT
            case '121', Q=[c2.*s13,  s2.*c1_3, s2.*s1_3, c2.*c13];
            case '232', Q=[s2.*s1_3, c2.*s13,  s2.*c1_3, c2.*c13];
            case '313', Q=[s2.*c1_3, s2.*s1_3, c2.*s13,  c2.*c13];
            case '131', Q=[c2.*s13,  s2.*s3_1, s2.*c3_1, c2.*c13];
            case '212', Q=[s2.*c3_1, c2.*s13,  s2.*s3_1, c2.*c13];
            case '323', Q=[s2.*s3_1, s2.*c3_1, c2.*s13,  c2.*c13];
            case '123', Q=[s1.*c2.*c3+c1.*s2.*s3, c1.*s2.*c3-s1.*c2.*s3, c1.*c2.*s3+s1.*s2.*c3, c1.*c2.*c3-s1.*s2.*s3];
            case '231', Q=[c1.*c2.*s3+s1.*s2.*c3, s1.*c2.*c3+c1.*s2.*s3, c1.*s2.*c3-s1.*c2.*s3, c1.*c2.*c3-s1.*s2.*s3];
            case '312', Q=[c1.*s2.*c3-s1.*c2.*s3, c1.*c2.*s3+s1.*s2.*c3, s1.*c2.*c3+c1.*s2.*s3, c1.*c2.*c3-s1.*s2.*s3];
            case '132', Q=[s1.*c2.*c3-c1.*s2.*s3, c1.*c2.*s3-s1.*s2.*c3, c1.*s2.*c3+s1.*c2.*s3, c1.*c2.*c3+s1.*s2.*s3];
            case '213', Q=[c1.*s2.*c3+s1.*c2.*s3, s1.*c2.*c3-c1.*s2.*s3, c1.*c2.*s3-s1.*s2.*c3, c1.*c2.*c3+s1.*s2.*s3];
            case '321', Q=[c1.*c2.*s3-s1.*s2.*c3, c1.*s2.*c3+s1.*c2.*s3, s1.*c2.*c3-c1.*s2.*s3, c1.*c2.*c3+s1.*s2.*s3];
        otherwise
            error('Invalid input Euler angle order (TYPES string)');            
        end
        clear c1 c2 c3 s1 s2 s3 c13 s13 c1_3 s1_3 c3_1 s3_1

    case 'Q'
        if ichk && any(abs(sqrt(sum(INPUT.*INPUT, 2)) - 1) > tol)
            errordlg('Warning: (At least one of the) Input quaternion(s) is not a unit vector')
        end
        Q = INPUT;
end
clear TYPE.INPUT EAorder.INPUT

% Normalize quaternion(s) in case of deviation from unity. 
% User has already been warned of deviation.
Qnorms = sqrt(sum(Q.*Q,2));
Q = [Q(:,1)./Qnorms, Q(:,2)./Qnorms, Q(:,3)./Qnorms, Q(:,4)./Qnorms]; % N4

switch TYPE.OUTPUT
    
    case 'DCM'
        Q  = reshape(Q', [1 4 N]); % (14N)
        SQ = Q.^2;
        OUTPUT = [   SQ(1,1,:)-SQ(1,2,:)-SQ(1,3,:)+SQ(1,4,:),  2.*(Q(1,1,:).*Q(1,2,:) +Q(1,3,:).*Q(1,4,:)), 2.*(Q(1,1,:).*Q(1,3,:) -Q(1,2,:).*Q(1,4,:));
                  2.*(Q(1,1,:).*Q(1,2,:) -Q(1,3,:).*Q(1,4,:)),   -SQ(1,1,:)+SQ(1,2,:)-SQ(1,3,:)+SQ(1,4,:),  2.*(Q(1,2,:).*Q(1,3,:) +Q(1,1,:).*Q(1,4,:));
                  2.*(Q(1,1,:).*Q(1,3,:) +Q(1,2,:).*Q(1,4,:)), 2.*(Q(1,2,:).*Q(1,3,:) -Q(1,1,:).*Q(1,4,:)),   -SQ(1,1,:)-SQ(1,2,:)+SQ(1,3,:)+SQ(1,4,:)];
    
    case 'EV'
        % Angle MU in radians and sine of MU/2
        halfMUrad = atan2( sqrt(sum(Q(:,1:3).*Q(:,1:3),2)), Q(:,4) ); % N1
        SIN = sin(halfMUrad); % N1
        index = (SIN==0); % (N1) Logical index
        if any(index)
            % Initializing
            OUTPUT = zeros(N,4);
            % Singular cases (MU is zero degrees)
            OUTPUT(index, 1) = 1;
            % Non-singular cases
            SIN = SIN(~index, 1);
            OUTPUT(~index, :) = [Q(~index,1) ./ SIN, ...
                                 Q(~index,2) ./ SIN, ...
                                 Q(~index,3) ./ SIN, ...
                                 halfMUrad .* (360/pi)];
        else
            % Non-singular cases            
            OUTPUT = [Q(:,1)./SIN, Q(:,2)./SIN, Q(:,3)./SIN, halfMUrad.*(360/pi)];
        end
        % MU greater than 180 degrees
        index = (OUTPUT(:,4) > 180); % (N1) Logical index
        OUTPUT(index, :) = [-OUTPUT(index,1:3), 360-OUTPUT(index,4)];

    case 'EA'
        SQ = Q.^2;
        switch EAorder.OUTPUT
        case '121'
            OUTPUT = [atan2(Q(:,1).*Q(:,2) +Q(:,3).*Q(:,4), Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3)),...
                      acos(SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3)),...
                      atan2(Q(:,1).*Q(:,2) -Q(:,3).*Q(:,4), Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4))];
        case '232'
            OUTPUT = [atan2(Q(:,1).*Q(:,4) +Q(:,2).*Q(:,3), Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),...
                      acos(SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3)),...
                      atan2(Q(:,2).*Q(:,3) -Q(:,1).*Q(:,4), Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4))];
        case '313'
            OUTPUT = [atan2(Q(:,1).*Q(:,3) +Q(:,2).*Q(:,4), Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)),...
                      acos(SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3)),...
                      atan2(Q(:,1).*Q(:,3) -Q(:,2).*Q(:,4), Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3))];
        case '131'
            OUTPUT = [atan2(Q(:,1).*Q(:,3) -Q(:,2).*Q(:,4), Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)),...
                      acos(SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3)),...
                      atan2(Q(:,1).*Q(:,3) +Q(:,2).*Q(:,4), Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2))];
        case '212'
            OUTPUT = [atan2(Q(:,1).*Q(:,2) -Q(:,3).*Q(:,4), Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3)),...
                      acos(SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3)),...
                      atan2(Q(:,1).*Q(:,2) +Q(:,3).*Q(:,4), Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3))];
        case '323'
            OUTPUT = [atan2(Q(:,2).*Q(:,3) -Q(:,1).*Q(:,4), Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)),...
                      acos(SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3)),...
                      atan2(Q(:,1).*Q(:,4) +Q(:,2).*Q(:,3), Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3))];
        case '123'
            OUTPUT = [atan2(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)), SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3)),...
                       asin(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4))),...
                      atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)), SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3))];
        case '231'
            OUTPUT = [atan2(2.*(Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3)), SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3)),...
                       asin(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4))),...
                      atan2(2.*(Q(:,1).*Q(:,4)-Q(:,3).*Q(:,2)), SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3))];
        case '312'
            OUTPUT = [atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)), SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3)),...
                       asin(2.*(Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3))),...
                      atan2(2.*(Q(:,2).*Q(:,4)-Q(:,3).*Q(:,1)), SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3))];
        case '132'
            OUTPUT = [atan2(2.*(Q(:,1).*Q(:,4)+Q(:,2).*Q(:,3)), SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3)),...
                       asin(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2))),...
                      atan2(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)), SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3))];
        case '213'
            OUTPUT = [atan2(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)), SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3)),...
                       asin(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3))),...
                      atan2(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)), SQ(:,4)-SQ(:,1)+SQ(:,2)-SQ(:,3))];
        case '321'
            OUTPUT = [atan2(2.*(Q(:,1).*Q(:,2)+Q(:,3).*Q(:,4)), SQ(:,4)+SQ(:,1)-SQ(:,2)-SQ(:,3)),...
                       asin(2.*(Q(:,2).*Q(:,4)-Q(:,1).*Q(:,3))),...
                      atan2(2.*(Q(:,1).*Q(:,4)+Q(:,3).*Q(:,2)), SQ(:,4)-SQ(:,1)-SQ(:,2)+SQ(:,3))];
        otherwise
            error('Invalid output Euler angle order (TYPES string).');
        end
        OUTPUT = OUTPUT * (180/pi); % (N3) Euler angles in degrees
        theta  = OUTPUT(:,2);       % (N1) Angle THETA in degrees
        % Check OUTPUT
        if any(~isreal( OUTPUT(:) ))
	        error('SpinConv:Unreal', ...
                 ['Unreal Euler output. Input resides too close to singularity.\n' ...
                  'Please choose different output type.'])
        end
        % Type 1 rotation (rotations about three distinct axes)
        % THETA is computed using ASIN and ranges from -90 to 90 degrees
        if EAtype.OUTPUT == 1
	        singularities = abs(theta) > 89.9; % (N1) Logical index
	        if any(singularities)
                firstsing = find(singularities, 1); % (11)
		        error(['Input rotation # %s resides too close to Type 1 Euler singularity.\n' ...
                       'Type 1 Euler singularity occurs when second angle is -90 or 90 degrees.\n' ...
                       'Please choose different output type.'], num2str(firstsing));
	        end
        % Type 2 rotation (1st and 3rd rotation about same axis)
        % THETA is computed using ACOS and ranges from 0 to 180 degrees
        else
	        singularities = theta<0.1 | theta>179.9; % (N1) Logical index
	        if any(singularities)
                firstsing = find(singularities, 1); % (11)
		        error(['Input rotation # %s resides too close to Type 2 Euler singularity.\n' ...
                       'Type 2 Euler singularity occurs when second angle is 0 or 180 degrees.\n' ...
                       'Please choose different output type.'], num2str(firstsing));
	        end
        end

    case 'Q'
        OUTPUT = Q;
end

Contact us