matlab/quaternion/quaternion.m
classdef quaternion
% classdef quaternion, implements quaternion mathematics and 3D rotations
%
% Properties (SetAccess = protected):
%  e(4,1)   components, basis [1; i; j; k]: e(1) + i*e(2) + j*e(3) + k*e(4)
%           i*j=k, j*i=-k, j*k=i, k*j=-i, k*i=j, i*k=-j, i*i = j*j = k*k = -1
%
% Constructors:
%  q  = quaternion              scalar zero quaternion, q.e = [0;0;0;0]
%  q  = quaternion(x)           x is a matrix size [4,s1,s2,...] or [s1,4,s2,...],
%                               q is size [s1,s2,...], q(i1,i2,...).e = ...
%                               x(1:4,i1,i2,...) or x(i1,1:4,i2,...).'
%  q  = quaternion(v)           v is a matrix size [3,s1,s2,...] or [s1,3,s2,...],
%                               q is size [s1,s2,...], q(i1,i2,...).e = ...
%                               [0;v(1:3,i1,i2,...)] or [0;v(i1,1:3,i2,...).']
%  q  = quaternion(c)           c is a complex matrix size [s1,s2,...],
%                               q is size [s1,s2,...], q(i1,i2,...).e = ...
%                               [real(c(i1,i2,...));imag(c(i1,i2,...));0;0]
%  q  = quaternion(x1,x2)       x1,x2 are matrices size [s1,s2,...] or scalars,
%                               q(i1,i2,...).e = [x1(i1,i2,...);x2(i1,i2,...);0;0]
%  q  = quaternion(v1,v2,v3)    v1,v2,v3 matrices size [s1,s2,...] or scalars,
%                               q(i1,i2,...).e = [0;v1(i1,i2,...);v2(i1,i2,...);...
%                               v3(i1,i2,...)]
%  q  = quaternion(x1,x2,x3,x4) x1,x2,x3,x4 matrices size [s1,s2,...] or scalars,
%                               q(i1,i2,...).e = [x1(i1,i2,...);x2(i1,i2,...);...
%                               x3(i1,i2,...);x4(i1,i2,...)]
%
% Quaternion array constructor methods:
%  q  = quaternion.complexmatrix(Q)
%                               construct quaternions from complex 2x2 matrices
%  q  = quaternion.eye(N)       quaternion NxN identity matrix
%  q  = quaternion.nan(siz)     q(:).e = [NaN;NaN;NaN;NaN]
%  q  = quaternion.ones(siz)    q(:).e = [1;0;0;0]
%  q  = quaternion.rand(siz)    uniform random quaternions, NOT normalized
%                               to 1, 0 <= q.e(1) <= 1, -1 <= q.e(2:4) <= 1
%  q  = quaternion.randRot(siz) random quaternions uniform in rotation space
%  q  = quaternion.zeros(siz)   q(:).e = [0;0;0;0]
%
% Rotation constructor methods (all lower case):
%  q  = quaternion.angleaxis(angle,axis)
%                               angle is an array in radians, axis is an array
%                               of vectors size [3,s1,s2,...] or [s1,3,s2,...],
%                               q is size [s1,s2,...], quaternions normalized to 1
%                               equivalent to rotations about axis by angle
%  q  = quaternion.eulerangles(axes,angles) or
%  q  = quaternion.eulerangles(axes,ang1,ang2,ang3)
%                               axes is a string array or cell string array,
%                               '123' = 'xyz' = 'XYZ' = 'ijk', etc.,
%                               angles is an array of Euler angles in radians,
%                               size [3,s1,s2,...] or [s1,3,s2,...], or
%                               (ang1, ang2, ang3) are arrays or scalars of
%                               Euler angles in radians, q is size
%                               [s1,s2,...], quaternions normalized to 1
%                               equivalent to Euler Angle rotations
%  q  = quaternion.integrateomega(t,w,odeoptions) or
%  q  = quaternion.integrateomega(t,omega,axis,odeoptions)
%                               integrate angular velocities over time
%  q  = quaternion.modifiedrodrigues(mrp)
%                               quaternions from Modified Rodrigues parameters
%  q  = quaternion.rodrigues(rp)
%                               quaternions from Rodrigues parameters
%  q  = quaternion.rotateutov(u,v,dimu,dimv)
%                               quaternions normalized to 1 that rotate 3
%                               element vectors u into the directions of 3
%                               element vectors v
%  q  = quaternion.rotationmatrix(R)
%                               R is an array of rotation or Direction Cosine
%                               Matrices size [3,3,s1,s2,...] with det(R) == 1,
%                               q(i1,i2,...) = quaternions normalized to 1,
%                               equivalent to R(1:3,1:3,i1,i2,...)
%
% Rotation methods (Mixed Case):
%  [angle,axis] = AngleAxis(q)  angles in radians, unit vector rotation axes
%                               equivalent to q
%  qd = Derivative(q,w)         quaternion derivatives, w are 3 component
%                               angular velocity vectors, qd = 0.5*q*quaternion(w)
%  angles = EulerAngles(q,axes) angles are 3 Euler angles equivalent to q, axes
%                               are strings or cell strings, '123' = 'xyz', etc.
%  q1  = Integral(q0,t,w,odeoptions) or
%  q1  = Integral(q0,t,omega,axis,odeoptions)
%                               integrate angular velocities to quaternions
%  mrp = ModifiedRodrigues(q)   Modified Rodrigues parameters equivalent to q
%  [omega,axis] = OmegaAxis(q,t,dim)
%                               instantaneous angular velocities and rotation axes
%  PlotRotation(q,interval)     plot columns of rotation matrices of q,
%                               pause interval between figure updates in seconds
%  [q1,w1,t1] = PropagateEulerEq(q0,w0,I,t,@torque,odeoptions)
%                               Euler equation numerical propagator, see
%                               help quaternion.PropagateEulerEq
%  rp = Rodrigues(q)            Rodrigues parameters equivalent to q
%  Tp = RotateTensor(q,T)       rotations q converted to rotation matrices R,
%                               acting on 3x3 tensors T: Tp = R * T * R.'
%  vp = RotateVector(q,v,dim)   vp are 3 component vectors, rotations q acting
%                               on vectors v, uses rotation matrix multiplication
%  vp = RotateVectorQ(q,v,dim)  vp are 3 component vectors, rotations q acting
%                               on vectors v, uses quaternion multiplication,
%                               RotateVector is 7 times faster than RotateVectorQ
%  R  = RotationMatrix(q)       3x3 rotation matrices equivalent to q
%
% Note:
%  In all rotation operations, the rotations operate from left to right on
%  3x1 column vectors and create rotated vectors, not representations of
%  those vectors in rotated coordinate systems.
%  For Euler angles, '123' means rotate the vector about x first, about y
%  second, about z third, i.e.:
%  vp = rotate(z,angle(3)) * rotate(y,angle(2)) * rotate(x,angle(1)) * v
%
% Ordinary methods:
%  n  = abs(q)                  quaternion norm, n = sqrt( sum( q.e.^2 ))
%  q3 = bsxfun(func,q1,q2)      binary singleton expansion of operation func
%  c  = complex(q)              complex( real(q), imag(q) )
%  Q  = ComplexMatrix(q)        convert quaternions into complex 2x2 matrices
%  qc = conj(q)                 quaternion conjugate, qc.e =
%                               [q.e(1);-q.e(2);-q.e(3);-q.e(4)]
%  qt = ctranspose(q)           qt = q'; quaternion conjugate transpose,
%                               2-D (or scalar) q only
%  qp = cumprod(q,dim)          cumulative quaternion array product over
%                               dimension dim
%  qs = cumsum(q,dim)           cumulative quaternion array sum over dimension dim
%  qd = diff(q,ord,dim)         quaternion array difference, order ord, over
%                               dimension dim
%  ans = display(q)             'q = ( e(1) ) + i( e(2) ) + j( e(3) ) + k( e(4) )'
%  d  = dot(q1,q2)              quaternion element dot product, d = dot(q1.e,q2.e)
%  d  = double(q)               d = q.e; if size(q) == [s1,s2,...], size(d) ==
%                               [4,s1,s2,...]
%  l  = eq(q1,q2)               quaternion equality, l = all( q1.e == q2.e )
%  l  = equiv(q1,q2,tol)        quaternion rotational equivalence, within
%                               tolerance tol, l = (q1 == q2) | (q1 == -q2)
%  qe = exp(q)                  quaternion exponential, v = q.e(2:4), qe.e =
%                               exp(q.e(1))*[cos(|v|);v.*sin(|v|)./|v|]
%  ei = imag(q)                 imaginary e(2) components
%  qi = interp1(t,q,ti,method)  interpolate quaternion array
%  qi = inverse(q)              quaternion inverse, qi = conj(q)./norm(q).^2,
%                               q .* qi = qi .* q = 1 for q ~= 0
%  l  = isequal(q1,q2,...)      true if equal sizes and values
%  l  = isequaln(q1,q2,...)     true if equal including NaNs
%  l  = isequalwithequalnans(q1,q2,...) true if equal including NaNs
%  l  = isfinite(q)             true if all( isfinite( q.e ))
%  l  = isinf(q)                true if any( isinf( q.e ))
%  l  = isnan(q)                true if any( isnan( q.e ))
%  ej = jmag(q)                 e(3) components
%  ek = kmag(q)                 e(4) components
%  q3 = ldivide(q1,q2)          quaternion left division, q3 = q1 \. q2 =
%                               inverse(q1) *. q2
%  ql = log(q)                  quaternion logarithm, v = q.e(2:4), ql.e =
%                               [log(|q|);v.*acos(q.e(1)./|q|)./|v|]
%  q3 = minus(q1,q2)            quaternion subtraction, q3 = q1 - q2
%  q3 = mldivide(q1,q2)         left division only defined for scalar q1
%  qp = mpower(q,p)             quaternion matrix power, qp = q^p, p scalar
%                               integer >= 0, q square quaternion matrix
%  q3 = mrdivide(q1,q2)         right division only defined for scalar q2
%  q3 = mtimes(q1,q2)           2-D matrix quaternion multiplication, q3 = q1 * q2
%  l  = ne(q1,q2)               quaternion inequality, l = ~all( q1.e == q2.e )
%  n  = norm(q)                 quaternion norm, n = sqrt( sum( q.e.^2 ))
%  [q,n] = normalize(q)         make quaternion norm == 1, unless q == 0,
%                               n = matrix of previous norms
%  q3 = plus(q1,q2)             quaternion addition, q3 = q1 + q2
%  qp = power(q,p)              quaternion power, qp = q.^p
%  qp = prod(q,dim)             quaternion array product over dimension dim
%  qp = product(q1,q2)          quaternion product of scalar quaternions,
%                               qp = q1 .* q2, noncommutative
%  q3 = rdivide(q1,q2)          quaternion right division, q3 = q1 ./ q2 =
%                               q1 .* inverse(q2)
%  er = real(q)                 real e(1) components
%  qs = slerp(q0,q1,t)          quaternion spherical linear interpolation
%  qr = sqrt(q)                 qr = q.^0.5, square root
%  qs = sum(q,dim)              quaternion array sum over dimension dim
%  q3 = times(q1,q2)            matrix component quaternion multiplication,
%                               q3 = q1 .* q2, noncommutative
%  qm = uminus(q)               quaternion negation, qm = -q
%  qp = uplus(q)                quaternion unitary plus, qp = +q
%  ev = vector(q)               vector e(2:4) components
%
% Author:
%  Mark Tincknell, MIT LL, 29 July 2011, revised 25 June 2015

properties (SetAccess = protected)
    e   = zeros(4,1);
end % properties

% Array constructors
methods
    function q = quaternion( varargin ) % (constructor)
        perm = [];
        sqz  = false;
        switch nargin

            case 0  % nargin == 0
                q.e = zeros(4,1);
                return;

            case 1  % nargin == 1
                siz = size( varargin{1} );
                nel = prod( siz );
                if nel == 0
                    q	= quaternion.empty;
                    return;
                elseif isa( varargin{1}, 'quaternion' )
                    q   = varargin{1};
                    return;
                elseif (nel == 1) || ~isreal( varargin{1}(:) )
                    for iel = nel : -1 : 1
                        q(iel).e = chop( [real(varargin{1}(iel)); ...
                                          imag(varargin{1}(iel)); ...
                                          0; ...
                                          0] );
                    end
                    q   = reshape( q, siz );
                    return;
                end
                [arg4, dim4, perm4] = finddim( varargin{1}, 4 );
                if dim4 > 0
                    siz(dim4)   = 1;
                    nel         = prod( siz );
                    if dim4 > 1
                        perm    = perm4;
                    else
                        sqz     = true;
                    end
                    for iel = nel : -1 : 1
                        q(iel).e = chop( arg4(:,iel) );
                    end
                else
                    [arg3, dim3, perm3] = finddim( varargin{1}, 3 );
                    if dim3 > 0
                        siz(dim3)   = 1;
                        nel         = prod( siz );
                        if dim3 > 1
                            perm    = perm3;
                        else
                            sqz     = true;
                        end
                        for iel = nel : -1 : 1
                            q(iel).e = chop( [0; arg3(:,iel)] );
                        end
                    else
                        error( 'quaternion:constructor:nargin1', ...
                               'Invalid input' );
                    end
                end

            case 2  % nargin == 2
% real-imaginary only (no j or k) inputs
                na  = cellfun( 'prodofsize', varargin );
                [nel, jel]  = max( na );
                if ~all( (na == 1) | (na == nel) )
                    error( 'quaternion:constructor:nargin2', ...
                           'All inputs must be singletons or have the same number of elements' );
                end
                siz = size( varargin{jel} );
                for iel = nel : -1 : 1
                    q(iel).e = chop( [varargin{1}(min(iel,na(1))); ...
                                      varargin{2}(min(iel,na(2))); ...
                                      0;
                                      0] );
                end

            case 3  % nargin == 3
% vector inputs (no real, only i, j, k)
                na  = cellfun( 'prodofsize', varargin );
                [nel, jel]  = max( na );
                if ~all( (na == 1) | (na == nel) )
                    error( 'quaternion:constructor:nargin3', ...
                           'All inputs must be singletons or have the same number of elements' );
                end
                siz = size( varargin{jel} );
                for iel = nel : -1 : 1
                    q(iel).e = chop( [0; ...
                                      varargin{1}(min(iel,na(1))); ...
                                      varargin{2}(min(iel,na(2))); ...
                                      varargin{3}(min(iel,na(3)))] );
                end

            otherwise   % nargin >= 4
                na  = cellfun( 'prodofsize', varargin );
                [nel, jel]  = max( na );
                if ~all( (na == 1) | (na == nel) )
                    error( 'quaternion:constructor:nargin4', ...
                           'All inputs must be singletons or have the same number of elements' );
                end
                siz = size( varargin{jel} );
                for iel = nel : -1 : 1
                    q(iel).e = chop( [varargin{1}(min(iel,na(1))); ...
                                      varargin{2}(min(iel,na(2))); ...
                                      varargin{3}(min(iel,na(3))); ...
                                      varargin{4}(min(iel,na(4)))] );
                end
        end % switch nargin

        if nel == 0
            q   = quaternion.empty;
        end
        q   = reshape( q, siz );
        if ~isempty( perm )
            q   = ipermute( q, perm );
        end
        if sqz
            q   = squeeze( q );
        end
    end % quaternion (constructor)

% Ordinary methods
    function n = abs( q )
        n   = q.norm;
    end % abs

    function q3 = bsxfun( func, q1, q2 )
% function q3 = bsxfun( func, q1, q2 )
% Binary Singleton Expansion for quaternion arrays. Apply the element by
% element binary operation specified by the function handle func to arrays
% q1 and q2. All dimensions of q1 and q2 must either agree or be length 1.
% Inputs:
%  func     function handle (e.g. @plus) of quaternion function or operator
%  q1(n1)   quaternion array
%  q2(n2)   quaternion array
% Output:
%  q3(n3)   quaternion array of function or operator outputs
%           size(q3) = max( size(q1), size(q2) )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        s1  = size( q1 );
        s2  = size( q2 );
        nd1 = length( s1 );
        nd2 = length( s2 );
        s1  = [s1, ones(1,nd2-nd1)];
        s2  = [s2, ones(1,nd1-nd2)];
        if ~all( (s1 == s2) | (s1 == 1) | (s2 == 1) )
            error( 'quaternion:bsxfun:notconformable', ...
                   'Non-singleton dimensions of q1 and q2 must match each other' );
        end
        c1  = num2cell( s1 );
        c2  = num2cell( s2 );
        s3  = max( s1, s2 );
        nd3 = length( s3 );
        n3  = prod( s3 );
        q3  = quaternion.nan( s3 );
        for i3 = 1 : n3
            [ix3{1:nd3}] = ind2sub( s3, i3 ); 
            ix1     = cellfun( @min, ix3, c1, 'UniformOutput', false );
            ix2     = cellfun( @min, ix3, c2, 'UniformOutput', false );
            q3(i3)  = func( q1(ix1{:}), q2(ix2{:}) );
        end
    end % bsxfun

    function c  = complex( q )
        c   = complex( real( q ), imag( q ));
    end % complex

    function qc = conj( q )
        d   = double( q );
        qc  = reshape( quaternion( d(1,:), -d(2,:), -d(3,:), -d(4,:) ), ...
                       size( q ));
    end % conj

    function qt = ctranspose( q )
        qt  = transpose( q.conj );
    end % ctranspose

    function qp = cumprod( q, dim )
% function qp = cumprod( q, dim )
% cumulative quaternion array product, dim defaults to first dimension of
% length > 1
        if isempty( q )
            qp  = q;
            return;
        end
        if (nargin < 2) || isempty( dim )
            [q, dim, perm]  = finddim( q, -2 );
        elseif dim > 1
            ndm  = ndims( q );
            perm = [ dim : ndm, 1 : dim-1 ];
            q    = permute( q, perm );
        end
        qp  = q;
        for is = 2 : size(q,1)
            qp(is,:) = qp(is-1,:) .* q(is,:);
        end
        if dim > 1
            qp  = ipermute( qp, perm );
        end
    end % cumprod

    function qs = cumsum( q, dim )
% function qs = cumsum( q, dim )
% cumulative quaternion array sum, dim defaults to first dimension of
% length > 1
        if isempty( q )
            qs  = q;
            return;
        end
        if (nargin < 2) || isempty( dim )
            [q, dim, perm]  = finddim( q, -2 );
        elseif dim > 1
            ndm  = ndims( q );
            perm = [ dim : ndm, 1 : dim-1 ];
            q    = permute( q, perm );
        end
        qs  = q;
        for is = 2 : size(q,1)
            qs(is,:) = qs(is-1,:) + q(is,:);
        end
        if dim > 1
            qs  = ipermute( qs, perm );
        end
    end % cumsum

    function qd = diff( q, ord, dim )
% function qd = diff( q, ord, dim )
% quaternion array difference, ord is the order of difference (default = 1)
% dim defaults to first dimension of length > 1
        if isempty( q )
            qd  = q;
            return;
        end
        if (nargin < 2) || isempty( ord )
            ord = 1;
        end
        if ord <= 0
            qd  = q;
            return;
        end
        if (nargin < 3) || isempty( dim )
            [q, dim, perm]  = finddim( q, -2 );
        elseif dim > 1
            ndm  = ndims( q );
            perm = [ dim : ndm, 1 : dim-1 ];
            q    = permute( q, perm );
        end
        siz = size( q );
        if siz(1) <= 1
            qd  = quaternion.empty;
            return;
        end
        qd  = quaternion.zeros( [(siz(1)-1), siz(2:end)] );
        for is = 1 : siz(1)-1
            qd(is,:) = q(is+1,:) - q(is,:);
        end
        ord = ord - 1;
        if ord > 0
            qd  = diff( qd, ord, 1 );
        end
        if dim > 1
            qd  = ipermute( qd, perm );
        end
    end % diff

    function display( q ) %#ok<DISPLAY>
        if ~isequal( get(0,'FormatSpacing'), 'compact' )
            disp(' ');
        end
        if isempty( q )
            fprintf( '%s \t= ([]) + i([]) + j([]) + k([])\n', inputname(1) )
            return;
        end
        siz = size( q );
        nel = [1 cumprod( siz )];
        ndm = length( siz );
        for iel = 1 : nel(end)
            if nel(end) == 1
                sub = '';
            else
                sub = ')';
                jel = iel - 1;
                for idm = ndm : -1 : 1
                    idx = floor( jel / nel(idm) ) + 1;
                    sub = [',' int2str(idx) sub]; %#ok<AGROW>
                    jel = rem( jel, nel(idm) );
                end
                sub(1)  = '(';
            end
            fprintf( '%s%s \t= (%-12.5g) + i(%-12.5g) + j(%-12.5g) + k(%-12.5g)\n', ...
                inputname(1), sub, q(iel).e )
        end
    end % display

    function d = dot( q1, q2 )
% function d = dot( q1, q2 )
% quaternion element dot product: d = dot( q1.e, q2.e ), using binary
% singleton expansion of quaternion arrays
% dn = dot( q1, q2 )/( norm(q1) * norm(q2) ) is the cosine of the angle in
% 4D space between 4D vectors q1.e and q2.e
        d   = squeeze( sum( bsxfun( @times, double( q1 ), double( q2 )), 1 ));
    end % dot

    function d = double( q )
        siz = size( q );
        d   = reshape( [q.e], [4 siz] );
        d   = chop( d );
    end % double

    function l = eq( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            l   = logical([]);
            return;
        elseif ne1 == 1
            siz = si2;
        elseif ne2 == 1
            siz = si1;
        elseif isequal( si1, si2 )
            siz = si1;
        else
            error( 'quaternion:eq:baddims', ...
                   'Matrix dimensions must agree' );
        end
        l   = bsxfun( @eq, [q1.e], [q2.e] );
        l   = reshape( all( l, 1 ), siz );
    end % eq

    function l = equiv( q1, q2, tol )
% function l  = equiv( q1, q2, tol )
% quaternion rotational equivalence, within tolerance tol,
% l = (q1 == q2) | (q1 == -q2)
% optional argument tol (default = eps) sets tolerance for difference
% from exact equality
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        if (nargin < 3) || isempty( tol )
            tol = eps;
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            l   = logical([]);
            return;
        elseif ne1 == 1
            siz = si2;
        elseif ne2 == 1
            siz = si1;
        elseif isequal( si1, si2 )
            siz = si1;
        else
            error( 'quaternion:equiv:baddims', ...
                   'Matrix dimensions must agree' );
        end
        dm  = chop( bsxfun( @minus, [q1.e], [q2.e] ), tol );
        dp  = chop( bsxfun( @plus,  [q1.e], [q2.e] ), tol );
        l   = all( (dm == 0) | (dp == 0), 1 );
        l   = reshape( l, siz );
    end % equiv

    function qe = exp( q )
% function qe = exp( q )
% quaternion exponential, v = q.e(2:4),
% qe.e = exp(q.e(1))*[cos(|v|);v.*sin(|v|)./|v|]
        d       = double( q );
        siz     = size( d );
        od      = ones( 1, ndims( q ));
        vn      = reshape( sqrt( sum( d(2:4,:).^2, 1 )), [1 siz(2:end)] );
        cv      = cos( vn );
        sv      = sin( vn );
        n0      = vn ~= 0;
        sv(n0)  = sv(n0) ./ vn(n0);
        sv      = repmat( sv, [3, od] );
        ex      = repmat( reshape( exp( d(1,:) ), [1 siz(2:end)] ), [4, od] );
        de      = ex .* [ cv; sv .* reshape( d(2:4,:), [3 siz(2:end)] )];
        qe      = reshape( quaternion( de(1,:), de(2,:), de(3,:), de(4,:) ), ...
                           size( q ));
    end % exp

    function ei = imag( q )
        siz = size( q );
        d   = double( q );
        ei  = reshape( d(2,:), siz );
    end % imag

    function qi = interp1( varargin )
% function qi = interp1( t, q, ti, method ) or
%          qi = q.interp1( t, ti, method )  or
%          qi = interp1( q, ti, method )
% Interpolate quaternion array. If q are orientation quaternions (i.e.
% normalized to 1), then -q is equivalent to q, and the sign of q to use as
% the second knot of the interpolation is chosen by which ever is closer to
% the first knot. Extrapolation (i.e. ti < min(t) or ti > max(t)) gives
% qi = quaternion.nan.
% Inputs:
%  t(nt)      array of ordinates (e.g. times); if t is not provided t=1:nt
%  q(nt,nq)   quaternion array
%  ti(ni)     array of query (interpolation) points, t(1) <= ti <= t(end)
%  method [OPTIONAL] 'slerp' or 'linear'; default = 'slerp'
% Output:
%  qi(ni,nq)  interpolated quaternion array
        nna = nnz( ~cellfun( @ischar, varargin ));
        im  = 4;
        if isa( varargin{1}, 'quaternion' )
            q   = varargin{1};
            siq = size( q );
            if nna == 2
                if size( q, 1 ) == 1
                    t = (1 : siq(2)).';
                else
                    t = (1 : siq(1)).';
                end
                ti  = varargin{2}(:);
                im  = 3;
            elseif isempty( varargin{2} )
                if size( q, 1 ) == 1
                    t = (1 : siq(2)).';
                else
                    t = (1 : siq(1)).';
                end
                ti  = varargin{3}(:);
            else
                t   = varargin{2}(:);
                ti  = varargin{3}(:);
            end
        elseif isa( varargin{2}, 'quaternion' )
            t   = varargin{1}(:);
            q   = varargin{2};
            ti  = varargin{3}(:);
            siq = size( q );
        else
            error( 'quaternion:interp1:notq', ...
                   'Input q must be a quaterion' );
        end
        neq = prod( siq );
        if neq == 0
            qi  = quaternion.empty;
            return;
        end
        nt  = numel( t );
        if siq(1) == nt
            dim = 1;
        else
            [q, dim, perm]  = finddim( q, nt );
            if dim == 0
                error( 'quaternion:interp1:badt', ...
                       'q must have a dimension the same size as t' );
            end
        end
        iNf = interp1( t, (1:nt).', ti );
        iN  = max( 1, min( nt-1, floor( iNf )));
        jN  = max( 2, min( nt,   ceil(  iNf )));
        iNm = repmat( iNf - iN, [1, neq / nt] );
% If q are orientation quaternions (i.e. all normalized to 1), then -q
% represents the same rotation. Pick the sign of +/-q that has the closest
% dot product to use as the second knot of the interpolation.
        qj  = q(jN,:);
        if all( abs( norm( q(:) ) - 1 ) <= eps(16) )
            qd  = dot( q(iN,:), qj );
            lq  = qd < -qd;
            qj(lq)  = -qj(lq);
        end
        if (length( varargin ) >= im) && ...
           (strncmpi( 'linear', varargin{im}, length( varargin{im} )))
            qi  = (1 - iNm) .* q(iN,:) + iNm .* qj;
        else
            qi  = slerp( q(iN,:), qj, iNm );
        end
        if length( siq ) > 2
            sin = siq;
            sin(dim) = numel( ti );
            sin = circshift( sin, [0, 1-dim] );
            qi  = reshape( qi, sin );
        end
        if dim > 1
            qi  = ipermute( qi, perm );
        end
    end % interp1

    function qi = inverse( q )
% function qi = inverse( q )
% quaternion inverse, qi = conj(q)/norm(q)^2, q*qi = qi*q = 1 for q ~= 0
        if isempty( q )
            qi  = q;
            return;
        end
        d   = double( q );
        d(2:4,:) = -d(2:4,:);
        n2  = repmat( sum( d.^2, 1 ), [4, ones(1,ndims(d)-1 )] );
        ne0 = n2 ~= 0;
        di  = Inf( size( d ));
        di(ne0)  = d(ne0) ./ n2(ne0);
        qi  = reshape( quaternion( di(1,:), di(2,:), di(3,:), di(4,:) ), ...
                       size( q ));
    end % inverse

    function l = isequal( q1, varargin )
% function l = isequal( q1, q2, ... )
        nar = numel( varargin );
        if nar == 0
            error( 'quaternion:isequal:noargs', ...
                   'Not enough input arguments' );
        end
        l   = false;
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        si1 = size( q1 );
        for iar = 1 : nar
            si2 = size( varargin{iar} );
            if (length( si1 ) ~= length( si2 )) || ...
               ~all( si1 == si2 )
                return;
            else
                if ~isa( varargin{iar}, 'quaternion' )
                    q2  = quaternion( ...
                          real(varargin{iar}), imag(varargin{iar}), 0, 0 );
                else
                    q2  = varargin{iar};
                end
                if ~isequal( [q1.e], [q2.e] )
                    return;
                end
            end
        end
        l   = true;
    end % isequal

    function l = isequaln( q1, varargin )
% function l = isequaln( q1, q2, ... )
        nar = numel( varargin );
        if nar == 0
            error( 'quaternion:isequaln:noargs', ...
                   'Not enough input arguments' );
        end
        l   = false;
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        si1 = size( q1 );
        for iar = 1 : nar
            si2 = size( varargin{iar} );
            if (length( si1 ) ~= length( si2 )) || ...
               ~all( si1 == si2 )
                return;
            else
                if ~isa( varargin{iar}, 'quaternion' )
                    q2  = quaternion( ...
                          real(varargin{iar}), imag(varargin{iar}), 0, 0 );
                else
                    q2  = varargin{iar};
                end
                if ~isequaln( [q1.e], [q2.e] )
                    return;
                end
            end
        end
        l   = true;
    end % isequaln

    function l = isequalwithequalnans( q1, varargin )
% function l = isequalwithequalnans( q1, q2, ... )
        nar = numel( varargin );
        if nar == 0
            error( 'quaternion:isequalwithequalnans:noargs', ...
                   'Not enough input arguments' );
        end
        l   = false;
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        si1 = size( q1 );
        for iar = 1 : nar
            si2 = size( varargin{iar} );
            if (length( si1 ) ~= length( si2 )) || ...
               ~all( si1 == si2 )
                return;
            else
                if ~isa( varargin{iar}, 'quaternion' )
                    q2  = quaternion( ...
                          real(varargin{iar}), imag(varargin{iar}), 0, 0 );
                else
                    q2  = varargin{iar};
                end
                if ~isequalwithequalnans( [q1.e], [q2.e] ) %#ok<FPARK>
                    return;
                end
            end
        end
        l   = true;
    end % isequalwithequalnans

    function l = isfinite( q )
% function l = isfinite( q ), l = all( isfinite( q.e ))
        d   = [q.e];
        l   = reshape( all( isfinite( d ), 1 ), size( q ));
    end % isfinite

    function l = isinf( q )
% function l = isinf( q ), l = any( isinf( q.e ))
        d   = [q.e];
        l   = reshape( any( isinf( d ), 1 ), size( q ));
    end % isinf

    function l = isnan( q )
% function l = isnan( q ), l = any( isnan( q.e ))
        d   = [q.e];
        l   = reshape( any( isnan( d ), 1 ), size( q ));
    end % isnan

    function ej = jmag( q )
        siz = size( q );
        d   = double( q );
        ej  = reshape( d(3,:), siz );
    end % jmag

    function ek = kmag( q )
        siz = size( q );
        d   = double( q );
        ek  = reshape( d(4,:), siz );
    end % kmag

    function q3 = ldivide( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            q3  = quaternion.empty;
            return;
        elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1)
            error( 'quaternion:ldivide:baddims', ...
                   'Matrix dimensions must agree' );
        end
        for iel = max( ne1, ne2 ) : -1 : 1
            q3(iel) = product( q1(min(iel,ne1)).inverse, ...
                               q2(min(iel,ne2)) );
        end
        if ne2 > ne1
            q3  = reshape( q3, si2 );
        else
            q3  = reshape( q3, si1 );
        end
    end % ldivide

    function ql = log( q )
% function ql = log( q )
% quaternion logarithm, v = q.e(2:4), ql.e = [log(|q|);v.*acos(q.e(1)./|q|)./|v|]
% logarithm of negative real quaternions is ql.e = [log(|q|);pi;0;0]
        d       = double( q );
        d2      = d.^2;
        siz     = size( d );
        od      = ones( 1, ndims( q ));
        [vn,qn] = deal( zeros( [1 siz(2:end)] ));
        vn(:)   = sqrt( sum( d2(2:4,:), 1 ));
        qn(:)   = sqrt( sum( d2(1:4,:), 1 ));
        lq      = log( qn );
        d1      = reshape( d(1,:), [1 siz(2:end)] );
        nq      = qn ~= 0;
        d1(nq)  = d1(nq) ./ qn(nq);
        ac      = acos( d1 );
        nv      = vn ~= 0;
        ac(nv)  = ac(nv) ./ vn(nv);
        ac      = reshape( repmat( ac, [3, od] ), 3, [] );
        va      = reshape( d(2:4,:) .* ac, [3 siz(2:end)] );
        nn      = (d1 < 0) & (vn == 0);
        va(1,nn)= pi;
        dl      = [ lq; va ];
        ql      = reshape( quaternion( dl(1,:), dl(2,:), dl(3,:), dl(4,:) ), ...
                           size( q ));
    end % log

    function q3 = minus( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            q3  = quaternion.empty;
            return;
        elseif ne1 == 1
            siz = si2;
        elseif ne2 == 1
            siz = si1;
        elseif isequal( si1, si2 )
            siz = si1;
        else
            error( 'quaternion:minus:baddims', ...
                   'Matrix dimensions must agree' );
        end
        d3  = bsxfun( @minus, [q1.e], [q2.e] );
        q3  = quaternion( d3(1,:), d3(2,:), d3(3,:), d3(4,:) );
        q3  = reshape( q3, siz );
    end % minus

    function q3 = mldivide( q1, q2 )
% function q3 = mldivide( q1, q2 ), left division only defined for scalar q1
        if numel( q1 ) > 1
            error( 'quaternion:mldivide:undefined', ...
                   'Left matix division undefined for quaternion arrays' );
        end
        q3  = ldivide( q1, q2 );
    end % mldivide

    function qp = mpower( q, p )
% function qp = mpower( q, p ), quaternion matrix power
        siq = size( q );
        neq = prod( siq );
        nep = numel( p );
        if neq == 1
            qp  = power( q, p );
            return;
        elseif isa( p, 'quaternion' )
            error( 'quaternion:mpower:undefined', ...
                   'Quaternion as matrix exponent is not defined' );
        end
        if (neq == 0) || (nep == 0)
            qp  = quaternion.empty;
            return;
        elseif (nep > 1) || (mod( p, 1 ) ~= 0) || (p < 0) || ...
               (numel( siq ) > 2) || (siq(1) ~= siq(2))
            error( 'quaternion:mpower:notconformable', ...
                   'Inputs must be a scalar non-negative integer power and a square quaternion matrix' );
        elseif p == 0
            qp  = quaternion.eye( siq(1) );
            return;
        end
        qp  = q;
        for ip = 2 : p
            qp  = qp * q;
        end
    end % mpower

    function q3 = mrdivide( q1, q2 )
% function q3 = mrdivide( q1, q2 ), right division only defined for scalar q2
        if numel( q2 ) > 1
            error( 'quaternion:mrdivide:undefined', ...
                   'Right matix division undefined for quaternion arrays' );
        end
        q3  = rdivide( q1, q2 );
    end % mrdivide

    function q3 = mtimes( q1, q2 )
% function q3 = mtimes( q1, q2 )
% q3 = matrix quaternion product of 2-D conformable quaternion matrices q1
% and q2
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 1) || (ne2 == 1)
            q3  = times( q1, q2 );
            return;
        end
        if (length( si1 ) ~= 2) || (length( si2 ) ~= 2)
            error( 'quaternion:mtimes:not2D', ...
                   'Input arguments must be 2-D' );
        end
        if si1(2) ~= si2(1)
            error( 'quaternion:mtimes:notconformable', ...
                   'Inner matrix dimensions must agree' );
        end
        q3  = repmat( quaternion, [si1(1) si2(2)] );
        for i1 = 1 : si1(1)
            for i2 = 1 : si2(2)
                for i3 = 1 : si1(2)
                    q3(i1,i2) = q3(i1,i2) + product( q1(i1,i3), q2(i3,i2) );
                end
            end
        end
    end % mtimes

    function l = ne( q1, q2 )
        l   = ~eq( q1, q2 );
    end % ne

    function n = norm( q )
        n   = shiftdim( sqrt( sum( double( q ).^2, 1 )), 1 );
    end % norm

    function [q, n] = normalize( q )
% function [q, n] = normalize( q )
% q = quaternions with norm == 1 (unless q == 0), n = former norms
        siz = size( q );
        nel = prod( siz );
        if nel == 0
            if nargout > 1
                n   = zeros( siz );
            end
            return;
        end
        d   = double( q );
        n   = sqrt( sum( d.^2, 1 ));
        if all( n(:) == 1 )
            if nargout > 1
                n   = shiftdim( n, 1 );
            end
            return;
        end
        n4  = repmat( n, [4, ones(1,ndims(n)-1)] );
        ne0 = (n4 ~= 0) & (n4 ~= 1);
        d(ne0)  = d(ne0) ./ n4(ne0);
        q   = reshape( quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ), siz );
        if nargout > 1
            n   = shiftdim( n, 1 );
        end
    end % normalize

    function q3 = plus( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            q3  = quaternion.empty;
            return;
        elseif ne1 == 1
            siz = si2;
        elseif ne2 == 1
            siz = si1;
        elseif isequal( si1, si2 )
            siz = si1;
        else
            error( 'quaternion:plus:baddims', ...
                   'Matrix dimensions must agree' );
        end
        d3  = bsxfun( @plus, [q1.e], [q2.e] );
        q3  = quaternion( d3(1,:), d3(2,:), d3(3,:), d3(4,:) );
        q3  = reshape( q3, siz );
    end % plus

    function qp = power( q, p )
% function qp = power( q, p ), quaternion power
        siq = size( q );
        sip = size( p );
        neq = prod( siq );
        nep = prod( sip );
        if (neq == 0) || (nep == 0)
            qp  = quaternion.empty;
            return;
        elseif ~isequal( siq, sip ) && (neq ~= 1) && (nep ~= 1)
            error( 'quaternion:power:baddims', ...
                   'Matrix dimensions must agree' );
        end
        qp  = exp( p .* log( q ));
    end % power

    function qp = prod( q, dim )
% function qp = prod( q, dim )
% quaternion array product over dimension dim
% dim defaults to first dimension of length > 1
        if isempty( q )
            qp  = q;
            return;
        end
        if (nargin < 2) || isempty( dim )
            [q, dim, perm]  = finddim( q, -2 );
        elseif dim > 1
            ndm  = ndims( q );
            perm = [ dim : ndm, 1 : dim-1 ];
            q    = permute( q, perm );
        end
        siz = size( q );
        qp  = reshape( q(1,:), [1 siz(2:end)] );
        for is = 2 : siz(1)
            qp(1,:) = qp(1,:) .* q(is,:);
        end
        if dim > 1
            qp  = ipermute( qp, perm );
        end
    end % prod

    function q3 = product( q1, q2 )
% function q3 = product( q1, q2 )
% q3 = quaternion product of scalar quaternions q1 and q2
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        if (numel( q1 ) ~= 1) || (numel( q2 ) ~= 1)
            error( 'quaternion:product:onlyscalars', ...
                   'product not defined for arrays, use mtimes or times' );
        end
        ee  = q1.e * q2.e.';
        eo  = [ee(1,1) - ee(2,2) - ee(3,3) - ee(4,4); ...
               ee(1,2) + ee(2,1) + ee(3,4) - ee(4,3); ...
               ee(1,3) - ee(2,4) + ee(3,1) + ee(4,2); ...
               ee(1,4) + ee(2,3) - ee(3,2) + ee(4,1)];
        eo  = chop( eo );
        q3  = quaternion( eo(1), eo(2), eo(3), eo(4) );
    end % product

    function q3 = rdivide( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            q3  = quaternion.empty;
            return;
        elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1)
            error( 'quaternion:rdivide:baddims', ...
                   'Matrix dimensions must agree' );
        end
        for iel = max( ne1, ne2 ) : -1 : 1
            q3(iel) = product( q1(min(iel,ne1)), ...
                               q2(min(iel,ne2)).inverse );
        end
        if ne2 > ne1
            q3  = reshape( q3, si2 );
        else
            q3  = reshape( q3, si1 );
        end
    end % rdivide

    function er = real( q )
        siz = size( q );
        d   = double( q );
        er  = reshape( d(1,:), siz );
    end % real

    function qs = slerp( q0, q1, t )
% function qs = slerp( q0, q1, t )
% quaternion spherical linear interpolation, qs = q0.*(q0.inverse.*q1).^t,
% default t = 0.5; see http://en.wikipedia.org/wiki/Slerp
        if (nargin < 3) || isempty( t )
            t   = 0.5;
        end
        qs  = q0 .* (q0.inverse .* q1).^t;
    end % slerp

    function qr = sqrt( q )
        qr  = q.^0.5;
    end % sqrt

    function qs = sum( q, dim )
% function qs = sum( q, dim )
% quaternion array sum over dimension dim
% dim defaults to first dimension of length > 1
        if isempty( q )
            qs  = q;
            return;
        end
        if (nargin < 2) || isempty( dim )
            [q, dim, perm]  = finddim( q, -2 );
        elseif dim > 1
            ndm  = ndims( q );
            perm = [ dim : ndm, 1 : dim-1 ];
            q    = permute( q, perm );
        end
        siz = size( q );
        qs  = reshape( q(1,:), [1 siz(2:end)] );
        for is = 2 : siz(1)
            qs(1,:) = qs(1,:) + q(is,:);
        end
        if dim > 1
            qs  = ipermute( qs, perm );
        end
    end % sum

    function q3 = times( q1, q2 )
        if ~isa( q1, 'quaternion' )
            q1  = quaternion( real(q1), imag(q1), 0, 0 );
        end
        if ~isa( q2, 'quaternion' )
            q2  = quaternion( real(q2), imag(q2), 0, 0 );
        end
        si1 = size( q1 );
        si2 = size( q2 );
        ne1 = prod( si1 );
        ne2 = prod( si2 );
        if (ne1 == 0) || (ne2 == 0)
            q3  = quaternion.empty;
            return;
        elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1)
            error( 'quaternion:times:baddims', ...
                   'Matrix dimensions must agree' );
        end
        for iel = max( ne1, ne2 ) : -1 : 1
            q3(iel) = product( q1(min(iel,ne1)), q2(min(iel,ne2)) );
        end
        if ne2 > ne1
            q3  = reshape( q3, si2 );
        else
            q3  = reshape( q3, si1 );
        end
    end % times

    function qm = uminus( q )
        d   = -double( q );
        qm  = reshape( quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ), ...
                       size( q ));
    end % uminus

    function qp = uplus( q )
        qp  = q;
    end % uplus

    function ev = vector( q )
        siz = size( q );
        d   = double( q );
        ev  = reshape( d(2:4,:), [3 siz] );
    end % vector

    function [angle, axis] = AngleAxis( q )
% function [angle, axis] = AngleAxis( q )  or  [angle, axis] = q.AngleAxis
% Construct angle-axis pairs equivalent to quaternion rotations
% Input:
%  q        quaternion array
% Outputs:
%  angle    rotation angles in radians, 0 <= angle <= 2*pi
%  axis     3xN or Nx3 rotation axis unit vectors
% Note: angle and axis are constructed so at least 2 out of 3 elements of
% axis are >= 0.
        siz         = size( q );
        ndm         = length( siz );
        [angle, s]  = deal( zeros( siz ));
        axis        = zeros( [3 siz] );
        nel         = prod( siz );
        if nel == 0
            return;
        end
        [q, n]      = normalize( q );
        d           = double( q );
        neg         = repmat( reshape( d(1,:) < 0, [1 siz] ), ...
                              [4, ones(1,ndm)] );
        d(neg)      = -d(neg);
        angle(1:end)= 2 * acos( d(1,:) );
        s(1:end)    = sin( 0.5 * angle );
        angle(n==0) = 0;
        s(s==0)     = 1;
        s3          = shiftdim( s, -1 );
        axis(1:end) = bsxfun( @rdivide, reshape( d(2:4,:), [3 siz] ), s3 );
        axis(1,(mod(angle,2*pi)==0)) = 1;
        angle       = chop( angle );
        axis        = chop( axis );
% Flip axis so at least 2 out of 3 elements are >= 0
        flip        = (sum( axis < 0, 1 ) > 1) | ...
                      ((sum( axis == 0, 1 ) == 2) & (any( axis < 0, 1 ) == 1));
        angle(flip) = 2 * pi - angle(flip);
        flip        = repmat( flip, [3, ones(1,ndm)] );
        axis(flip)  = -axis(flip);
        axis        = squeeze( axis );
    end % AngleAxis

    function Q = ComplexMatrix( q )
% function Q = ComplexMatrix( q )  or  Q = q.ComplexMatrix
% Convert quaternions into special complex 2x2 matrices. Matrix algebra
% with Q (e.g. sum, difference, matrix product, matrix inverse) is
% equivalent to the same operation with quaternions. Quaternions can be
% created from 2x2 complex matrices with quaternion.complexmatrix(Q)
% (lower case).
% Input:
%  q        array of N quaternions, q.e = [A; B; C; D]
% Output:
%  Q        2x2xN array of complex matrices
%           Q = [ A + B*i, C - D*i;
%                -C - D*i, A - B*i]
        siz = size( q );
        d   = double( q );
        Q   = [complex( d(1,:), d(2,:));
               complex(-d(3,:),-d(4,:));
               complex( d(3,:),-d(4,:));
               complex( d(1,:),-d(2,:))];
        Q   = squeeze( reshape( Q, [2, 2, siz] ));
    end % ComplexMatrix

    function qd = Derivative( varargin )
% function qd = Derivative( q, w )  or  qd = q.Derivative( w )
% Inputs:
%  q        quaternion array
%  w        3xN or Nx3 element angle rate vectors in radians/s
% Output:
%  qd       quaternion derivatives, qd = 0.5 * q * quaternion(w)
        if isa( varargin{1}, 'quaternion' )
            qd  = 0.5 .* varargin{1} .* quaternion( varargin{2} );
        else
            qd  = 0.5 .* varargin{2} .* quaternion( varargin{1} );
        end
    end % Derivative

    function angles = EulerAngles( varargin )
% function angles = EulerAngles( q, axes )  or  angles = q.EulerAngles( axes )
% Construct Euler angle triplets equivalent to quaternion rotations
% Inputs:
%  q        quaternion array
%  axes     axes designation strings (e.g. '123' = xyz) or cell strings
%           (e.g. {'123'})
% Output:
%  angles   3 element Euler Angle vectors in radians
        ics     = cellfun( @ischar, varargin );
        if any( ics )
            varargin{ics} = cellstr( varargin{ics} );
        else
            ics = cellfun( @iscellstr, varargin );
        end
        if ~any( ics )
            error( 'quaternion:EulerAngles:badaxes', ...
                   'Must provide axes as a string (e.g. ''123'') or cell string (e.g. {''123''})' );
        end
        siv     = cellfun( @size, varargin, 'UniformOutput', false );
        axes    = varargin{ics};
        six     = siv{ics};
        nex     = prod( six );
        q       = varargin{~ics};
        siq     = siv{~ics};
        neq     = prod( siq );
        if neq == 1
            siz = six;
            nel = nex;
        elseif nex == 1
            siz = siq;
            nel = neq;
        elseif nex == neq
            siz = siq;
            nel = neq;
        else
            error( 'quaternion:EulerAngles:notconformable', ...
                   'Must have compatible dimensions for quaternion and axes' );
        end
        angles  = zeros( [3 siz] );
        q       = normalize( q );
        for jel = 1 : nel
            iel = min( jel, neq );
            switch axes{min(jel,nex)}
                case {'121', 'xyx', 'XYX', 'iji'}
                    angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(3)- ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)));
                    angles(2,iel) = acos(q(iel).e(1).^2+q(iel).e(2).^2- ...
                        q(iel).e(3).^2-q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(3).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(4)));
                case {'123', 'xyz', 'XYZ', 'ijk'}
                    angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(4).*q(iel).e(3)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(3).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(4)));
                    angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(1).^2+ ...
                        q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2));
                case {'131', 'xzx', 'XZX', 'iki'}
                    angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(4).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(3)));
                    angles(2,iel) = acos(q(iel).e(1).^2+q(iel).e(2).^2- ...
                        q(iel).e(3).^2-q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(4)- ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)));
                case {'132', 'xzy', 'XZY', 'ikj'}
                    angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)- ...
                        q(iel).e(4).*q(iel).e(3)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)));
                    angles(3,iel) = atan2(2.*(q(iel).e(3).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(4)),(q(iel).e(1).^2+ ...
                        q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2));
                case {'212', 'yxy', 'YXY', 'jij'}
                    angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)- ...
                        q(iel).e(3).*q(iel).e(4)));
                    angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2+ ...
                        q(iel).e(3).^2-q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(3)- ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)));
                case {'213', 'yxz', 'YXZ', 'jik'}
                    angles(1,iel) = atan2(2.*(q(iel).e(3).*q(iel).e(1)- ...
                        q(iel).e(4).*q(iel).e(2)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)));
                    angles(3,iel) = atan2(2.*(q(iel).e(4).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(3)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2));
                case {'231', 'yzx', 'YZX', 'jki'}
                    angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(1).^2+ ...
                        q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(4).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(3)));
                    angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2));
                case {'232', 'yzy', 'YZY', 'jkj'}
                    angles(1,iel) = atan2((q(iel).e(3).*q(iel).e(4)- ...
                        q(iel).e(2).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)));
                    angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2+ ...
                        q(iel).e(3).^2-q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)),(q(iel).e(4).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(3)));
                case {'312', 'zxy', 'ZXY', 'kij'}
                    angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(3)+ ...
                        q(iel).e(4).*q(iel).e(1)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(1)- ...
                        q(iel).e(3).*q(iel).e(4)));
                    angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2));
                case {'313', 'zxz', 'ZXZ', 'kik'}
                    angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(4)- ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)));
                    angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2- ...
                        q(iel).e(3).^2+q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)- ...
                        q(iel).e(3).*q(iel).e(4)));
                case {'321', 'zyx', 'ZYX', 'kji'}
                    angles(1,iel) = atan2(2.*(q(iel).e(4).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(3)),(q(iel).e(1).^2+ ...
                        q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2));
                    angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)));
                    angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)- ...
                        q(iel).e(3).*q(iel).e(4)),(q(iel).e(1).^2- ...
                        q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2));
                case {'323', 'zyz', 'ZYZ', 'kjk'}
                    angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(1)+ ...
                        q(iel).e(3).*q(iel).e(4)),(q(iel).e(3).*q(iel).e(1)- ...
                        q(iel).e(2).*q(iel).e(4)));
                    angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2- ...
                        q(iel).e(3).^2+q(iel).e(4).^2);
                    angles(3,iel) = atan2((q(iel).e(3).*q(iel).e(4)- ...
                        q(iel).e(2).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(4)+ ...
                        q(iel).e(3).*q(iel).e(1)));
                otherwise
                    error( 'quaternion:EulerAngles:badaxes', ...
                           'Invalid output Euler angle axes' );
            end % switch axes
        end % for iel
        angles  = chop( angles );
    end % EulerAngles

    function q1 = Integral( q0, t, w, varargin )
