3次元回転について、その回転行列が分かっている場合、回転軸および回転角度を求める方法の覚書です。
任意の回転軸を\(N=\begin{bmatrix}n_x&n_y&n_z\end{bmatrix}^T\)（単位ベクトル）として、その軸まわりに角度\(\theta\)だけ回転させる回転行列は、ロドリゲスの公式より以下となってます。
\begin{align} R_\theta &= \begin{bmatrix} R_{11} & R_{12} & R_{13}\\ R_{21} & R_{22} & R_{23}\\ R_{31} & R_{32} & R_{33} \end{bmatrix} \\\\ &=\begin{bmatrix} {n_x}^2(1-\cos\theta)+\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 & {n_y}^2(1-\cos\theta)+\cos\theta & n_yn_z(1-\cos\theta)-n_x\sin\theta \\ n_zn_x(1-\cos\theta)-n_y\sin\theta & n_zn_y(1-\cos\theta)+n_x\sin\theta & {n_z}^2(1-\cos\theta)+\cos\theta \end{bmatrix} \\\\ \end{align}ここから回転角度を求めるには
\begin{align} R_{11}+R_{22}+R_{33} &= {n_x}^2(1-\cos\theta)+\cos\theta + {n_y}^2(1-\cos\theta)+\cos\theta + {n_z}^2(1-\cos\theta)+\cos\theta\\\\ &= {n_x}^2 + {n_y}^2 + {n_z}^2 + 3\cos\theta-({n_x}^2 + {n_y}^2 + {n_z}^2)\cos\theta\\\\ &= 1+2\cos\theta\\\\ \cos\theta&=\frac{R_{11}+R_{22}+R_{33}-1}{2}\\\\ \theta&=\arccos\left(\frac{R_{11}+R_{22}+R_{33}-1}{2}\right)\\\\ \end{align}このように展開して求められます。
回転軸についても、同様にロドリゲスの公式から
\begin{align} R_{32}-R_{23} = 2n_x\sin\theta \qquad n_x=\frac{R_{32}-R_{23}}{2\sin\theta}\\\\ R_{13}-R_{31} = 2n_y\sin\theta \qquad n_y=\frac{R_{13}-R_{31}}{2\sin\theta}\\\\ R_{21}-R_{12} = 2n_z\sin\theta \qquad n_z=\frac{R_{21}-R_{12}}{2\sin\theta} \end{align}この式で求められます。
また、\(\sin\theta\)について式を弄っていくと
\begin{align} \left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2&= 4{n_x}^2\sin^2\theta+4{n_y}^2\sin^2\theta+4{n_z}^2\sin^2\theta\\\\ &=({n_x}^2+{n_y}^2+{n_z}^2)4\sin^2\theta\\\\ &=4\sin^2\theta\\\\ \sin\theta&=\frac{\sqrt{\left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2}}{2}\\\\ \end{align}となって、この式からでも角度を求めることが可能。さらに先ほどのもう一つの角度を求める式から
\begin{align} \tan\theta=\frac{\sin\theta}{\cos\theta}&=\frac{\sqrt{\left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2}}{R_{11}+R_{22}+R_{33}-1}\\\\ \end{align}▲このようにもなります。プログラムなんかに使う場合だと、atan2が使えるからこっちの方が少しだけ便利かも。
また回転軸を求める式は\(\sin\theta\)を消すようにすると以下でも求められます。
\begin{align} n_x=\frac{R_{32}-R_{23}}{\sqrt{\left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2}}\\\\ n_y=\frac{R_{13}-R_{31}}{\sqrt{\left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2}}\\\\ n_z=\frac{R_{21}-R_{12}}{\sqrt{\left(R_{32}-R_{23}\right)^2 +\left(R_{13}-R_{31}\right)^2 +\left(R_{21}-R_{12}\right)^2}}\\\\ \end{align}少し複雑になりますが、三角関数が無しで求められます。
▼参考サイト▼
コメント