Code covered by the BSD License

# quaternion.m

### Mark Tincknell (view profile)

18 Oct 2011 (Updated )

quaternion class, vectorized, converts among rotation representations, numerical Euler propagation

quaternion
```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
'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
'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
'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)
'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
'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
'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)
'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)
'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)
'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 )
'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
'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
'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
'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
'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
'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
'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
'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) )
'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
'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```