ZYXの順の回転行列からオイラー角を抽出するまとめ

ZYXの順の回転行列からオイラー角を抽出するまとめ。自分用メモ。

$R = \left( \begin{array} m_{00}&m_{01}&m_{02} \\ m_{10}&m_{11}&m_{12} \\ m_{20}&m_{21}&m_{22} \end{array} \right)$
$R_x = \left( \begin{array} 1&0&0 \\ 0&cos(x)&sin(x) \\ 0&-sin(x)&cos(x) \end{array} \right)$
$R_y = \left( \begin{array} \cos(y)&0&-sin(y) \\ 0&1&0 \\ sin(y)&0&cos(y) \end{array} \right)$
$R_z = \left( \begin{array} \cos(z)&sin(z)&0 \\ -sin(z)&cos(z)&0 \\ 0&0&1 \end{array} \right)$

$R_{zyx} = \left( \begin{array} \cos(z)cos(y) & sin(z)cos(x)+cos(z)sin(y)sin(x) & sin(z)sin(x)-cos(z)sin(y)cos(x) \\ -sin(z)cos(y) & cos(z)cos(x)-sin(z)sin(y)sin(x) & cos(z)sin(x)+sin(z)sin(y)cos(x) \\ sin(y) & -cos(y)sin(x) & cos(y)cos(x) \end{array} \right)$

行列を眺めると、
$m_{20} = sin(y)$ なので、 $y = asin(m_{20})$
$\frac{m_{21}}{m_{22}} = \frac{-sin(x)}{cos(x)} = -tan(x)$ なので、 $x = atan(-m_{21}, m_{22})$
$\frac{m_{10}}{m_{00}} = \frac{-cos(z)}{sin(z)} = -tan(z)$ なので、 $z = atan(-m_{10}, m_{00})$

あとは、例外として、 $m_{20} = sin(y) = \pm1$ のときを考える。( $cos(y) = 0$ になるので上記の割り算で都合が悪い。)
こういうときは適当に $x = 0$ として計算しちゃうのが良いらしい。
すると、 $sin(x) = 0$ なので $m_{01}$ , $m_{11}$ の値が整理されるので、
$\frac{m_{01}}{m_{11}} = \frac{sin(z)cos(x)}{cos(z)cos(x)} = \frac{sin(z)}{cos(z)} = tan(z)$
よって、 $z = atan(m_{01}, m_{11})$

参考url:
http://d.hatena.ne.jp/It_lives_vainly/20070829/1188384519
http://qiita.com/q_tarou/items/46e5045068742dfb2fa6