%% Rotations, Orientation, and Quaternions
% This example reviews concepts in three-dimensional rotations and how
% quaternions are used to describe orientation and rotations.  Quaternions
% are a skew field of hypercomplex numbers. They have found applications in
% aerospace, computer graphics, and virtual reality. In MATLAB(R),
% quaternion mathematics can be represented by manipulating the
% |quaternion| class.

%   Copyright 2017-2018 The MathWorks, Inc.

%%
% The |HelperDrawRotation| class is used to illustrate several
% portions of this example.

dr = HelperDrawRotation;

%% Rotations in Three Dimensions
% All rotations in 3-D can be defined by an axis of rotation and an angle
% of rotation about that axis. Consider the 3-D image of a teapot in the
% leftmost plot. The teapot is rotated by 45 degrees around the Z-axis in
% the second plot. A more complex rotation of 15 degrees around the axis 
% [1 0 1] is shown in the third plot. Quaternions encapsulate the axis and
% angle of rotation and have an algebra for manipulating these rotations.
% The |quaternion| class, and this example, use the "right-hand rule"
% convention to define rotations. That is, positive rotations are clockwise
% around the axis of rotation when viewed from the origin.

dr.drawTeapotRotations;

%% Point Rotation
% The vertices of the teapot were rotated about the axis of rotation in the
% reference frame. Consider a point (0.7, 0.5) rotated 30 degrees
% about the Z-axis. 

figure;
dr.draw2DPointRotation(gca);

%% Frame Rotation
% Frame rotation is, in some sense, the opposite of point rotation. In
% frame rotation, the points of the object stay fixed, but the frame of
% reference is rotated. Again, consider the point (0.7, 0.5). Now the
% reference frame is rotated by 30 degrees around the Z-axis. Note that
% while the point (0.7, 0.5) stays fixed, it has different coordinates in
% the new, rotated frame of reference.

figure;
dr.draw2DFrameRotation(gca);

%% Orientation
% Orientation refers to the angular displacement of an object relative to a
% frame of reference. Typically, orientation is described by the rotation
% that causes this angular displacement from a starting orientation. In
% this example, orientation is defined as the rotation that takes a
% quantity in a parent reference frame to a child reference frame.
% Orientation is usually given as a quaternion, rotation matrix, set of
% Euler angles, or rotation vector. It is useful to think about orientation
% as a frame rotation: the child reference frame is rotated relative to the
% parent frame.
%
% Consider an example where the child reference frame is rotated 30 degrees
% around the vector [1/3 2/3 2/3]. 

figure;
dr.draw3DOrientation(gca, [1/3 2/3 2/3], 30); 

%% Quaternions
% Quaternions are numbers of the form 
%
% $$a + b\textbf{i} + c\textbf{j} + d\textbf{k}$$
%
% where
%
% $$i^2=j^2=k^2=ijk=-1$$
%
% and $a, b, c,$ and $d$ are real numbers. In the rest of this example, the
% four numbers $a, b, c,$ and $d$ are referred to as the _parts_ of the
% quaternion. 


%% Quaternions for Rotations and Orientation
% The axis and the angle of rotation are encapsulated in the quaternion
% parts.  For a unit vector axis of rotation [ _x_, _y_, _z_], and rotation
% angle $\alpha$, the quaternion describing this rotation is 
%%
%
% $$\cos\left(\frac{\alpha}{2}\right) + 
% \sin\left(\frac{\alpha}{2}\right)\left(x\textbf{i} + y\textbf{j} 
% + z\textbf{k}\right)$$
%
% Note that to describe a rotation using a quaternion, the
% quaternion must be a _unit quaternion_. A unit quaternion has a norm of
% 1, where the norm is defined as
%
%%
%
% $$norm(q) = \sqrt{a^2 + b^2 + c^2 + d^2}$$
%
%%
% There are a variety of ways to construct a quaternion in MATLAB, for
% example:

q1 = quaternion(1,2,3,4)

%%
% Arrays of quaternions can be made in the same way:

quaternion([1 10; -1 1], [2 20; -2 2], [3 30; -3 3], [4 40; -4 4])

%%
% Arrays with four columns can also be used to construct quaternions, with
% each column representing a quaternion part:

qmgk = quaternion(magic(4)) 

%%
% Quaternions can be indexed and manipulated just like any other array:
qmgk(3)

