3D CGでは回転を表現するのに回転行列、クォータニオン、オイラー角の3つがよく使われます。この記事では、その3つの表現を相互に変換する方法について解説します。
準備
回転行列$\boldsymbol{R}$の各要素を次のように表します。
\boldsymbol{R} =
\left(
\begin{array}{ccc}
m_{00} & m_{01} & m_{02} \\
m_{10} & m_{11} & m_{12} \\
m_{20} & m_{21} & m_{22}
\end{array}
\right)\\
クォータニオン$\boldsymbol{q}$を次のように表します。
\boldsymbol{q} =
q_w + q_xi + q_yj + q_zk
=
\left[
\begin{array}{c}
q_x \\
q_y \\
q_z \\
q_w
\end{array}
\right] \\
i^2 + j^2 + k ^2 = -1\\
ij = k\\
jk = i\\
ki = j
オイラー角のXYZ各軸に対する回転角度をそれぞれ$\theta_x$、$\theta_y$、$\theta_z$と表現します。オイラー角は各軸に対する回転を適用する順番により最終的な結果が変わります。この記事では回転順を後から適用されるものから順に軸を並べたXYZというような形式で表現します。例えばXYZの場合、Z軸、Y軸、X軸という順に回転を適用していきます。
オイラー角から回転行列
オイラー角から回転行列への変換です。
X軸に対する回転行列$\boldsymbol{R_x}$、Y軸に対する回転行列$\boldsymbol{R_y}$、Z軸に対する回転行列$\boldsymbol{R_z}$はそれぞれ次のようになります。
\begin{eqnarray}
\boldsymbol{R_x} &=&
\left(
\begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos\theta_{x} & -\sin\theta_{x} \\
0 & \sin\theta_{x} & \cos\theta_{x}
\end{array}
\right) \\
\boldsymbol{R_y} &=&
\left(
\begin{array}{ccc}
\cos\theta_{y} & 0 & \sin\theta_{y} \\
0 & 1 & 0 \\
-\sin\theta_{y} & 0 & \cos\theta_{y}
\end{array}
\right) \\
\boldsymbol{R_z} &=&
\left(
\begin{array}{ccc}
\cos\theta_{z} & -\sin\theta_{z} & 0 \\
\sin\theta_{z} & \cos\theta_{z} & 0 \\
0 & 0 & 1
\end{array}
\right)
\end{eqnarray}
この3つの行列$\boldsymbol{R_x}$、$\boldsymbol{R_y}$、$\boldsymbol{R_z}$の積を求めることで、オイラー角を回転行列に変換することができます。先述したようにオイラー角では回転順が最終結果に影響するので、各回転順での変換を求めていきます。
回転順XYZ
\boldsymbol{R_{xyz}} = \boldsymbol{R_x}\boldsymbol{R_y}\boldsymbol{R_z} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\cos\theta_{y}\sin\theta_{z} & \sin\theta_{y} \\
\sin\theta_{x}\sin\theta_{y}\cos\theta_{z} + \cos\theta_{x}\sin\theta_{z} & -\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{y} \\
-\cos\theta_{x}\sin\theta_{y}\cos\theta_{z} + \sin\theta_{x}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} + \sin\theta_{x}\cos\theta_{z} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
回転順XZY
\boldsymbol{R_{xzy}} = \boldsymbol{R_x}\boldsymbol{R_z}\boldsymbol{R_y} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\sin\theta_{z} & \sin\theta_{y}\cos\theta_{z} \\
\cos\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{x}\sin\theta_{y} & \cos\theta_{x}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} - \sin\theta_{x}\cos\theta_{y} \\
\sin\theta_{x}\cos\theta_{y}\sin\theta_{z} - \cos\theta_{x}\sin\theta_{y} & \sin\theta_{x}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
回転順YXZ
\boldsymbol{R_{yxz}} = \boldsymbol{R_y}\boldsymbol{R_x}\boldsymbol{R_z} =
\left(
\begin{array}{ccc}
\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{y}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\cos\theta_{z} -\cos\theta_{y}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y} \\
\cos\theta_{x}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x} \\
\sin\theta_{x}\cos\theta_{y}\sin\theta_{z} -\sin\theta_{y}\cos\theta_{z} & \sin\theta_{x}\cos\theta _{y}\cos\theta_{z} + \sin\theta_{y}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
回転順YZX
\boldsymbol{R_{yzx}} = \boldsymbol{R_y}\boldsymbol{R_z}\boldsymbol{R_x} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\cos\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{x}\sin\theta_{y} & \sin\theta_{x}\cos\theta_{y}\sin\theta_{z} + \cos\theta_{x}\sin\theta_{y} \\
\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{z} \\
-\sin\theta_{y}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} + \sin\theta_{x}\cos\theta_{y} & -\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
回転順ZXY
\boldsymbol{R_{zxy}} = \boldsymbol{R_z}\boldsymbol{R_x}\boldsymbol{R_y} =
\left(
\begin{array}{ccc}
-\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{y}\cos\theta_{z} & -\cos\theta_{x}\sin\theta_{z} & \sin\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{y}\cos\theta_{z} \\
\sin\theta_{x}\sin\theta_{y}\cos\theta_{z} + \cos\theta_{y}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{y}\cos\theta_{z} + \sin\theta_{y}\sin\theta_{z} \\
-\cos\theta_{x}\sin\theta_{y} & \sin\theta_{x} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
回転順ZYX
\boldsymbol{R_{zyx}} = \boldsymbol{R_z}\boldsymbol{R_y}\boldsymbol{R_x} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\cos\theta_{z} - \cos\theta_{x}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y}\cos\theta_{z} + \sin\theta_{x}\sin\theta_{z} \\
\cos\theta_{y}\sin\theta_{z} & \sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} -\sin\theta_{x}\cos\theta_{z} \\
-\sin\theta_{y} & \sin\theta_{x}\cos\theta _{y} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
回転行列からオイラー角
回転行列からオイラー角への変換です。
先ほど求めたオイラー角から回転行列への変換をもとにして、回転行列からオイラー角への変換を求めます。
回転順XYZ
回転順XYZのオイラー角から回転行列への変換は次のようになります。
\boldsymbol{R_{xyz}} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\cos\theta_{y}\sin\theta_{z} & \sin\theta_{y} \\
\sin\theta_{x}\sin\theta_{y}\cos\theta_{z} + \cos\theta_{x}\sin\theta_{z} & -\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{y} \\
-\cos\theta_{x}\sin\theta_{y}\cos\theta_{z} + \sin\theta_{x}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} + \sin\theta_{x}\cos\theta_{z} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
ここから、$\theta_y$が次のように求まります。
m_{02} = \sin\theta_{y} \\
\theta_{y} = \arcsin(m_{02})
また、$\cos\theta_y \neq 0$のとき、$\theta_x$、$\theta_z$が次のように求まります。
\begin{eqnarray}
m_{12} &=& -\sin\theta_x\cos\theta_y \\
\sin\theta_x &=& -\frac{m_{12}}{\cos\theta_y} \\
m_{22} &=& \cos\theta_x\cos\theta_y \\
\cos\theta_x &=& \frac{m_{22}}{\cos\theta_y} \\
\tan\theta_x &=& \frac{\sin\theta_x}{\cos\theta_x} = -\frac{m_{12}}{m_{22}} \\
\theta_x &=& \arctan(-\frac{m_{12}}{m_{22}})
\end{eqnarray}
\begin{eqnarray}
m_{00} &=& \cos\theta_y\cos\theta_z \\
\cos\theta_z &=& \frac{m_{00}}{\cos\theta_y} \\
m_{01} &=& -\cos\theta_y\sin\theta_z \\
\sin\theta_z &=& -\frac{m_{01}}{\cos\theta_y} \\
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = -\frac{m_{01}}{m_{00}} \\
\theta_z &=& \arctan(-\frac{m_{01}}{m_{00}})
\end{eqnarray}
$\cos\theta_y = 0$のとき、ジンバルロックが発生してX軸とZ軸の回転が同軸での回転となります。$\theta_z=0$と仮定すると、回転行列$\boldsymbol{R_{xyz}}$は次のようになります。
\boldsymbol{R_{xyz}} =
\left(
\begin{array}{ccc}
0 & 0 & \sin\theta_{y} \\
\sin\theta_{x}\sin\theta_{y} & \cos\theta_{x} & 0 \\
-\cos\theta_{x}\sin\theta_{y} & \sin\theta_{x} & 0
\end{array}
\right)
ここから$\theta_x$が次のように求まります。
\tan\theta_x = \frac{\sin\theta_x}{\cos\theta_x} = \frac{m_{21}}{m_{11}} \\
\theta_x = \arctan(\frac{m_{21}}{m_{11}})
結果として、回転順XYZのときの回転行列からオイラー角への変換は次のようになります。
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{12}}{m_{22}}) & (cos\theta_y \neq 0) \\
\arctan(\frac{m_{21}}{m_{11}}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(m_{02}) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{01}}{m_{00}}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
以下、各回転順での回転行列からオイラー角への変換を同様の方法で求めていきます。
回転順XZY
\boldsymbol{R_{xzy}} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\sin\theta_{z} & \sin\theta_{y}\cos\theta_{z} \\
\cos\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{x}\sin\theta_{y} & \cos\theta_{x}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} - \sin\theta_{x}\cos\theta_{y} \\
\sin\theta_{x}\cos\theta_{y}\sin\theta_{z} - \cos\theta_{x}\sin\theta_{y} & \sin\theta_{x}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
より、
\begin{eqnarray}
m_{01} &=& -\sin\theta_z \\
\theta_z &=& \arcsin(-m_{01})
\end{eqnarray}
$\cos\theta_z \neq 0$のとき、
\begin{eqnarray}
m_{21} &=& \sin\theta_x\cos\theta_z \\
\sin\theta_x &=& \frac{m_{21}}{\cos\theta_z} \\
m_{11} &=& \cos\theta_x\cos\theta_z \\
\cos\theta_x &=& \frac{m_{11}}{\cos\theta_z} \\
\tan\theta_x &=& \frac{\sin\theta_x}{\cos\theta_x} = \frac{m_{21}}{m_{11}} \\
\theta_x &=& \arctan(\frac{m_{21}}{m_{11}}) \\
m_{02} &=& \sin\theta_y\cos\theta_z \\
\sin\theta_y &=& \frac{m_{02}}{\cos\theta_z} \\
m_{00} &=& \cos\theta_y\cos\theta_z \\
\cos\theta_y &=& \frac{m_{00}}{\cos\theta_z} \\
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = \frac{m_{02}}{m_{00}} \\
\theta_y &=& \arctan(\frac{m_{02}}{m_{00}})
\end{eqnarray}
$\cos\theta_z = 0$のとき、ジンバルロックにより$\theta_y = 0$と仮定すると、
\boldsymbol{R_{xzy}} =
\left(
\begin{array}{ccc}
0 & -\sin\theta_z & 0 \\
\cos\theta_x\sin\theta_z & 0 & -\sin\theta_x \\
\sin\theta_x\sin\theta_z & 0 & \cos\theta_x
\end{array}
\right)
よって、
\begin{eqnarray}
\tan\theta_x &=& \frac{\sin\theta_x}{\cos\theta_x} = -\frac{m_{12}}{m_{22}} \\
\theta_x &=& \arctan(-\frac{m_{12}}{m_{22}})
\end{eqnarray}
結果、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{21}}{m_{11}}) & (cos\theta_z \neq 0) \\
\arctan(-\frac{m_{12}}{m_{22}}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{02}}{m_{00}}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(-m_{01}) \\
\end{eqnarray}
回転順YXZ
\boldsymbol{R_{yxz}} =
\left(
\begin{array}{ccc}
\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{y}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\cos\theta_{z} -\cos\theta_{y}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y} \\
\cos\theta_{x}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x} \\
\sin\theta_{x}\cos\theta_{y}\sin\theta_{z} -\sin\theta_{y}\cos\theta_{z} & \sin\theta_{x}\cos\theta _{y}\cos\theta_{z} + \sin\theta_{y}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
より、
\begin{eqnarray}
m_{12} &=& -\sin\theta_x \\
\theta_x &=& \arcsin(-m_{12})
\end{eqnarray}
$\cos\theta_x \neq 0$のとき、
\begin{eqnarray}
m_{02} &=& \cos\theta_x\sin\theta_y \\
\sin\theta_y &=& \frac{m_{02}}{\cos\theta_x} \\
m_{22} &=& \cos\theta_x\cos\theta_y \\
\cos\theta_y &=& \frac{m_{22}}{\cos\theta_x} \\
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = \frac{m_{02}}{m_{22}} \\
\theta_y &=& \arctan(\frac{m_{02}}{m_{22}}) \\
m_{10} &=& \cos\theta_x\sin\theta_z \\
\sin\theta_z &=& \frac{m_{10}}{\cos\theta_x} \\
m_{11} &=& \cos\theta_x\cos\theta_z \\
\cos\theta_z &=& \frac{m_{11}}{\cos\theta_x} \\
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = \frac{m_{10}}{m_{11}} \\
\theta_z &=& \arctan(\frac{m_{10}}{m_{11}})
\end{eqnarray}
$\cos\theta_x = 0$のとき、ジンバルロックにより$\theta_z = 0$と仮定すると、
\boldsymbol{R_{yxz}} =
\left(
\begin{array}{ccc}
\cos\theta_y & \sin\theta_x\sin\theta_y & 0 \\
0 & 0 & -\sin\theta_x \\
-\sin\theta_y & \sin\theta_x\sin\theta_y & 0
\end{array}
\right)
よって、
\begin{eqnarray}
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = -\frac{m_{20}}{m_{00}} \\
\theta_y &=& \arctan(-\frac{m_{20}}{m_{00}})
\end{eqnarray}
結果、
\begin{eqnarray}
\theta_x &=& \arcsin(-m_{12}) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{02}}{m_{22}}) & (cos\theta_x \neq 0) \\
\arctan(-\frac{m_{20}}{m_{00}}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{10}}{m_{11}}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
回転順YZX
\boldsymbol{R_{yzx}} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & -\cos\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{x}\sin\theta_{y} & \sin\theta_{x}\cos\theta_{y}\sin\theta_{z} + \cos\theta_{x}\sin\theta_{y} \\
\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{z} \\
-\sin\theta_{y}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} + \sin\theta_{x}\cos\theta_{y} & -\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
より、
\begin{eqnarray}
m_{10} &=& \sin\theta_z \\
\theta_z &=& \arcsin(m_{10})
\end{eqnarray}
$\cos\theta_z \neq 0$のとき、
\begin{eqnarray}
m_{12} &=& -\sin\theta_x\cos\theta_z \\
\sin\theta_x &=& -\frac{m_{12}}{\cos\theta_z} \\
m_{11} &=& \cos\theta_x\cos\theta_z \\
\cos\theta_x &=& \frac{m_{11}}{\cos\theta_z} \\
\tan\theta_x &=& \frac{\sin\theta_x}{\cos\theta_x} = -\frac{m_{12}}{m_{11}} \\
\theta_x &=& \arctan(-\frac{m_{12}}{m_{11}}) \\
m_{20} &=& -\sin\theta_y\cos\theta_z \\
\sin\theta_y &=& -\frac{m_{20}}{\cos\theta_z} \\
m_{00} &=& \cos\theta_y\cos\theta_z \\
\cos\theta_y &=& \frac{m_{00}}{\cos\theta_z} \\
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = -\frac{m_{20}}{m_{00}} \\
\theta_y &=& \arctan(-\frac{m_{20}}{m_{00}})
\end{eqnarray}
$\cos\theta_z = 0$のとき、ジンバルロックにより$\theta_x = 0$と仮定すると、
\boldsymbol{R_{yzx}} =
\left(
\begin{array}{ccc}
0 & -\cos\theta_y\sin\theta_z & \sin\theta_y \\
\sin\theta_z & 0 & 0 \\
0 & \sin\theta_y\sin\theta_z & \cos\theta_y
\end{array}
\right)
よって、
\begin{eqnarray}
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = \frac{m_{02}}{m_{22}} \\
\theta_y &=& \arctan(\frac{m_{02}}{m_{22}})
\end{eqnarray}
結果、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{12}}{m_{11}}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{20}}{m_{00}}) & (cos\theta_z \neq 0) \\
\arctan(\frac{m_{02}}{m_{22}}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(m_{10}) \\
\end{eqnarray}
回転順ZXY
\boldsymbol{R_{zxy}} =
\left(
\begin{array}{ccc}
-\sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{y}\cos\theta_{z} & -\cos\theta_{x}\sin\theta_{z} & \sin\theta_{x}\cos\theta_{y}\sin\theta_{z} + \sin\theta_{y}\cos\theta_{z} \\
\sin\theta_{x}\sin\theta_{y}\cos\theta_{z} + \cos\theta_{y}\sin\theta_{z} & \cos\theta_{x}\cos\theta_{z} & -\sin\theta_{x}\cos\theta_{y}\cos\theta_{z} + \sin\theta_{y}\sin\theta_{z} \\
-\cos\theta_{x}\sin\theta_{y} & \sin\theta_{x} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)
より、
\begin{eqnarray}
m_{21} &=& \sin\theta_x \\
\theta_x &=& \arcsin(m_{21})
\end{eqnarray}
$\cos\theta_x \neq 0$のとき、
\begin{eqnarray}
m_{20} &=& -\cos\theta_x\sin\theta_y \\
\sin\theta_y &=& -\frac{m_{20}}{\cos\theta_x} \\
m_{22} &=& \sin\theta_x\cos\theta_y \\
\cos\theta_y &=& \frac{m_{22}}{\sin\theta_x} \\
\tan\theta_y &=& \frac{\sin\theta_y}{\cos\theta_y} = -\frac{m_{20}}{m_{22}} \\
\theta_y &=& \arctan(-\frac{m_{20}}{m_{21}}) \\
m_{01} &=& -\cos\theta_x\sin\theta_z \\
\sin\theta_z &=& -\frac{m_{01}}{\cos\theta_x} \\
m_{11} &=& \cos\theta_x\cos\theta_z \\
\cos\theta_z &=& \frac{m_{11}}{\cos\theta_x} \\
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = -\frac{m_{01}}{m_{11}} \\
\theta_z &=& \arctan(-\frac{m_{01}}{m_{11}})
\end{eqnarray}
$\cos\theta_x = 0$のとき、ジンバルロックにより$\theta_y = 0$と仮定すると、
\boldsymbol{R_{zxy}} =
\left(
\begin{array}{ccc}
\cos\theta_z & 0 & \sin\theta_x\sin\theta_z \\
\sin\theta_z & 0 & -\sin\theta_x\cos\theta_z \\
0 & \sin\theta_x & 0
\end{array}
\right)
よって、
\begin{eqnarray}
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = \frac{m_{10}}{m_{00}} \\
\theta_z &=& \arctan(\frac{m_{10}}{m_{00}})
\end{eqnarray}
結果、
\begin{eqnarray}
\theta_x &=& \arcsin(m_{21}) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{20}}{m_{22}}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{01}}{m_{11}}) & (cos\theta_x \neq 0) \\
\arctan(\frac{m_{10}}{m_{00}}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
回転順ZYX
\boldsymbol{R_{zyx}} =
\left(
\begin{array}{ccc}
\cos\theta_{y}\cos\theta_{z} & \sin\theta_{x}\sin\theta_{y}\cos\theta_{z} - \cos\theta_{x}\sin\theta_{z} & \cos\theta_{x}\sin\theta_{y}\cos\theta_{z} + \sin\theta_{x}\sin\theta_{z} \\
\cos\theta_{y}\sin\theta_{z} & \sin\theta_{x}\sin\theta_{y}\sin\theta_{z} + \cos\theta_{x}\cos\theta_{z} & \cos\theta_{x}\sin\theta_{y}\sin\theta_{z} -\sin\theta_{x}\cos\theta_{z} \\
-\sin\theta_{y} & \sin\theta_{x}\cos\theta _{y} & \cos\theta_{x}\cos\theta_{y}
\end{array}
\right)\\
より、
\begin{eqnarray}
m_{20} &=& -\sin\theta_y \\
\theta_y &=& \arcsin(-m_{20})
\end{eqnarray}
$\cos\theta_y \neq 0$のとき、
\begin{eqnarray}
m_{21} &=& \sin\theta_x\cos\theta_y \\
\sin\theta_x &=& \frac{m_{21}}{\cos\theta_y} \\
m_{22} &=& \cos\theta_x\cos\theta_y \\
\cos\theta_x &=& \frac{m_{22}}{\sin\theta_y} \\
\tan\theta_x &=& \frac{\sin\theta_x}{\cos\theta_x} = \frac{m_{21}}{m_{22}} \\
\theta_x &=& \arctan(\frac{m_{21}}{m_{22}}) \\
m_{10} &=& \cos\theta_y\sin\theta_z \\
\sin\theta_z &=& \frac{m_{10}}{\cos\theta_y} \\
m_{00} &=& \cos\theta_y\cos\theta_z \\
\cos\theta_z &=& \frac{m_{00}}{\cos\theta_y} \\
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = \frac{m_{10}}{m_{00}} \\
\theta_z &=& \arctan(\frac{m_{10}}{m_{00}})
\end{eqnarray}
$\cos\theta_y = 0$のとき、ジンバルロックにより$\theta_x = 0$と仮定すると、
\boldsymbol{R_{zyx}} =
\left(
\begin{array}{ccc}
0 & -\sin\theta_z & \sin\theta_y\cos\theta_z \\
0 & \cos\theta_z & \sin\theta_y\sin\theta_z \\
-\sin\theta_y & 0 & 0
\end{array}
\right)
よって、
\begin{eqnarray}
\tan\theta_z &=& \frac{\sin\theta_z}{\cos\theta_z} = -\frac{m_{01}}{m_{11}} \\
\theta_z &=& \arctan(-\frac{m_{01}}{m_{11}})
\end{eqnarray}
結果、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{21}}{m_{22}}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(-m_{20}) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{10}}{m_{00}}) & (cos\theta_y \neq 0) \\
\arctan(-\frac{m_{01}}{m_{11}}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
オイラー角からクォータニオン
オイラー角からクォータニオンへの変換です。
単位ベクトル$\vec{n}$を軸として$\theta$だけ回転させるクォータニオンは次のようになります。
\vec{n} = (n_x, n_y, v_z), \|\vec{n}\| = 1\\
\boldsymbol{q} =
\left[
\begin{array}{c}
n_x\sin\frac{\theta}{2} \\
n_y\sin\frac{\theta}{2} \\
n_z\sin\frac{\theta}{2} \\
\cos\frac{\theta}{2}
\end{array}
\right]
そのため、X軸、Y軸、Z軸周りの回転を表すクォータニオン$\boldsymbol{q_x}$、$\boldsymbol{q_y}$、$\boldsymbol{q_z}$はそれぞれ次のようになります。
\boldsymbol{q_x} =
\left[
\begin{array}{c}
\sin\frac{\theta_x}{2} \\
0 \\
0 \\
\cos\frac{\theta_x}{2}
\end{array}
\right],
\boldsymbol{q_y} =
\left[
\begin{array}{c}
0 \\
\sin\frac{\theta_y}{2} \\
0 \\
\cos\frac{\theta_y}{2}
\end{array}
\right],
\boldsymbol{q_z} =
\left[
\begin{array}{c}
0 \\
0 \\
\sin\frac{\theta_z}{2} \\
\cos\frac{\theta_z}{2}
\end{array}
\right]
この3つのクォータニオン$\boldsymbol{q_x}$、$\boldsymbol{q_y}$、$\boldsymbol{q_z}$の積を求めることで、オイラー角からクォータニオンへの変換を求めることができます。
回転順XYZ
\boldsymbol{q_x}\boldsymbol{q_y}\boldsymbol{q_z} =
\left[
\begin{array}{c}
\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
\cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
回転順XZY
\boldsymbol{q_x}\boldsymbol{q_z}\boldsymbol{q_y} =
\left[
\begin{array}{c}
-\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} -\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
回転順YXZ
\boldsymbol{q_y}\boldsymbol{q_x}\boldsymbol{q_z} =
\left[
\begin{array}{c}
\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
\cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} - \sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
回転順YZX
\boldsymbol{q_y}\boldsymbol{q_z}\boldsymbol{q_x} =
\left[
\begin{array}{c}
\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
回転順ZXY
\boldsymbol{q_z}\boldsymbol{q_x}\boldsymbol{q_y} =
\left[
\begin{array}{c}
-\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
回転順ZYX
\boldsymbol{q_z}\boldsymbol{q_y}\boldsymbol{q_x} =
\left[
\begin{array}{c}
\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2} - \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} \\
-\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} \\
\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} + \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}
\end{array}
\right]
クォータニオンから回転行列
クォータニオンから回転行列への変換です。
単位ベクトル$\vec{n}$を軸として$\theta$だけ回転させる回転行列は次のようになります。
\vec{n} = (n_x, n_y, v_z), \|\vec{n}\| = 1\\
\boldsymbol{R} =
\left(
\begin{array}{ccc}
\cos\theta + n_x^2(1 - \cos\theta) & n_xn_y(1 - \cos\theta) - n_z\sin\theta & n_xn_z(1 - \cos\theta) + n_y\sin\theta \\
n_xn_y(1 - \cos\theta) + n_z\sin\theta & \cos\theta + n_y^2(1 - \cos\theta) & n_yn_z(1 - \cos\theta) - n_x\sin\theta \\
n_xn_z(1 - \cos\theta) - n_y\sin\theta & n_yn_z(1 - cos\theta) + n_x\sin\theta & \cos\theta + n_z^2(1 - \cos\theta)
\end{array}
\right)
また、オイラー角からクォータニオンへの変換で紹介したように、任意軸に対する回転を表すクォータニオンは次のようになります。
\vec{n} = (n_x, n_y, v_z), \|\vec{n}\| = 1\\
\boldsymbol{q} =
\left[
\begin{array}{c}
q_x \\
q_y \\
q_z \\
q_w
\end{array}
\right]
=
\left[
\begin{array}{c}
n_x\sin\frac{\theta}{2} \\
n_y\sin\frac{\theta}{2} \\
n_z\sin\frac{\theta}{2} \\
\cos\frac{\theta}{2}
\end{array}
\right]
下に示した三角関数の2倍角の公式を用いることで、行列の各要素をクォータニオンの各要素を用いた値に置き換えることができます。
\begin{eqnarray}
\sin\theta &=& 2\sin\frac{\theta}{2}\cos\frac{\theta}{2}\\
\cos\theta &=& 2\cos^2\frac{\theta}{2} - 1 = 1 - 2\sin^2\frac{\theta}{2}
\end{eqnarray}
行列の各要素の置き換えは次のようになります。
\begin{eqnarray}
m_{00} &=& \cos\theta + n_x^2(1 - \cos\theta) \\
&=& (2\cos^2\frac{\theta}{2} - 1) + n_x^2\{1 - (1 - 2\sin^2\frac{\theta}{2})\} \\
&=& 2\cos^2\frac{\theta}{2} + 2n_x^2\sin^2\frac{\theta}{2} - 1 \\
&=& 2q_w^2 + 2q_x^2 - 1 \\
m_{01} &=& n_xn_y(1 - \cos\theta) - n_z\sin\theta \\
&=& n_xn_y\{1 - (1 - 2\sin^2\frac{\theta}{2})\} - n_z(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_xn_y\sin^2\frac{\theta}{2} - 2n_z\sin\frac{\theta}{2}\cos\frac{\theta}{2} \\
&=& 2q_xq_y - 2q_zq_w \\
m_{02} &=& n_xn_z(1 - \cos\theta) + n_y\sin\theta \\
&=& n_xn_z\{1 - (1 - 2\sin^2\frac{\theta}{2})\} + n_y(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_xn_z\sin^2\frac{\theta}{2} + 2n_y\sin\frac{\theta}{2}\cos\frac{\theta}{2} \\
&=& 2q_xq_z + 2q_yq_w \\
m_{10} &=& n_xn_y(1 - \cos\theta) + n_z\sin\theta \\
&=& n_xn_y\{1 - (1 - 2\sin^2\frac{\theta}{2})\} + n_z(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_xn_y\sin^2\frac{\theta}{2} + 2n_z\sin\frac{\theta}{2}\cos\frac{\theta}{2} \\
&=& 2q_xq_y + 2q_zq_w \\
m_{11} &=& \cos\theta + n_y^2(1 - \cos\theta) \\
&=& (2\cos^2\frac{\theta}{2} - 1) + n_y^2\{1 - (1 - 2\sin^2\frac{\theta}{2})\} \\
&=& 2\cos^2\frac{\theta}{2} + 2n_y^2\sin^2\frac{\theta}{2} - 1\\
&=& 2q_w^2 + 2q_y^2 - 1\\
m_{12} &=& n_yn_z(1 - \cos\theta) - n_x\sin\theta \\
&=& n_yn_z\{1 - (1 - 2\sin^2\frac{\theta}{2})\} - n_x(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_yn_z\sin^2\frac{\theta}{2} - 2n_x\sin\frac{\theta}{2}\cos\frac{\theta}{2} \\
&=& 2q_yq_z - 2q_xq_w \\
m_{20} &=& n_xn_z(1 - \cos\theta) - n_y\sin\theta \\
&=& n_xn_z\{1 - (1 - 2\sin^2\frac{\theta}{2})\} - n_y(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_xn_z\sin^2\frac{\theta}{2} - 2n_y\sin\frac{\theta}{2}\cos\frac{\theta}{2}\\
&=& 2q_xq_z - 2q_yq_w \\
m_{21} &=& n_yn_z(1 - cos\theta) + n_x\sin\theta \\
&=& n_yn_z\{1 - (1 - 2\sin^2\frac{\theta}{2})\} + n_x(2\sin\frac{\theta}{2}\cos\frac{\theta}{2}) \\
&=& 2n_yn_z\sin^2\frac{\theta}{2} + 2n_x\sin\frac{\theta}{2}\cos\frac{\theta}{2} \\
&=& 2q_yq_z + 2q_xq_w \\
m_{22} &=& \cos\theta + n_z^2(1 - \cos\theta) \\
&=& (2\cos^2\frac{\theta}{2} - 1) + n_z^2\{1 - (1 - 2\sin^2\frac{\theta}{2})\} \\
&=& 2\cos^2\frac{\theta}{2} + 2n_z^2\sin^2\frac{\theta}{2} - 1\\
&=& 2q_w^2 + 2q_z^2 - 1
\end{eqnarray}
結果、クォータニオンから回転行列への変換は次のようになります。
\boldsymbol{R} =
\left(
\begin{array}{ccc}
2q_w^2 + 2q_x^2 - 1 & 2q_xq_y - 2q_zq_w & 2q_xq_z + 2q_yq_w \\
2q_xq_y + 2q_zq_w & 2q_w^2 + 2q_y^2 - 1 & 2q_yq_z - 2q_xq_w \\
2q_xq_z - 2q_yq_w & 2q_yq_z + 2q_xq_w & 2q_w^2 + 2q_z^2 - 1
\end{array}
\right)
回転行列からクォータニオン
回転行列からクォータニオンへの変換です。
さきほど、クォータニオンから回転行列への変換が次のようになることを確認しました。
\boldsymbol{R} =
\left(
\begin{array}{ccc}
2q_w^2 + 2q_x^2 - 1 & 2q_xq_y - 2q_zq_w & 2q_xq_z + 2q_yq_w \\
2q_xq_y + 2q_zq_w & 2q_w^2 + 2q_y^2 - 1 & 2q_yq_z - 2q_xq_w \\
2q_xq_z - 2q_yq_w & 2q_yq_z + 2q_xq_w & 2q_w^2 + 2q_z^2 - 1
\end{array}
\right)
各要素の和・差をとります。
\begin{eqnarray}
m_{10} + m_{01} &=& (2q_xq_y + 2q_zq_w) + (2q_xq_y - 2q_zq_w) = 4q_xq_y \\
m_{10} - m_{01} &=& (2q_xq_y + 2q_zq_w) - (2q_xq_y - 2q_zq_w) = 4q_zq_w \\
m_{02} + m_{20} &=& (2q_xq_z + 2q_yq_w) + (2q_xq_z - 2q_yq_w) = 4q_xq_z \\
m_{02} - m_{20} &=& (2q_xq_z + 2q_yq_w) - (2q_xq_z - 2q_yq_w) = 4q_yq_w \\
m_{21} + m_{12} &=& (2q_yq_z + 2q_xq_w) + (2q_yq_z - 2q_xq_w) = 4q_yq_z \\
m_{21} - m_{12} &=& (2q_yq_z + 2q_xq_w) - (2q_yq_z - 2q_xq_w) = 4q_xq_w \\
\end{eqnarray}
これを整理すると次のようになり、$q_x$、$q_y$,、$q_z$、$q_w$のうち1つをもとに他の値も求まることがわかります。
q_x = \frac{m_{10} + m_{01}}{4q_y} = \frac{m_{02} + m_{20}}{4q_z} = \frac{m_{21} - m_{12}}{4q_w} \\
q_y = \frac{m_{10} + m_{01}}{4q_x} = \frac{m_{21} + m_{12}}{4q_z} = \frac{m_{02} - m_{20}}{4q_w} \\
q_z = \frac{m_{02} + m_{20}}{4q_x} = \frac{m_{21} + m_{12}}{4q_y} = \frac{m_{10} - m_{01}}{4q_w} \\
q_w = \frac{m_{21} - m_{12}}{4q_x} = \frac{m_{02} - m_{20}}{4q_y} = \frac{m_{10} - m_{01}}{4q_z}
$q_x$、$q_y$,、$q_z$、$q_w$の値は次のように求めることができます。
\begin{eqnarray}
m_{00} - m_{11} - m_{22} &=& (2q_w^2 + 2q_x^2 - 1) - (2q_w^2 + 2q_y^2 - 1) - (2q_w^2 + 2q_z^2 - 1) \\
q_x^2 &=& \frac{m_{00} - m_{11} - m_{22} + 1}{4} \\
q_x &=& \pm\frac{\sqrt{m_{00} - m_{11} - m_{22} + 1}}{2}
\end{eqnarray}
\begin{eqnarray}
-m_{00} + m_{11} - m_{22} &=& -(2q_w^2 + 2q_x^2 - 1) + (2q_w^2 + 2q_y^2 - 1) - (2q_w^2 + 2q_z^2 - 1) \\
q_y^2 &=& \frac{-m_{00} + m_{11} - m_{22} + 1}{4} \\
q_y &=& \pm\frac{\sqrt{-m_{00} + m_{11} - m_{22} + 1}}{2}
\end{eqnarray}
\begin{eqnarray}
-m_{00} - m_{11} + m_{22} &=& -(2q_w^2 + 2q_x^2 - 1) - (2q_w^2 + 2q_y^2 - 1) + (2q_w^2 + 2q_z^2 - 1) \\
q_z^2 &=& \frac{-m_{00} - m_{11} + m_{22} + 1}{4} \\
q_z &=& \pm\frac{\sqrt{-m_{00} - m_{11} + m_{22} + 1}}{2}
\end{eqnarray}
\begin{eqnarray}
m_{00} + m_{11} + m_{22} &=& (2q_w^2 + 2q_x^2 - 1) + (2q_w^2 + 2q_y^2 - 1) + (2q_w^2 + 2q_z^2 - 1) \\
q_w^2 &=& \frac{m_{00} + m_{11} + m_{22} + 1}{2} \\
q_w &=& \pm\frac{\sqrt{m_{00} + m_{11} + m_{22} + 1}}{2}
\end{eqnarray}
$q_x$、$q_y$,、$q_z$、$q_w$の各解をもとに、回転行列からクォータニオンへの変換を整理すると次のようになります。解が負の場合は省略していますが、負の場合は各解を符号反転したものとなり、同じ回転になります。
\boldsymbol{q} =
\left[
\begin{array}{c}
\frac{\sqrt{m_{00} - m_{11} - m_{22} + 1}}{2} \\
\frac{m_{10} + m_{01}}{4q_x} \\
\frac{m_{02} + m_{20}}{4q_x} \\
\frac{m_{21} - m_{12}}{4q_x}
\end{array}
\right]
=
\left[
\begin{array}{c}
\frac{\sqrt{m_{00} - m_{11} - m_{22} + 1}}{2} \\
\frac{m_{10} + m_{01}}{2\sqrt{m_{00} - m_{11} - m_{22} + 1}} \\
\frac{m_{02} + m_{20}}{2\sqrt{m_{00} - m_{11} - m_{22} + 1}} \\
\frac{m_{21} - m_{12}}{2\sqrt{m_{00} - m_{11} - m_{22} + 1}}
\end{array}
\right]
\boldsymbol{q} =
\left[
\begin{array}{c}
\frac{m_{10} + m_{01}}{4q_y} \\
\frac{\sqrt{-m_{00} + m_{11} - m_{22} + 1}}{2} \\
\frac{m_{21} + m_{12}}{4q_y} \\
\frac{m_{02} - m_{20}}{4q_y}
\end{array}
\right]
=
\left[
\begin{array}{c}
\frac{m_{10} + m_{01}}{2\sqrt{-m_{00} + m_{11} - m_{22} + 1}} \\
\frac{\sqrt{-m_{00} + m_{11} - m_{22} + 1}}{2} \\
\frac{m_{21} + m_{12}}{2\sqrt{-m_{00} + m_{11} - m_{22} + 1}} \\
\frac{m_{02} - m_{20}}{2\sqrt{-m_{00} + m_{11} - m_{22} + 1}}
\end{array}
\right]
\boldsymbol{q} =
\left[
\begin{array}{c}
\frac{m_{02} + m_{20}}{4q_z} \\
\frac{m_{21} + m_{12}}{4q_z} \\
\frac{\sqrt{-m_{00} - m_{11} + m_{22} + 1}}{2} \\
\frac{m_{10} - m_{01}}{4q_z}
\end{array}
\right]
=
\left[
\begin{array}{c}
\frac{m_{02} + m_{20}}{2\sqrt{-m_{00} - m_{11} + m_{22} + 1}} \\
\frac{m_{21} + m_{12}}{2\sqrt{-m_{00} - m_{11} + m_{22} + 1}} \\
\frac{\sqrt{-m_{00} - m_{11} + m_{22} + 1}}{2} \\
\frac{m_{10} - m_{01}}{\sqrt{-m_{00} - m_{11} + m_{22} + 1}}
\end{array}
\right]
\boldsymbol{q} =
\left[
\begin{array}{c}
\frac{m_{21} - m_{12}}{4q_w} \\
\frac{m_{02} - m_{20}}{4q_w} \\
\frac{m_{10} - m_{01}}{4q_w} \\
\frac{\sqrt{m_{00} + m_{11} + m_{22} + 1}}{2}
\end{array}
\right]
=
\left[
\begin{array}{c}
\frac{m_{21} - m_{12}}{2\sqrt{m_{00} + m_{11} + m_{22} + 1}} \\
\frac{m_{02} - m_{20}}{2\sqrt{m_{00} + m_{11} + m_{22} + 1}} \\
\frac{m_{10} - m_{01}}{2\sqrt{m_{00} + m_{11} + m_{22} + 1}} \\
\frac{\sqrt{m_{00} + m_{11} + m_{22} + 1}}{2}
\end{array}
\right]
このように回転行列からクォータニオンへの変換では複数の変換候補がありますが、実務的にはゼロ除算が発生しないなど安定して解が求まる候補を選択します。
クォータニオンからオイラー角
クォータニオンからオイラー角への変換です。
ここでは、先述したクォータニオンから回転行列への変換と回転行列からオイラー角への変換を利用します。クォータニオンから回転行列への変換は次のようになります。
\boldsymbol{R} =
\left(
\begin{array}{ccc}
2q_w^2 + 2q_x^2 - 1 & 2q_xq_y - 2q_zq_w & 2q_xq_z + 2q_yq_w \\
2q_xq_y + 2q_zq_w & 2q_w^2 + 2q_y^2 - 1 & 2q_yq_z - 2q_xq_w \\
2q_xq_z - 2q_yq_w & 2q_yq_z + 2q_xq_w & 2q_w^2 + 2q_z^2 - 1
\end{array}
\right)
この各要素を回転行列からオイラー角への変換に適用することで、クォータニオンからオイラー角への変換を求めます。
回転順XYZ
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{12}}{m_{22}}) & (cos\theta_y \neq 0) \\
\arctan(\frac{m_{21}}{m_{11}}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(m_{02}) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{01}}{m_{00}}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_yq_z - 2q_xq_w}{2q_w^2 + 2q_z^2 - 1}) & (cos\theta_y \neq 0) \\
\arctan(\frac{2q_yq_z + 2q_xq_w}{2q_w^2 + 2q_y^2 - 1}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(2q_xq_z + 2q_yq_w) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_xq_y - 2q_zq_w}{2q_w^2 + 2q_x^2 - 1}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
回転順XZY
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{21}}{m_{11}}) & (cos\theta_z \neq 0) \\
\arctan(-\frac{m_{12}}{m_{22}}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{02}}{m_{00}}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(-m_{01}) \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_yq_z + 2q_xq_w}{2q_w^2 + 2q_y^2 - 1}) & (cos\theta_z \neq 0) \\
\arctan(-\frac{2q_yq_z - 2q_xq_w}{2q_w^2 + 2q_z^2 - 1}) & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_xq_z + 2q_yq_w}{2q_w^2 + 2q_x^2 - 1}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(-(2q_xq_y - 2q_zq_w)) \\
\end{eqnarray}
回転順YXZ
\begin{eqnarray}
\theta_x &=& \arcsin(-m_{12}) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{02}}{m_{22}}) & (cos\theta_x \neq 0) \\
\arctan(-\frac{m_{20}}{m_{00}}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{10}}{m_{11}}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \arcsin(-(2q_yq_z - 2q_xq_w)) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_xq_z + 2q_yq_w}{2q_w^2 + 2q_z^2 - 1}) & (cos\theta_x \neq 0) \\
\arctan(-\frac{2q_xq_z - 2q_yq_w}{2q_w^2 + 2q_x^2 - 1}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_xq_y + 2q_zq_w}{2q_w^2 + 2q_y^2 - 1}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\end{eqnarray}
回転順YZX
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{12}}{m_{11}}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{20}}{m_{00}}) & (cos\theta_z \neq 0) \\
\arctan(\frac{m_{02}}{m_{22}}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(m_{10}) \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_yq_z - 2q_xq_w}{2q_w^2 + 2q_y^2 - 1}) & (cos\theta_z \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_xq_z - 2q_yq_w}{2q_w^2 + 2q_x^2 - 1}) & (cos\theta_z \neq 0) \\
\arctan(\frac{2q_xq_z + 2q_yq_w}{2q_w^2 + 2q_z^2 - 1}) & (otherwise)
\end{array} \right. \\
\theta_z &=& \arcsin(2q_xq_y + 2q_zq_w) \\
\end{eqnarray}
回転順ZXY
\begin{eqnarray}
\theta_x &=& \arcsin(m_{21}) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{20}}{m_{22}}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{m_{01}}{m_{11}}) & (cos\theta_x \neq 0) \\
\arctan(\frac{m_{10}}{m_{00}}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \arcsin(2q_yq_z + 2q_xq_w) \\
\theta_y &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_xq_z - 2q_yq_w}{2q_w^2 + 2q_z^2 - 1}) & (cos\theta_x \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(-\frac{2q_xq_y - 2q_zq_w}{2q_w^2 + 2q_y^2 - 1}) & (cos\theta_x \neq 0) \\
\arctan(\frac{2q_xq_y + 2q_zq_w}{2q_w^2 + 2q_x^2 - 1}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
回転順ZYX
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{21}}{m_{22}}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(-m_{20}) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{m_{10}}{m_{00}}) & (cos\theta_y \neq 0) \\
\arctan(-\frac{m_{01}}{m_{11}}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
より、
\begin{eqnarray}
\theta_x &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_yq_z + 2q_xq_w}{2q_w^2 + 2q_z^2 - 1}) & (cos\theta_y \neq 0) \\
0 & (otherwise)
\end{array} \right. \\
\theta_y &=& \arcsin(-(2q_xq_z - 2q_yq_w)) \\
\theta_z &=& \left\{ \begin{array}{ll}
\arctan(\frac{2q_xq_y + 2q_zq_w}{2q_w^2 + 2q_x^2 - 1}) & (cos\theta_y \neq 0) \\
\arctan(-\frac{2q_xq_y - 2q_zq_w}{2q_w^2 + 2q_y^2 - 1}) & (otherwise)
\end{array} \right. \\
\end{eqnarray}
追記
実装編も書きました。
回転行列、クォータニオン(四元数)、オイラー角の相互変換 (実装編) - Qiita