Documentation to the math module of the libeng library.
A quaternion can be calculated from a rotation vector and an angle by:
$$\mathbf{q} = \cos \left ( {\frac{\theta}{2}} \right ) + u_{x} \sin \left ( {\frac{\theta}{2}} \right ) \mathbf{i} + u_{y} \sin \left ( {\frac{\theta}{2}} \right ) \mathbf{j} + u_{z} \sin \left ( {\frac{\theta}{2}} \right ) \mathbf{k}$$For a quaternion given by:
$$\mathbf{q} = q_{r} + q_{i} \mathbf{i} + q_{j} \mathbf{j} + q_{k} \mathbf{k}$$The equivalent rotation matrix is calculated from:
$$ \mathbf{R} = \begin{bmatrix} 1 - 2 q_{j}^{2} - 2 q_{k}^{2} & 2 \left ( {q_{i} q_{j} - q_{k} q_{r}} \right ) & 2 \left ( {q_{i} q_{k} + q_{j} q_{r}} \right )\\ 2 \left ( {q_{i} q_{j} + q_{k} q_{r}} \right ) & 1 - 2 q_{i}^{2} - 2 q_{k}^{2} & 2 \left ( {q_{j} q_{k} - q_{i} q_{r}} \right )\\ 2 \left ( {q_{i} q_{k} - q_{j} q_{r}} \right ) & 2 \left ( {q_{j} q_{k} + q_{i} q_{r}} \right ) & 1 - 2 q_{i}^{2} - 2 q_{j}^{2} \end{bmatrix} $$Using the definition of the quaternion based on the rotation vector we have:
$$ \mathbf{R} = \begin{bmatrix} 1 - 2 {\left [ {u_{y} \sin \left ( {\frac{\theta}{2}} \right )} \right ]}^{2} - 2 {\left [ {u_{z} \sin \left ( {\frac{\theta}{2}} \right )} \right ]}^{2} & 2 \left ( {u_{x} \sin \left ( {\frac{\theta}{2}} \right ) u_{y} \sin \left ( {\frac{\theta}{2}} \right ) - u_{z} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) & 2 \left ( {u_{x} \sin \left ( {\frac{\theta}{2}} \right ) u_{z} \sin \left ( {\frac{\theta}{2}} \right ) + u_{y} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) \\ 2 \left ( {u_{x} \sin \left ( {\frac{\theta}{2}} \right ) u_{y} \sin \left ( {\frac{\theta}{2}} \right ) + u_{z} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) & 1 - 2 {\left [ {u_{x} \sin \left ( {\frac{\theta}{2}} \right )} \right ]}^{2} - 2 {\left [ {u_{z} \sin \left ( {\frac{\theta}{2}} \right )} \right ]}^{2} & 2 \left ( {u_{y} \sin \left ( {\frac{\theta}{2}} \right ) u_{z} \sin \left ( {\frac{\theta}{2}} \right ) - u_{x} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) \\ 2 \left ( {u_{x} \sin \left ( {\frac{\theta}{2}} \right ) u_{z} \sin \left ( {\frac{\theta}{2}} \right ) - u_{y} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) & 2 \left ( {u_{y} \sin \left ( {\frac{\theta}{2}} \right ) u_{z} \sin \left ( {\frac{\theta}{2}} \right ) + u_{x} \sin \left ( {\frac{\theta}{2}} \right ) \cos \left ( {\frac{\theta}{2}} \right )} \right ) & 1 - 2 {\left [ {u_{x} \sin \left ( {\frac{\theta}{2}} \right )} \right]}^{2} - 2 {\left [ {u_{y} \sin \left ( {\frac{\theta}{2}} \right )} \right ]}^{2} \end{bmatrix} $$Using trigonometric identities and the fact that the rotation vector in unitary, this matrix can be re-written as:
$$ \mathbf{R} = \begin{bmatrix} \cos \left ( {\theta} \right ) + u_{x}^{2} \left [ {1 - \cos \left ( {\theta} \right )} \right ] & u_{x} u_{y} \left [ {1 - \cos \left( {\theta} \right )} \right ] - u_{z} \sin \left ( {\theta} \right ) & u_{x} u_{z} \left [ {1 - \cos \left( {\theta} \right )} \right ] + u_{y} \sin \left ( {\theta} \right ) \\ u_{x} u_{y} \left [ {1 - \cos \left( {\theta} \right )} \right ] + u_{z} \sin \left ( {\theta} \right ) & \cos \left ( {\theta} \right ) + u_{y}^{2} \left [ {1 - \cos \left ( {\theta} \right )} \right ] & u_{y} u_{z} \left [ {1 - \cos \left( {\theta} \right )} \right ] - u_{x} \sin \left ( {\theta} \right ) \\ u_{x} u_{z} \left [ {1 - \cos \left( {\theta} \right )} \right ] - u_{y} \sin \left ( {\theta} \right ) & u_{y} u_{z} \left [ {1 - \cos \left( {\theta} \right )} \right ] + u_{x} \sin \left ( {\theta} \right ) & \cos \left ( {\theta} \right ) + u_{z}^{2} \left [ {1 - \cos \left ( {\theta} \right )} \right ] \end{bmatrix} $$To obtain the original rotation angle and rotation vector based on the rotation matriz, one can use:
$$ \theta = \arcsin \left [ {\frac{\sqrt{{\left ( {R_{2,1} - R_{1,2}} \right )}^{2} + {\left ( {R_{0,2} - R_{2,0}} \right )}^{2} + {\left ( {R_{1,0} - R_{0,1}} \right )}^{2}}}{2}} \right] $$ $$ \mathbf{r} = \frac{1}{2 \sin \left ( {\theta} \right )} \left [ {\left ( {R_{2,1} - R_{1,2}} \right ) \mathbf{i} + \left ( {R_{0,2} - R_{2,0}} \right ) \mathbf{j} + \left ( {R_{1,0} - R_{0,1}} \right ) \mathbf{k}} \right ] $$