% Integrate angular velocities over time (using ode45) to obtain the
% orientation quaternions at those times, starting from initial scalar q0.
% Angular velocities (and rotation axes) are computed at intermediate times
% by spline interpolation. Use q0 = quaternion(1,0,0,0) as the initial
% value unless there is an initial orientation.
%
% Calling syntax 1:
% function q1 = Integral( q0, t, w, odeoptions )
% Inputs:
%  q0           initial orientation quaternion (normalized, scalar)
%  t(nt)        initial and subsequent (or previous) times t = [t0,t1,...]
%               (monotonic)
%  w(3,nt)      3D angular velocity vectors, radians/(unit time)
%  odeoptions [OPTIONAL] ode45 options
%
% Calling syntax 2:
% function q1 = Integral( q0, t, omega, axis, odeoptions )
% Inputs:
%  q0           initial orientation quaternion (normalized, scalar)
%  t(nt)        initial and subsequent (or previous) times t = [t0,t1,...]
%               (monotonic)
%  omega(nt)    angular velocities, radians/(unit time)
%  axis(3,nt)   3D rotation axis vectors (normalized to unit vectors
%               internally)
%  odeoptions [OPTIONAL] ode45 options
%
% Output (either syntax):
%  q1(nt)       array of normalized quaternions at times t
%
% Calls:
%  Derivative   quaternion derivative method
%  odeset       matlab ode options setter
%  ode45        matlab ode numerical differential equation integrator
        if (nargin > 3) && isnumeric( varargin{1} ) && ...
           (numel( varargin{1} ) >= 3)
            omega       = w;
            [axis, dim] = finddim( varargin{1}, 3 );
            if dim == 0
                error( 'quaternion:Integral:badaxis', ...
                       'axis must have a dimension of size 3' );
            end
            axis        = unitvector( axis(1:3,:), 1 );
            w           = bsxfun( @times, omega(:).', axis );
            varargin    = varargin(2:end);
        end
        [w, dim]    = finddim( w, 3 );
        if dim == 0
            error( 'quaternion:Integral:badw', ...
                   'w must have a dimension of size 3' );
        end
        w           = reshape( w, 3, [] );
        nw          = size( w, 2 );
        wend        = w(:,end);
        nt          = numel( t );
        w           = [ w, wend(:,ones(1,nt-nw)) ];
        if isempty( q0 )
            y0      = [ 1; 0; 0; 0 ];
        else
            q0      = q0.normalize;
            y0      = q0.e;
        end
        if isempty( t )
            nt      = nw;
            t       = 1 : nt;
        end
        wpp         = spline( t, w );
        options     = odeset( varargin{:} );
        [~, Y]      = ode45( @Funct, t, y0, options );
            function yd = Funct( ti, yi )
                qi  = quaternion( yi(1), yi(2), yi(3), yi(4) );
                qi  = qi.normalize;
                wi  = ppval( wpp, ti );
                yd  = double( qi.Derivative( wi ));
            end
        q1          = quaternion( Y(:,1), Y(:,2), Y(:,3), Y(:,4) );
        q1          = q1.normalize;
        neg         = real( q1 ) < 0;
        q1(neg)     = -q1(neg);     % keep real element >= 0
        q1          = reshape( q1, size(t) );
    end % Integral

    function mrp = ModifiedRodrigues( q )
% function mrp = ModifiedRodrigues( q )  or  rp = q.ModifiedRodrigues
% Construct Modified Rodrigues parameters from rotation quaternions
% Input:
%  q        quaternion array
% Output:
%  mrp      Modified Rodrigues parameter array, mrp = axis .* tan(angle/4)
        [angle, axis] = q.AngleAxis;
        if size( angle, 1 ) ~= 1
            angle = shiftdim( angle, -1 );
        end
        mrp = bsxfun( @times, tan( 0.25 * angle ), axis );
    end % ModifiedRodrigues

    function [omega, axis] = OmegaAxis( q, t, dim )
% function [omega, axis] = OmegaAxis( q, t, dim )  or
%          [omega, axis] = q.OmegaAxis( t, dim )
% Estimate instantaneous angular velocities and rotation axes from a time
% series of quaternions. The angular velocity vector omegav is computed by:
%  omegav(:,1) = vector( 2*log( q(1) * inverse(q(2)) )/(t(2) - t(1)) );
%  omegav(:,i) = vector(...
%     (log( q(i-1) * inverse(q(i)) ) + log( q(i) * inverse(q(i+1))) )/...
%     (0.5*(t(i+1) - t(i-1))) );
%  omegav(:,end) = vector( 2*log( q(end-1) * inverse(q(end)) )/...
%     (t(end) - t(end-1)) );
%  [axis, omega] = unitvector( omegav );
% Inputs:
%  q          array of normalized (rotation) quaternions
%  t   [OPT]  array of monotonically increasing (or decreasing) times.
%             if omitted or empty, unit time steps are assumed.
%             t must either be a vector with the same length as dimension
%             dim of q, or the same size as q.
%  dim [OPT]  dimension of q that is varying in time; if omitted or empty,
%             the first non-singleton dimension is used.
% Outputs:
%  omega      array of instantaneous angular velocities, radians/(unit time)
%             omega >= 0
%  axis       instantaneous 3D rotation axis unit vectors at each time
        if isempty( q )
            omega   = [];
            axis    = [];
            return;
        end
        if (nargin < 3) || isempty( dim )
            if (nargin > 1) && ~isempty( t )
                siq = size( q );
                sit = size( t );
                if isequal( siq, sit )
                    dim   = find( siq > 1, 1 );
                else
                    dim   = find( siq == length( t ), 1 );
                end
                if isempty( dim )
                    error( 'quaternion:OmegaAxis:notconformable', ...
                           'size of t must agree with at least one dimension of q' );
                elseif dim > 1
                    ndm   = ndims( q );
                    perm  = [ dim : ndm, 1 : dim-1 ];
                    q     = permute( q, perm );
                    if isequal( siq, sit )
                        t = permute( t, perm );
                    end
                end
            else
                [q, dim, perm] = finddim( q, -2 );
                if dim == 0
                    omega = 0;
                    axis  = unitvector( q.e(2:4), 1 );
                    return;
                end
            end
        elseif dim > 1
            ndm   = ndims( q );
            perm  = [ dim : ndm, 1 : dim-1 ];
            q     = permute( q, perm );
        end
        n   = norm( q );
        if ~all( abs( n(:) - 1 ) < eps(16) )
            error( 'quaternion:OmegaAxis:qnotnorm', ...
                   'q must be normalized' );
        end
        siq = size( q );
        if (nargin < 2) || isempty( t )
            t   = repmat( (0 : (siq(1)-1)).', [1 siq(2:end)] );
        elseif length( t ) == siq(1)
            t   = repmat( t(:), [1 siq(2:end)] );
        elseif ~isequal( siq, size( t ))
            error( 'quaternion:OmegaAxis:notconformable', ...
                   'size of t must match size of q' );
        end
        dt            = zeros( siq );
        difft         = diff( t, 1 );
        dt(1,:)       = difft(1,:);
        dt(2:end-1,:) = 0.5 *( difft(1:end-1,:) + difft(2:end,:) );
        dt(end,:)     = difft(end,:);
        dq            = quaternion.zeros( siq );
        q1iq2         = q(1:end-1,:) .* inverse( q(2:end,:) );
        neg           = real( q1iq2 ) < 0;
        q1iq2(neg)    = -q1iq2(neg);    % keep real element >= 0
        derivq        = log( q1iq2 );
        dq(1,:)       = 2 .* derivq(1,:);
        dq(2:end-1,:) = derivq(1:end-1,:) + derivq(2:end,:);
        dq(end,:)     = 2 .* derivq(end,:);
        omegav        = vector( dq );   % angular velocity vectors
        [axis, omega] = unitvector( omegav, 1 );
        omega         = reshape( omega(1,:), siq )./ dt;
        axis          = -axis;
        if dim > 1
            axis      = ipermute( axis, [1, 1+perm] );
            omega     = ipermute( omega, perm );
        end
    end % OmegaAxis

    function PlotRotation( q, interval )
% function PlotRotation( q, interval )  or  q.PlotRotation( interval )
% Inputs:
%  q          quaternion array
%  interval   pause between figure updates in seconds, default = 0.1
% Output:
%  figure plotting the 3 Cartesian axes orientations for the series of
%  quaternions in array q
        if (nargin < 2) || isempty( interval )
            interval    = 0.1;
        end
        nel = numel( q );
        or  = zeros(1,3);
        ax  = eye(3);
        alx = zeros( nel, 3, 3 );
        newplot;
        for iel = 1 : nel
%           plot3( [ or; ax(:,1).' ], [ or ; ax(:,2).' ], [ or; ax(:,3).' ], ':' );
            plot3( [ or; ax(1,:) ], [ or ; ax(2,:) ], [ or; ax(3,:) ], ':' );
            hold on
            set( gca, 'Xlim', [-1 1], 'Ylim', [-1 1], 'Zlim', [-1 1] );
            xlabel( 'x' );
            ylabel( 'y' );
            zlabel( 'z' );
            grid on
            nax = q(iel).RotationMatrix;
            alx(iel,:,:)    = nax; 
%           plot3( [ or; nax(:,1).' ], [ or ; nax(:,2).' ], [ or; nax(:,3).' ], '-', 'LineWidth', 2 );
            plot3( [ or; nax(1,:) ], [ or ; nax(2,:) ], [ or; nax(3,:) ], '-', 'LineWidth', 2 );
%           plot3( alx(1:iel,:,1), alx(1:iel,:,2), alx(1:iel,:,3), '*' );
            plot3( squeeze(alx(1:iel,1,:)), squeeze(alx(1:iel,2,:)), squeeze(alx(1:iel,3,:)), '*' );
            if interval
                pause( interval );
            end
            hold off
        end
    end % PlotRotation

    function [q1, w1, t1] = PropagateEulerEq( q0, w0, I, t, torque, varargin )
% function [q1, w1, t1] = PropagateEulerEq( q0, w0, I, t, torque, odeoptions )
% Inputs:
%  q0           initial orientation quaternion (normalized, scalar)
%  w0(3)        initial body frame angular velocity vector
%  I(3)         principal body moments of inertia (if no torque, only
%               ratios of elements of I are used)
%  t(nt)        initial and subsequent (or previous) times t = [t0,t1,...]
%               (monotonic)
%  @torque [OPTIONAL] function handle to calculate torque vector:
%               tau(1:3) = torque( t, y ), where y = [q.e(1:4); w(1:3)]
%  odeoptions [OPTIONAL] ode45 options
% Outputs:
%  q1(1,nt)     array of normalized quaternions at times t1
%  w1(3,nt)     array of body frame angular velocity vectors at times t1
%  t1(1,nt)     array of output times
% Calls:
%  Derivative   quaternion derivative method
%  odeset       matlab ode options setter
%  ode45        matlab ode numerical differential equation integrator
%  torque [OPTIONAL] user-supplied torque as function of time, orientation,
%               and angular rates; default is no torque
% Author:
%  Mark Tincknell, 20 December 2010
%         modified 25 July 2012, enforce normalization of q0 and q1
        options = odeset( varargin{:} );
        q0      = q0.normalize;
        y0      = [q0.e; w0(:)];
        I0      = [ (I(2) - I(3)) / I(1);
                    (I(3) - I(1)) / I(2);
                    (I(1) - I(2)) / I(3) ];
        [T, Y]  = ode45( @Euler, t, y0, options );
            function yd = Euler( ti, yi )
                qi  = quaternion( yi(1), yi(2), yi(3), yi(4) );
                wi  = yi(5:7);
                qd  = double( qi.Derivative( wi ));
                wd  = [ wi(2) * wi(3) * I0(1);
                        wi(3) * wi(1) * I0(2);
                        wi(1) * wi(2) * I0(3) ];
                if exist( 'torque', 'var' ) && isa( torque, 'function_handle' )
                    tau = torque( ti, yi );
                    wd  = tau(:) ./ I + wd;
                end
                yd  = [ qd; wd ];
            end
        if numel(t) == 2
            nT  = 2;
            T   = [T(1);   T(end)];
            Y   = [Y(1,:); Y(end,:)];
        else
            nT  = length(T);
        end
        q1      = repmat( quaternion, [1 nT] );
        w1      = zeros( [3 nT] );
        t1      = T(:).';
        for it = 1 : nT
            q1(it)   = quaternion( Y(it,1), Y(it,2), Y(it,3), Y(it,4) );
            w1(:,it) = Y(it,5:7).';
        end
        q1      = q1.normalize;
        neg     = real( q1 ) < 0;
        q1(neg) = -q1(neg);     % keep real element >= 0
    end % PropagateEulerEq

    function rp = Rodrigues( q )
% function rp = Rodrigues( q )  or  rp = q.Rodrigues
% Construct Rodrigues parameters from rotation quaternions
% Input:
%  q        quaternion array
% Output:
%  rp       Rodrigues parameter array, rp = axis .* tan(angle/2)
        [angle, axis] = q.AngleAxis;
        if size( angle, 1 ) ~= 1
            angle = shiftdim( angle, -1 );
        end
        rp  = bsxfun( @times, tan( 0.5 * angle ), axis );
    end % Rodrigues

    function Tp = RotateTensor( varargin )
% function Tp = RotateTensor( q, T )  or  Tp = q.RotateTensor( T )
% 3x3 rotation matrices R are created from q and matrix multiplication
% rotates 3x3 matrices T into Tp: Tp = R * T * R.'.
% Inputs:
%  q        quaternion array
%  T        3x3xN element Cartesian tensors
% Output:
%  Tp       3x3xN element rotated tensors
        if nargin < 2
            error( 'quaternion:RotateTensor:noargs', ...
                   'RotateTensor requires 2 inputs: a 3x3xN array and a quaternion' );
        end
        if isa( varargin{1}, 'quaternion' )
            q   = varargin{1};
            T   = varargin{2};
        else
            T   = varargin{1};
            q   = varargin{2};
        end
        siT = [size( T ), 1];
        if ~all( siT(1:2) == [3 3] )
            error( 'quaternion:RotateTensor:not3x3', ...
                   'Dimension of T must be 3x3xN' );
        end
        neT = prod( siT(3:end) );
        neq = numel( q );
        R   = q.RotationMatrix;
        if (neq == neT) || (neq == 1)
            Tp  = zeros( siT );
            for iel = 1 : neT
                Rm  = R(:,:,min(neq,iel));
                Tp(:,:,iel) = Rm * T(:,:,iel) * Rm.';
            end
        elseif neT == 1
            Tp  = zeros( [3, 3, neq] );
            for iel = 1 : neq
                Tp(:,:,iel) = R(:,:,iel) * T * R(:,:,iel).';
            end
        else
            error( 'quaternion:RotateTensor:notconformable', ...
                   'q and T must have compatible dimensions' );
        end
    end % RotateTensor

    function vp = RotateVector( varargin )
% function vp = RotateVector( q, v, dim )  or
%          vp = q.RotateVector( v, dim )
% 3x3 rotation matrices are created from q and matrix multiplication
% rotates v into vp. RotateVector is 7 times faster than RotateVectorQ.
% Inputs:
%  q        quaternion array
%  v        3xN or Nx3 element Cartesian vectors
%  dim [OPTIONAL] dimension of v with size 3 to rotate
% Output:
%  vp       3xN or Nx3 element rotated vectors
        if nargin < 2
            error( 'quaternion:RotateVector:noargs', ...
                   'RotateVector requires 2 inputs: a vector and a quaternion' );
        end
        if isa( varargin{1}, 'quaternion' )
            q   = varargin{1};
            v   = varargin{2};
        else
            v   = varargin{1};
            q   = varargin{2};
        end
        if (nargin > 2) && ~isempty( varargin{3} )
            dim = varargin{3};
            if size( v, dim ) ~= 3
                error( 'quaternion:RotateVector:not3D', ...
                       'Dimension dim of vector v must be size 3' );
            end
            if dim > 1
                ndm  = ndims( v );
                perm = [ dim : ndm, 1 : dim-1 ];
                v    = permute( v, perm );
            end
        else
            [v, dim, perm] = finddim( v, 3 );
            if dim == 0
                error( 'quaternion:RotateVector:not3D', ...
                       'v must have a dimension of size 3' );
            end
        end
        sip = size( v );
        v   = reshape( v, 3, [] );
        nev = prod( sip )/ 3;
        R   = q.RotationMatrix;
        siq = size( q );
        neq = prod( siq );
        if neq == nev
            vp  = zeros( sip );
            for iel = 1 : neq
                vp(:,iel)   = R(:,:,iel) * v(:,iel);
            end
            if dim > 1
                vp  = ipermute( vp, perm );
            end
        elseif nev == 1
            siz = [3 siq];
            vp  = zeros( siz );
            for iel = 1 : neq
                vp(:,iel)   = R(:,:,iel) * v;
            end
            if siz(2) == 1
                vp  = squeeze( vp );
            end
        elseif neq == 1
            vp  = R * v;
            vp  = reshape( vp, sip );
            if dim > 1
                vp  = ipermute( vp, perm );
            end
        else
            error( 'quaternion:RotateVector:notconformable', ...
                   'q and v must have compatible dimensions' );
        end
    end % RotateVector

    function vp = RotateVectorQ( varargin )
% function vp = RotateVectorQ( q, v, dim )  or
%          vp = q.RotateVectorQ( v, dim )
% quaternions are created from v and quaternion multiplication rotates v
% into vp. RotateVector is 7 times faster than RotateVectorQ.
% Inputs:
%  q        quaternion array
%  v        3xN or Nx3 element Cartesian vectors
%  dim [OPTIONAL] dimension of v with size 3 to rotate
% Output:
%  vp       3xN or Nx3 element rotated vectors
        if nargin < 2
            error( 'quaternion:RotateVectorQ:noargs', ...
                   'RotateVectorQ requires 2 inputs: a vector and a quaternion' );
        end
        if isa( varargin{1}, 'quaternion' )
            q   = varargin{1};
            v   = varargin{2};
        else
            v   = varargin{1};
            q   = varargin{2};
        end
        siv = size( v );
        if (nargin > 2) && ~isempty( varargin{3} )
            dim = varargin{3};
            if size( v, dim ) ~= 3
                error( 'quaternion:RotateVectorQ:not3D', ...
                       'Dimension dim of vector v must be size 3' );
            end
            if dim > 1
                ndm  = ndims( v );
                perm = [ dim : ndm, 1 : dim-1 ];
                v    = permute( v, perm );
            end
        else
            [v, dim, perm] = finddim( v, 3 );
            if dim == 0
                error( 'quaternion:RotateVectorQ:not3D', ...
                       'v must have a dimension of size 3' );
            end
        end
        sip = size( v );
        qv  = quaternion( v(1,:), v(2,:), v(3,:) );
        qv  = reshape( qv, [1 sip(2:end)] );
        if dim > 1
            qv  = ipermute( qv, perm );
        end
        q   = q.normalize;
        qp  = q .* qv .* q.conj;
        dp  = qp.double;
        nev = prod( siv )/ 3;
        sqz = false;
        if nev == 1
            siz = [3 size(q)];
            if siz(2) == 1
                sqz = true;
            end
        else
            siz = siv;
        end
        vp  = reshape( dp(2:4,:), siz );
        if sqz
            vp  = squeeze( vp );
        end
    end % RotateVectorQ

    function R = RotationMatrix( q )
% function R = RotationMatrix( q )  or  R = q.RotationMatrix
% Construct rotation (or direction cosine) matrices from quaternions
% Input:
%  q        quaternion array
% Output:
%  R        3x3xN rotation (or direction cosine) matrices
        siz = size( q );
        R   = zeros( [3 3 siz] );
        nel = prod( siz );
        q   = normalize( q );
        for iel = 1 : nel
            e11 = q(iel).e(1)^2;
            e12 = q(iel).e(1) * q(iel).e(2);
            e13 = q(iel).e(1) * q(iel).e(3);
            e14 = q(iel).e(1) * q(iel).e(4);
            e22 = q(iel).e(2)^2;
            e23 = q(iel).e(2) * q(iel).e(3);
            e24 = q(iel).e(2) * q(iel).e(4);
            e33 = q(iel).e(3)^2;
            e34 = q(iel).e(3) * q(iel).e(4);
            e44 = q(iel).e(4)^2;
            R(:,:,iel)  = ...
             [ e11 + e22 - e33 - e44, 2*(e23 - e14), 2*(e24 + e13); ...
               2*(e23 + e14), e11 - e22 + e33 - e44, 2*(e34 - e12); ...
               2*(e24 - e13), 2*(e34 + e12), e11 - e22 - e33 + e44 ];
        end
        R   = chop( R );
    end % RotationMatrix
end % methods

% Static methods
methods(Static)
    function q = angleaxis( angle, axis )
% function q = quaternion.angleaxis( angle, axis )
% Construct quaternions from rotation axes and rotation angles
% Inputs:
%  angle    array of rotation angles in radians
%  axis     3xN or Nx3 array of axes (need not be unit vectors)
% Output:
%  q        quaternion array
        sig = size( angle );
        six = size( axis );
        [axis, dim, perm] = finddim( axis, 3 );
        if dim == 0
            error( 'quaternion:angleaxis:not3D', ...
                   'axis must have a dimension of size 3' );
        end
        neg = prod( sig );
        nex = prod( six )/ 3;
        if neg == 1
            siz     = six;
            siz(dim)= 1;
            nel     = nex;
        elseif nex == 1
            siz     = sig;
            nel     = neg;
        elseif nex == neg
            siz     = sig;
            nel     = neg;
        else
            error( 'quaternion:angleaxis:notconformable', ...
                   'angle and axis must have compatible sizes' );
        end
        for iel = nel : -1 : 1
            d(:,iel) = AngAxis2e( angle(min(iel,neg)), axis(:,min(iel,nex)) );
        end
        q   = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) );
        q   = reshape( q, siz );
        if neg == 1
            q   = ipermute( q, perm );
        end
    end % quaternion.angleaxis

    function q = complexmatrix( Q )
% function q = quaternion.complexmatrix( Q )
% Construct quaternions from special complex 2x2 matrices. Matrix algebra
% with Q (e.g. sum, difference, matrix product, matrix inverse) is
% equivalent to the same operation with quaternions. Complex matrices can
% be created from quaternions with q.ComplexMatrix (upper case).
% Input:
%  Q        2x2xN array of complex matrices, [a, c; b, d]
% Output:
%  q        array of N quaternions
%           q.e(1) = (d + a)/2, q.e(2) = i*(d - a)/2,
%           q.e(3) = (c - b)/2, q.e(4) = i*(c + b)/2
        siQ = [size( Q ), 1, 1];
        q1  = 0.5 *( Q(2,2,:) + Q(1,1,:) );
        q2  = 0.5 *( Q(2,2,:) - Q(1,1,:) )* 1i;
        q3  = 0.5 *( Q(1,2,:) - Q(2,1,:) );
        q4  = 0.5 *( Q(1,2,:) + Q(2,1,:) )* 1i;
        q   = reshape( quaternion( q1, q2, q3, q4 ), siQ(3:end) );
    end % complexmatrix

    function q = eulerangles( varargin )
% function q = quaternion.eulerangles( axes, angles )  or
% function q = quaternion.eulerangles( axes, ang1, ang2, ang3 )
% Construct quaternions from triplets of axes and Euler angles
% Inputs:
%  axes                 string array or cell string array
%                       '123' = 'xyz' = 'XYZ' = 'ijk', etc.
%  angles               3xN or Nx3 array of angles in radians  OR
%  ang1, ang2, ang3     arrays of angles in radians
% Output:
%  q                    quaternion array
        ics = cellfun( @ischar, varargin );
        if any( ics )
            varargin{ics} = cellstr( varargin{ics} );
        else
            ics = cellfun( @iscellstr, varargin );
        end
        siv     = cellfun( @size, varargin, 'UniformOutput', false );
        axes    = varargin{ics};
        six     = siv{ics};
        nex     = prod( six );
        dim     = 1;

        if nargin == 2  % angles is 3xN or Nx3 array
            angles  = varargin{~ics};
            sig     = siv{~ics};
            [angles, dim, perm] = finddim( angles, 3 );
            if dim == 0
                error( 'quaternion:eulerangles:badaxes', ...
                       'Must supply 3 Euler angles' );
            end
            sig(dim)    = 1;
            neg         = prod( sig );
            if nex == 1
                siz     = sig;
            elseif neg == 1
                siz     = six;
            elseif nex == neg
                siz     = sig;
            end
            nel = prod( siz );
            for iel = nel : -1 : 1
                q(iel)  = EulerAng2q( axes{min(iel,nex)}, ...
                                      angles(:,min(iel,neg)) );
            end

        elseif nargin == 4  % each of 3 angles is separate input argument
            angles  = varargin(~ics);
            na      = cellfun( 'prodofsize', angles );
            [neg, jeg] = max( na );
            if ~all( (na == 1) | (na == neg) )
                error( 'quaternion:eulerangles:notconformable', ...
                       'All angles must be singletons or have the same number of elements' );
            end
            sig = size( angles{jeg} );
            if nex == 1
                siz     = sig;
            elseif neg == 1
                siz     = six;
            elseif nex == neg
                siz     = sig;
            end
            nel = prod( siz );
            for iel = nel : -1 : 1
                q(iel)  = EulerAng2q( axes{min(iel,nex)}, ...
                                      [angles{1}(min(iel,na(1))), ...
                                       angles{2}(min(iel,na(2))), ...
                                       angles{3}(min(iel,na(3)))] );
            end
        else
            error( 'quaternion:eulerangles:badinputs', ...
                   'Must supply either 2 or 4 input arguments' );
        end % if nargin

        q   = reshape( q, siz );
        if (dim > 1) && isequal( siz, sig )
            q   = ipermute( q, perm );
        end
        if ~ismatrix( q ) && (size( q, 1 ) == 1)
            q   = shiftdim( q, 1 );
        end
    end % quaternion.eulerangles

    function q = eye( N )
% function q = eye( N )
        if nargin < 1
            N   = 1;
        end
        if isempty(N) || (N <= 0)
            q   = quaternion.empty;
        else
            q   = quaternion( eye(N), 0, 0, 0 );
        end
    end % quaternion.eye

    function q = integrateomega( varargin )
% Integrate angular velocities over time (using ode45) to obtain the
% orientation quaternions at those times, using quaternion.Integral and
% initial quaternion(1,0,0,0)
%
% Calling syntax 1:
% function q = quaternion.integrateomega( t, w, odeoptions )
% Inputs:
%  t(nt)        initial and subsequent (or previous) times t = [t0,t1,...]
%               (monotonic)
%  w(3,nt)      3D angular velocity vectors, radians/(unit time)
%  odeoptions [OPTIONAL] ode45 options
%
% Calling syntax 2:
% function q = quaternion.integrateomega( t, omega, axis, odeoptions )
% Inputs:
%  t(nt)        initial and subsequent (or previous) times t = [t0,t1,...]
%               (monotonic)
%  omega(nt)    angular velocities, radians/(unit time)
%  axis(3,nt)   3D rotation axis vectors (normalized to unit vectors
%               internally)
%  odeoptions [OPTIONAL] ode45 options
%
% Output (either syntax):
%  q(nt)        array of normalized quaternions at times t
%
% Calls:
%  Integral     quaternion integral method
        q   = Integral( quaternion(1,0,0,0), varargin{:} );
    end % integrateomega

    function q = modifiedrodrigues( mrp )
% function q = quaternion.modifiedrodrigues( mrp )
% Construct rotation quaternions from Modified Rodrigues parameters
% Input:
%  mrp      array of Modified Rodrigues parameters, mrp = axis.*tan(angle/4)
% Output:
%  q        array of normalized (rotation) quaternions
        [mrp, dim, perm] = finddim( mrp, 3 );
        if dim == 0
            error( 'quaternion:modifiedrodrigues:badpar', ...
                   'Input must have a dimension of length 3' );
        end
        [axis, magn] = unitvector( mrp );
        siz   = size( magn );
        angle = 4 * atan( reshape( magn(1,:), [1, siz(2:end)] ));
        q     = quaternion.angleaxis( angle, axis );
        if dim > 1
            q = ipermute( q, perm );
        end
    end % modifiedrodrigues

    function q = nan( varargin )
% function q = quaternion.nan( siz )
        if isempty( varargin )
            siz = [1 1];
        elseif numel( varargin ) > 1
            siz = [varargin{:}];
        elseif isempty( varargin{1} )
            siz = [0 0];
        elseif numel( varargin{1} ) > 1
            siz = varargin{1};
        else
            siz = [varargin{1} varargin{1}];
        end
        if prod( siz ) == 0
            q   = reshape( quaternion.empty, siz );
        else
            q   = quaternion( nan(siz), nan, nan, nan );
        end
    end % quaternion.nan

    function q = NaN( varargin )
% function q = quaternion.NaN( siz )
        q   = quaternion.nan( varargin{:} );
    end % quaternion.NaN

    function q = ones( varargin )
% function q = quaternion.ones( siz )
        if isempty( varargin )
            siz = [1 1];
        elseif numel( varargin ) > 1
            siz = [varargin{:}];
        elseif isempty( varargin{1} )
            siz = [0 0];
        elseif numel( varargin{1} ) > 1
            siz = varargin{1};
        else
            siz = [varargin{1} varargin{1}];
        end
        if prod( siz ) == 0
            q   = reshape( quaternion.empty, siz );
        else
            q   = quaternion( ones(siz), 0, 0, 0 );
        end
    end % quaternion.ones

    function q = rand( varargin )
% function q = quaternion.rand( siz )
% Input:
%  siz      size of output array q
% Output:
%  q        uniform random quaternions, NOT normalized to 1,
%           0 <= q.e(1) <= 1, -1 <= q.e(2:4) <= 1
        if isempty( varargin )
            siz = [1 1];
        elseif numel( varargin ) > 1
            siz = [varargin{:}];
        elseif isempty( varargin{1} )
            siz = [0 0];
        elseif numel( varargin{1} ) > 1
            siz = varargin{1};
        else
            siz = [varargin{1} varargin{1}];
        end
        if prod( siz ) == 0
            q   = quaternion.empty;
            return;
        end
        d   = [ rand( [1, siz] ); 2 * rand( [3, siz] ) - 1 ];
        q   = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) );
        q   = reshape( q, siz );
    end % quaternion.rand

    function q = randRot( varargin )
% function q = quaternion.randRot( siz )
% Random quaternions uniform in rotation space
% Input:
%  siz      size of output array q
% Output:
%  q        random quaternions, normalized to 1, 0 <= q.e(1) <= 1,
%           uniform over the 3D surface of a 4 dimensional hypersphere
        if isempty( varargin )
            siz = [1 1];
        elseif numel( varargin ) > 1
            siz = [varargin{:}];
        elseif isempty( varargin{1} )
            siz = [0 0];
        elseif numel( varargin{1} ) > 1
            siz = varargin{1};
        else
            siz = [varargin{1} varargin{1}];
        end
        if prod( siz ) == 0
            q   = quaternion.empty;
            return;
        end
        d   = randn( [4, prod( siz )] );
        n   = sqrt( sum( d.^2, 1 ));
        dn  = bsxfun( @rdivide, d, n );
        neg = dn(1,:) < 0;
        dn(:,neg) = -dn(:,neg);
        q   = quaternion( dn(1,:), dn(2,:), dn(3,:), dn(4,:) );
        q   = reshape( q, siz );
    end % quaternion.randRot

    function q = rodrigues( rp )
% function q = quaternion.rodrigues( rp )
% Construct rotation quaternions from Rodrigues parameters
% Input:
%  rp       array of Rodrigues parameters, rp = axis .* tan(angle/2)
% Output:
%  q        array of normalized (rotation) quaternions
        [rp, dim, perm] = finddim( rp, 3 );
        if dim == 0
            error( 'quaternion:rodrigues:badpar', ...
                   'Input must have a dimension of length 3' );
        end
        [axis, magn] = unitvector( rp );
        siz   = size( magn );
        angle = 2 * atan( reshape( magn(1,:), [1, siz(2:end)] ));
        q     = quaternion.angleaxis( angle, axis );
        if dim > 1
            q = ipermute( q, perm );
        end
    end % rodrigues

    function q = rotateutov( u, v, dimu, dimv )
% function q = quaternion.rotateutov( u, v, dimu, dimv )
% Construct quaternions to rotate vectors u into directions of vectors v
% Inputs:
%  u        3x1 or 3xN or 1x3 or Nx3 arrays of vectors
%  v        3x1 or 3xN or 1x3 or Nx3 arrays of vectors
%  dimu [OPTIONAL] dimension of u with size 3 to use
%  dimv [OPTIONAL] dimension of v with size 3 to use
% Output:
%  q        quaternion array
        if (nargin < 3) || isempty( dimu )
            [u, dimu, permu] = finddim( u, 3 );
            if dimu == 0
                error( 'quaternion:rotateutov:not3D', ...
                       'u must have a dimension of size 3' );
            end
        elseif dimu > 1
            ndmu  = ndims( u );
            permu = [ dimu : ndmu, 1 : dimu-1 ];
            u     = permute( u, permu );
        else
            permu = 1 : ndims(u);
        end
        siu     = size( u );
        siu(1)  = 1;
        neu     = prod( siu );
        if (nargin < 4) || isempty( dimv )
            [v, dimv, permv] = finddim( v, 3 );
            if dimv == 0
                error( 'quaternion:rotateutov:not3D', ...
                       'v must have a dimension of size 3' );
            end
        elseif dimv > 1
            ndmv  = ndims( v );
            permv = [ dimv : ndmv, 1 : dimv-1 ];
            v     = permute( v, permv );
        else
            permv = 1 : ndims(v);
        end
        siv     = size( v );
        siv(1)  = 1;
        nev     = prod( siv );
        if neu == nev
            siz  = siu;
            nel  = neu;
            perm = permu;
            dim  = dimu;
        elseif (neu > 1) && (nev == 1)
            siz  = siu;
            nel  = neu;
            perm = permu;
            dim  = dimu;
        elseif (neu == 1) && (nev > 1)
            siz  = siv;
            nel  = nev;
            perm = permv;
            dim  = dimv;
        else
            error( 'quaternion:rotateutov:notconformable', ...
                   'Number of 3 element vectors in u and v must be 1 or equal' );
        end
        for iel = nel : -1 : 1
            q(iel)  = UV2q( u(:,min(iel,neu)), v(:,min(iel,nev)) );
        end
        if dim > 1
            q   = ipermute( reshape( q, siz ), perm );
        end
    end % quaternion.rotateutov

    function q = rotationmatrix( R )
% function q = quaternion.rotationmatrix( R )
% Construct quaternions from rotation (or direction cosine) matrices
% Input:
%  R        3x3xN rotation (or direction cosine) matrices
% Output:
%  q        quaternion array
        siz = [size(R) 1 1];
        if ~all( siz(1:2) == [3 3] ) || ...
           (abs( det( R(:,:,1) ) - 1 ) > eps(16) )
            error( 'quaternion:rotationmatrix:baddet', ...
                   'Rotation matrices must be 3x3xN with det(R) == 1' );
        end
        nel = prod( siz(3:end) );
        for iel = nel : -1 : 1
            d(:,iel) = RotMat2e( chop( R(:,:,iel) ));
        end
        q   = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) );
        q   = normalize( q );
        q   = reshape( q, siz(3:end) );
    end % quaternion.rotationmatrix

    function q = zeros( varargin )
