Снег. Keeps the dream alive.

% Verify Quaternion by printing the Quaternion itself,
% Euler Angles and return the Axis Angle.
%
% A - Angle of rotation in degrees
% V - Vector about which rotation is carried out
% X, Y, Z - Euler Angles in degrees
% rot_order - rotation order, for example [1,2,3] means XYZ;
% [2,3,1] means ZXY.
function [A, V] = verify_quat( X, Y, Z, rot_order )
 
x = euler_to_quat( X, 'x' );
y = euler_to_quat( Y, 'y' );
z = euler_to_quat( Z, 'z' );
 
vals = cell(1,3);
 
vals{ rot_order(1) } = x;
vals{ rot_order(2) } = y;
vals{ rot_order(3) } = z;
 
% combine according to rotation roder
interim = mult_quat( vals{2}, vals{1} );
q = mult_quat( vals{3}, interim );
 
% Show that we transformed properly
q
[X, Y, Z] = to_euler( q, rot_order );
X = X .* (180 / pi);
Y = Y .* (180 / pi);
Z = Z .* (180 / pi);
[X, Y, Z]
 
% to axis angle now
n = sqrt( 1.0 - q(1,1)*q(1,1) );
 
V = [ q(1,2), q(1,3), q(1,4) ];
V = V ./ n;
A = 2.0 * acos( q(1,1) );
A = A * 180.0 / pi;
m = sqrt( sum( V .^ 2 ) );
V = V ./ m;
 
end
 
% Transform 1 Euler Angle to a Quaternion
%
% A - Angle in degrees
% axis - Axis of the Euler Angle
% Q - Resulting Quaternion
function Q = euler_to_quat( A, axis )
Q = [0,0,0,1];
rads = A * pi / 180.0;
sin_a = sin( rads / 2.0 );
 
V = zeros(1, 3);
if axis == 'x'
    V(1,1) = 1;
elseif axis == 'y'
    V(1,2) = 1;
else
    V(1,3) = 1;
end
 
Q(1,1) = cos( rads / 2.0 );
Q(1,2) = V(1,1) * sin_a;
Q(1,3) = V(1,2) * sin_a;
Q(1,4) = V(1,3) * sin_a;
end
 
% Transform Quaternion to Euler Angles.
%
% q - Quaternion to transform
% rot_order - Rotation Order
% [X, Y, Z] - Resulting Euler Angles for each axis
function [X, Y, Z] = to_euler( q, rot_order )
 
result = zeros(1, 3);
 
p = get_p( q, rot_order );
 
result(2) = asin( 2 * p(1) * p(3) + 2 * p(5) * p(2) * p(4) );
 
if result(2) == pi/2 || result(2) == -pi/2
    result(1) = atan2( p(2), p(1) );
    result(3) = 0;
else
    result(1) = atan2( 2 * ( p(1) * p(2) - p(5) * p(3) * p(4) ), 1 - 2 * ( p(2) * p(2) + p(3) * p(3) ) );
    result(3) = atan2( 2 * ( p(1) * p(4) - p(5) * p(2) * p(3) ), 1 - 2 * ( p(3) * p(3) + p(4) * p(4) ) );
end
 
X = result( rot_order(1) );
Y = result( rot_order(2) );
Z = result( rot_order(3) );
 
end
 
% Rearrange coefficients of a Quaternion
% according to rot_order.
% So if rot order is ZYX then coeffs will be
% [qw, qz, qy, qx].
% Last item in array is e, which is coefficient
% used in transformation:
% e = dot( cross(e3, e2), e1 ) where e1,e2,e3 are axis of rotation
%
% q - Quaternion to Rearrange
% rot_order - Rotation order
% p - Rearranged array with Qaternion coefficients and value e.
function p = get_p( q, rot_order )
 
p = zeros(1,4);
 
p(1) = q(1);
p( rot_order(1) + 1 ) = q(2);
p( rot_order(2) + 1 ) = q(3);
p( rot_order(3) + 1 ) = q(4);
 
axis = cell(1,3);
 
axis{ rot_order(1) } = [1,0,0];
axis{ rot_order(2) } = [0,1,0];
axis{ rot_order(3) } = [0,0,1];
 
interim = cross( axis{3}, axis{2} );
f = dot( interim, axis{1} );
 
p(5) = f ;
 
end
 
% Multiply Quaternions together.
%
% Q0 - First Quaternion
% Q1 - Second Quaternion
% Q - Result of Q0 * Q1
function Q = mult_quat( Q0, Q1 )
 
Q = [
    Q0(1) * Q1(1) - Q0(2) * Q1(2) - Q0(3) * Q1(3) - Q0(4) * Q1(4);
    Q0(3) * Q1(4) - Q0(4) * Q1(3) + Q0(1) * Q1(2) + Q0(2) * Q1(1);
    Q0(4) * Q1(2) - Q0(2) * Q1(4) + Q0(1) * Q1(3) + Q0(3) * Q1(1);
    Q0(2) * Q1(3) - Q0(3) * Q1(2) + Q0(1) * Q1(4) + Q0(4) * Q1(1)
    ];
 
Q = transpose( Q );
 
end