%%
%

reshape(qmgk,2,2)

%%
%
[q1; q1]

%% Quaternion Math
% Quaternions have well-defined arithmetic operations. Addition and
% subtraction are similar to complex numbers: parts are added/subtracted
% independently. Multiplication is more complicated because of the earlier
% equation:  
%
% $$i^2=j^2=k^2=ijk=-1$$
%
% This means that multiplication of quaternions is not commutative. That
% is, $pq \neq qp$ for quaternions $p$ and $q$. However, every quaternion
% has a multiplicative inverse, so quaternions can be divided. Arrays of
% the |quaternion| class can be added, subtracted, multiplied, and divided
% in MATLAB.

q = quaternion(1,2,3,4);
p = quaternion(-5,6,-7,8);

%%
% Addition
p + q

%%
% Subtraction
p - q  

%%
% Multiplication
p*q

%% 
% Multiplication in the reverse order (note the different result)
q*p

%% 
% Right division of |p| by |q| is equivalent to $p(q^{-1})$.
p./q

%%
% Left division of |q| by |p| is equivalent to  $p^{-1}q$.
p.\q


%%
% The conjugate of a quaternion is formed by negating each of the non-real
% parts, similar to conjugation for a complex number:

conj(p)

%%
% Quaternions can be normalized in MATLAB:

pnormed = normalize(p)

%%
%

norm(pnormed)

%% Point and Frame Rotations with Quaternions
% Quaternions can be used to rotate points in a static frame of reference,
% or to rotate the frame of reference itself.  The |rotatepoint| function
% rotates a point $v = (v_x, v_y, v_z)$ using a quaternion q through the
% following equation:
%%
%
% $$p v_{quat} p^*$$
%
%%
%
% where $v_{quat}$ is 
%%
%
% $$v_{quat} = 0 + v_x\textbf{i} + v_y\textbf{j} + v_z\textbf{k}$$
%
%%
% 
% and $p^*$ indicates quaternion conjugation. Note the above quaternion
% multiplication results in a quaternion with the real part, $a$, equal to
% 0.  The $b$, $c$, and $d$ parts of the result form the rotated point 
% ($b$, $c$, $d$).

%%
% Consider the example of point rotation from above.
% The point (0.7, 0.5) was rotated 30 degrees around the Z-axis. In
% three dimensions this point has a 0 Z-coordinate.  Using the axis-angle
% formulation, a quaternion can be constructed using [0 0 1] as the axis of
% rotation. 

ang = deg2rad(30);
q = quaternion(cos(ang/2), 0, 0, sin(ang/2));
pt = [0.7, 0.5, 0];  % Z-coordinate is 0 in the X-Y plane
ptrot = rotatepoint(q, pt)


%%
% Similarly, the |rotateframe| function takes a quaternion $q$ and
% point $v$ to compute
%%
%
% $$p^* v_{quat} p$$
%
% Again the above quaternion multiplication results in a quaternion with 0
% real part. The ($b$, $c$, $d$) parts of the result form the coordinate of
% the point $v$ in the new, rotated reference frame. Using the |quaternion|
% class:

ptframerot = rotateframe(q, pt)

%%
% A quaternion and its conjugate have opposite effects because of the
% symmetry in the point and frame rotation equations. Rotating by the
% conjugate "undoes" the rotation.

rotateframe(conj(q), ptframerot)

%%
% Because of the symmetry of the equations, this code performs the same
% rotation. 

rotatepoint(q, ptframerot)


%% Other Rotation Representations
% Often rotations and orientations are described using alternate means:
% Euler angles, rotation matrices, and/or rotation vectors. All of these
% interoperate with quaternions in MATLAB. 
%
% Euler angles are frequently used because they are easy to interpret.
% Consider a frame of reference rotated by 30 degrees around the Z-axis,
% then 20 degrees around the Y-axis, and then -50 degrees around the
% X-axis.  Note here, and throughout, the rotations around each axis are
% _intrinsic_: each subsequent rotation is around the newly created set of
% axes. In other words, the second rotation is around the "new" Y-axis
% created by the first rotation, not around the original Y-axis.

figure;
euld = [30 20 -50];
dr.drawEulerRotation(gca, euld);

%%
% To build a quaternion from these Euler angles for the purpose of frame
% rotation, use the |quaternion| constructor. Since the order of rotations
% is around the Z-axis first, then around the new Y-axis, and finally
% around the new X-axis, use the |'ZYX'| flag.