% function q = quaternion.zeros( siz )
        if isempty( varargin )
            siz = [1 1];
        elseif numel( varargin ) > 1
            siz = [varargin{:}];
        elseif isempty( varargin{1} )
            siz = [0 0];
        elseif numel( varargin{1} ) > 1
            siz = varargin{1};
        else
            siz = [varargin{1} varargin{1}];
        end
        if prod( siz ) == 0
            q   = reshape( quaternion.empty, siz );
        else
            q   = quaternion( zeros(siz), 0, 0, 0 );
        end
    end % quaternion.zeros

end % methods(Static)
end % classdef quaternion

% Scalar rotation conversion functions
function eout = AngAxis2e( angle, axis )
% function eout = AngAxis2e( angle, axis )
% One Angle-Axis -> one quaternion
s   = sin( 0.5 * angle );
v   = axis(:);
vn  = norm( v );
if vn == 0
    if s == 0
        c   = 0;
    else
        c   = 1;
    end
    u   = zeros( 3, 1 );
else
    c   = cos( 0.5 * angle );
    u   = v(:) ./ vn;
end
eout    = [ c; s * u ];
if (eout(1) < 0) && (mod( angle/(2*pi), 2 ) ~= 1)
    eout = -eout; % rotationally equivalent quaternion with real element >= 0
