変換行列からオイラー角の求め方

今日はタイトルの通り、「変換行列からオイラー角を求める方法」でハマってしまい、
えらい時間が掛かったので、自分メモのつもりで投稿。

変換行列からオイラー角による回転を計算する場合、回転順序によって6種類の
計算方法があります。
（ちなみに、回転順序はXYZ、XZY、YXZ、YZX、ZXY、ZYXの6種類です。）

ちなみに、ベースとなる情報元は以下のwiki。
http://www7.atwiki.jp/lucifer/pages/13.html

ここの解説の仕方をベースに、誤っていると思われる部分を修正した表にしておきます。


まず、変換行列は以下の形になっているという前提。

		m00	m01	m02
行列R	=	m10	m11	m12
		m20	m21	m22

最初の回転軸をα、2番目の軸をβ、3番目の軸をγとすると、各回転順序での計算方法は
以下のようになります。

α	β	γ	γの角度	βの角度（-90～90）	αの角度
X	Y	Z	atan2(m01,m00)	asin(-m02)	asin(m12/cos(β))  if(m22<0) α=180-α
X	Z	Y	atan2(-m02,m00)	asin(m01)	asin(-m21/cos(β)) if(m11<0) α=180-α
Y	X	Z	atan2(-m10,m11)	asin(m12)	asin(-m02/cos(β)) if(m22<0) α=180-α
Y	Z	X	atan2(m12,m11)	asin(-m10)	asin(m20/cos(β)) if(m00<0) α=180-α
Z	X	Y	atan2(m20,m22)	asin(-m21)	asin(m01/cos(β)) if(m11<0) α=180-α
Z	Y	X	atan2(-m21,m22)	asin(m20)	asin(-m10/cos(β)) if(m00<0) α=180-α

ただし、cos(β)がゼロになる場合は、α=0、β=90又は-90としてγを計算しなおします。

α	β	γ	γの角度	βの角度（-90又は90）	αの角度
X	Y	Z	atan2(-m10,m11)	asin(-m02)	0
X	Z	Y	atan2(m20,m22)	asin(m01)	0
Y	X	Z	atan2(m01,m00)	asin(m12)	0
Y	Z	X	atan2(-m21,m22)	asin(-m10)	0
Z	X	Y	atan2(-m02,m00)	asin(-m21)	0
Z	Y	X	atan2(m12,m11)	asin(m20)	0

上の表の計算方法を使えば、各回転順序でオイラー角を取得出来るハズ。

ただ、浮動小数で計算していると、三角関数や除算なんかで不正値がでたり
するのでそっちも注意が必要。
例えば、if (cos(β)==0) みたいなコードにしちゃうと、cos(β)の結果が非常に小さい
値だけどゼロで無い時に、cos(β)での除算結果が不正値になったり。

ふう。自分メモ終了。
自分以外にも誰かの役に立ったりしたら幸いです。