qeul = quaternion(deg2rad(euld), 'euler', 'ZYX', 'frame')

%%
% The |'euler'| flag indicates that the first argument is in radians.
% If the argument is in degrees, use the |'eulerd'| flag.

qeuld = quaternion(euld, 'eulerd', 'ZYX', 'frame')

%%
% To convert back to Euler angles:

rad2deg(euler(qeul, 'ZYX', 'frame'))

%%
% Equivalently, the |eulerd| method can be used.

eulerd(qeul, 'ZYX', 'frame')


%%
% Alternatively, this same rotation can be represented as a rotation
% matrix:

rmat = rotmat(qeul, 'frame')

%%
% The conversion back to quaternions is similar:

quaternion(rmat, 'rotmat', 'frame')

%%
% Just as a quaternion can be used for either point or frame rotation, it
% can be converted to a rotation matrix (or set of Euler angles)
% specifically for point or frame rotation. The rotation matrix for point
% rotation is the transpose of the matrix for frame rotation. To convert
% between rotation representations, it is necessary to specify |'point'| or
% |'frame'|.

%%
% The rotation matrix for the point rotation section of this example is:
rotmatPoint = rotmat(q, 'point')

%%
% To find the location of the rotated point, right-multiply |rotmatPoint|
% by the transposed array |pt|. 
rotmatPoint * (pt')

%%
% The rotation matrix for the frame rotation section of this example is:
rotmatFrame = rotmat(q, 'frame')

%%
% To find the location of the point in the rotated reference frame,
% right-multiply |rotmatFrame| by the transposed array |pt|. 
rotmatFrame * (pt')

%%
% A rotation vector is an alternate, compact rotation encapsulation. A
% rotation vector is simply a three-element vector that represents the
% unit length axis of rotation scaled-up by the angle of rotation in
% radians. There is no frame-ness or point-ness associated with a rotation
% vector. To convert to a rotation vector:

rv = rotvec(qeul)

%%
% To convert to a quaternion: 
quaternion(rv, 'rotvec')

%% Distance
% One advantage of quaternions over Euler angles is the lack of
% discontinuities. Euler angles have discontinuities that vary depending on
% the convention being used.  The |dist| function compares the effect of
% rotation by two different quaternions. The result is a number in the
% range of 0 to |pi|. Consider two quaternions constructed from Euler
% angles:
eul1 = [0, 10, 0];
eul2 = [0, 15, 0];
qdist1 = quaternion(deg2rad(eul1), 'euler', 'ZYX', 'frame');
qdist2 = quaternion(deg2rad(eul2), 'euler', 'ZYX', 'frame');

%%
% Subtracting the Euler angles, you can see there is no rotation around the
% Z-axis or X-axis. 

eul2 - eul1

%%
% The difference between these two rotations is five degrees around the
% Y-axis. The |dist| shows the difference as well.

rad2deg(dist(qdist1, qdist2))

%%
% For Euler angles such as |eul1| and |eul2|, computing angular distance is
% trivial. A more complex example, which spans an Euler angle
% discontinuity, is:

eul3 = [0, 89, 0];
eul4 = [180, 89, 180];
qdist3 = quaternion(deg2rad(eul3), 'euler', 'ZYX', 'frame');
qdist4 = quaternion(deg2rad(eul4), 'euler', 'ZYX', 'frame');

%%
% Though |eul3| and |eul4| represent nearly the same orientation, simple
% Euler angle subtraction gives the impression that these two orientations
% are very far apart.

euldiff = eul4 - eul3

%%
% Using the |dist| function on the quaternions shows that there is only a 
% two-degree difference in these rotations:

euldist = rad2deg(dist(qdist3, qdist4))

%%
% A quaternion and its negative represent the same rotation. This is not
% obvious from subtracting quaternions, but the |dist| function makes it
% clear.   

qpos = quaternion(-cos(pi/4), 0 ,0, sin(pi/4))
%%
%
qneg = -qpos
%%
%
qdiff = qpos - qneg
%%
%
dist(qpos, qneg)

%% Supported Functions 
% The |quaternion| class lets you effectively describe rotations and
% orientations in MATLAB. The full list of quaternion-supported functions
% can be found with the |methods| function:

methods('quaternion')