回転行列から回転角を求める

回転行列\(\boldsymbol{R}\)

\begin{align}
\boldsymbol{R}=
\begin{pmatrix}
\cos \psi \cos \theta& \cos \psi \sin \phi \sin \theta – \cos \phi \sin \psi & \sin \phi \sin \psi + \cos \phi \cos \psi \sin \theta \\
\cos \theta \sin \psi& \cos \phi \cos \psi + \sin \phi \sin \psi \sin \theta & \cos \phi \sin \psi \sin \theta – \cos \psi \sin \phi \\
-\sin \theta & \cos \theta \sin \phi & \cos \phi \cos \theta
\end{pmatrix}
\end{align}

について

\begin{align}
\boldsymbol{R}=
\begin{pmatrix}
R_{11} & R_{12} & R_{13} \\
R_{21} & R_{22} & R_{23} \\
R_{31} & R_{32} & R_{33}
\end{pmatrix}
\end{align}

と定義する。これより回転行列から回転角は

\begin{align}
\phi &= \tan^{-1} \dfrac{R_{32} }{ R_{33}} \\
\theta &= – \sin^{-1} R_{31} =- \tan^{-1} \dfrac{R_{31} }{\sqrt{1- R_{31}^{2} }} \hspace{10mm} (\theta \neq \pm \frac{\pi}{2} )\\
\psi &= \tan^{-1} \dfrac{R_{21} }{ R_{11}}
\end{align}

と求まる。