end
end % AngAxis2e

function qout = EulerAng2q( axes, angles )
% function qout = EulerAng2q( axes, angles )
% One triplet Euler Angles -> one quaternion
na   = length( axes );
axis = zeros( 3, na );
for i0 = 1 : na
    switch axes(i0)
        case {'1', 'i', 'x', 'X'}
            axis(:,i0) = [ 1; 0; 0 ];
        case {'2', 'j', 'y', 'Y'}
            axis(:,i0) = [ 0; 1; 0 ];
        case {'3', 'k', 'z', 'Z'}
            axis(:,i0) = [ 0; 0; 1 ];
        otherwise
            error( 'quaternion:EulerAng2q:badaxis', ...
                   'Illegal axis designation' );
    end
end
q0   = quaternion.angleaxis( angles(:).', axis );
qout = q0(1);
for i0 = 2 : numel(q0)
    qout = product( q0(i0), qout );
end
if qout.e(1) < 0
    qout = -qout; % rotationally equivalent quaternion with real element >= 0
end
end % EulerAng2q

function eout = RotMat2e( R )
% function eout = RotMat2e( R )
% One Rotation Matrix -> one quaternion
eout    = zeros( 4, 1 );
if ~all( all( R == 0 ))
    eout(1) = 0.5 * sqrt( max( 0, R(1,1) + R(2,2) + R(3,3) + 1 ));
    if eout(1) == 0
        eout(2) = sqrt( max( 0, -0.5 *( R(2,2) + R(3,3) ))) * ...
                        sgn( -R(2,3) );
        eout(3) = sqrt( max( 0, -0.5 *( R(1,1) + R(3,3) ))) * ...
                        sgn( -R(1,3) );
        eout(4) = sqrt( max( 0, -0.5 *( R(1,1) + R(2,2) ))) * ...
                        sgn( -R(1,2) );
    else
        eout(2) = 0.25 *( R(3,2) - R(2,3) )/ eout(1);
        eout(3) = 0.25 *( R(1,3) - R(3,1) )/ eout(1);
        eout(4) = 0.25 *( R(2,1) - R(1,2) )/ eout(1);
    end
end
end % RotMat2e

function qout = UV2q( u, v )
% function qout = UV2q( u, v )
% One pair vectors U, V -> one quaternion
w       = cross( u, v );    % construct vector w perpendicular to u and v
magw    = norm( w );
dotuv   = dot( u, v );
if magw == 0
% Either norm(u) == 0 or norm(v) == 0 or dotuv/(norm(u)*norm(v)) == 1
    if dotuv >= 0
        qout    = quaternion( 1, 0, 0, 0 );
        return;
    end
% dotuv/(norm(u)*norm(v)) == -1
% If v == [v(1); 0; 0], rotate by pi about the [0; 0; 1] axis
    if (v(2) == 0) && (v(3) == 0)
        qout    = quaternion( 0, 0, 0, 1 );
        return;
    end
% Otherwise constuct "what" such that dot(v,what) == 0, and rotate about it
% by pi
    what    = [ 0; -v(3); v(2) ]./ sqrt( v(2)^2 + v(3)^2 );
    costh   = -1;
else
% Use w as rotation axis, angle between u and v as rotation angle
    what    = w(:) / magw;
    costh   = dotuv /( norm(u) * norm(v) );
end
c       = sqrt( 0.5 *( 1 + costh ));    % real element >= 0
s       = sqrt( 0.5 *( 1 - costh ));
eout    = [ c; s * what ];
qout    = quaternion( eout(1), eout(2), eout(3), eout(4) );
end % UV2q

% Helper functions
function out = chop( in, tol )
% function out = chop( in, tol )
% Replace values that differ from an integer by <= tol by the integer
% Inputs:
%  in       input array
%  tol      tolerance, default = eps
% Output:
%  out      input array with integer replacements, if any
if (nargin < 2) || isempty( tol )
    tol = eps;
end
out = in;
rin = round( in );
lx  = abs( rin - in ) <= tol;
out(lx) = rin(lx);
end % chop

function [aout, dim, perm] = finddim( ain, len )
% function [aout, dim, perm] = finddim( ain, len )
% Find first dimension in ain of length len, permute ain to make it first
% Inputs:
%  ain(s1,s2,...)   data array, size = [s1, s2, ...]
%  len              length sought, e.g. s2 == len
%                   if len < 0, then find first dimension >= |len|
% Outputs:
%  aout(s2,...,s1)  data array, permuted so first dimension is length len
%  dim              dimension number of length len, 0 if ain has none
%  perm             permutation order (for permute and ipermute) of aout,
%                   e.g. [2, ..., 1]
% Notes: if no dimension has length len, aout = ain, dim = 0, perm = 1:ndm
%        ain = ipermute( aout, perm )
siz  = size( ain );
ndm  = length( siz );
if len < 0
    dim  = find( siz >= -len, 1, 'first' );
else
    dim  = find( siz == len, 1, 'first' );
end
if isempty( dim )
    dim  = 0;
end
if dim < 2
    aout = ain;
    perm = 1 : ndm;
else
% Permute so that dim becomes the first dimension
    perm = [ dim : ndm, 1 : dim-1 ];
    aout = permute( ain, perm );
end
end % finddim

function s = sgn( x )
% function s = sgn( x ), if x >= 0, s = 1, else s = -1
s   = ones( size( x ));
s(x < 0) = -1;
end % sgn

function [u, n] = unitvector( v, dim )
% function [u, n] = unitvector( v, dim )
% Inputs:
%  v                matrix of vectors
%  dim [OPTIONAL]   dimension to normalize, dim >= 1
%                   if no dim input, use first dimension of length >= 2
% Outputs:
%  u                matrix of unit vectors (except for vectors of norm 0)
%  n                matrix same size as v and u of norms
ndm = ndims( v );
if (nargin < 2) || isempty( dim )
    [v, dim, perm] = finddim( v, -2 );
    if dim == 0
        n     = sqrt( v.*conj(v) );
        n0    = (n ~= 0) & (n ~= 1);
        u     = v;
        u(n0) = v(n0) ./ n(n0);
        return;
    end
else
    perm = [ dim : ndm, 1 : dim-1 ];
    v    = permute( v, perm );
end
u     = v;
sv    = size( v );
n     = repmat( sqrt( sum( v.*conj(v), 1 )), [sv(1) ones(1,ndm-1)] );
n0    = (n ~= 0) & (n ~= 1);
u(n0) = v(n0) ./ n(n0);
u     = ipermute( u, perm );
if nargout > 1
    n = ipermute( n, perm );
end
end % unitvector