差分

このページの2つのバージョン間の差分を表示します。

--- articles:rotation [2021/11/15 22:07] – [直交行列から回転軸，角度表現を求める] Takashi Suehiro
+++ articles:rotation [2022/03/04 08:56] (現在) – [プログラムと練習問題] Takashi Suehiro
@@ 行 628: / 行 628: @@
  + \mathrm{cos}\,\theta + n_z^2 (1 - \mathrm{cos}\,\theta) \\
 &=& 3 \mathrm{cos}\,\theta + (n_x^2 + n_y^2 + n_z^2)(1 - \mathrm{cos}\,\theta) \\
 &=& 1 + 2 \mathrm{cos}\,\theta
 \end{eqnarray}
 $$
@@ 行 636: / 行 636: @@
 $$
 $$
-\theta = \mathrm{cos}^{-1}((a_{11} + a_{22} + a_{33} - 1)/2), \; 0 \le \theta \le \pi
+\theta = \mathrm{cos}^{-1}(a_{11} + a_{22} + a_{33} - 1)/2), \; 0 \le \theta \le \pi
 $$
 また
@@ 行 710: / 行 710: @@
 ここの説明では$ n_x $から解いたが実際には上３つの式を解いて
 一番絶対値の大きい成分から残りを計算すると精度的にも良い値が得られる．
+ほとんどの場合，上記の解き方で問題はないが，$\theta = \pi$の場合は計算誤差などの関係で微妙な問題が発生する可能性がある．
+そこでその場合について別途解いておくことにする．式(9)に代入して，
+$$
+\left( \begin{array}{cc}
+-1 + 2 n_x^2  &
+n_x n_y  &
+n_z n_x  \\
+n_x n_y  &
+-1 + 2 n_y^2 &
+n_y n_z \\
+n_z n_x &
+n_y n_z &
+-1 + 2 n_z^2
+\end{array} \right)
+$$
+係数を比較すると
+$$
+-1 + 2 n_x^2 = a_{xx}
+$$
+$$
+-1 + 2 n_y^2 = a_{yy}
+$$
+$$
+-1 + 2 n_z^2 = a_{zz}
+$$
+$$
+n_x n_y = a_{yx} = a_{xy}
+$$
+$$
+n_y n_z = a_{zy} = a_{yz}
+$$
+$$
+n_z n_x = a_{xz} = a_{zx}
+$$
+よって
+$$
+n_x = \pm \sqrt{{1 + a_{xx}} \over 2}
+$$
+$ n_x \ne 0 $の場合，$\pi$回転はどちらに回しても同じなので正負はどちらを選んでもよい．
+他の成分はこれと整合を取るため以下のように計算する．
+$$
+n_y = a_{yx}/ 2 n_x
+$$
+$$
+n_z = a_{xz}/ 2 n_x
+$$
+$ n_x = 0 $の場合は他の成分から解けば良い．
+ここの説明では$ n_x $から解いたが実際には$a_{xx}$，$a_{yy}$，$a_{zz}$を比較して，
+一番大きい成分から残りを計算するとゼロ割が問題にならないし精度的にも良い値が得られる．
 -->
@@ 行 725: / 行 779: @@
 よって
 $$
-\mathrm{cos}\,\theta = (a_{xx} + a_{yy} + a_{zz} - 1)/2   \tag{5}
+\mathrm{cos}\,\theta = (a_{xx} + a_{yy} + a_{zz} - 1)/2
 $$
 $$
-\theta = \mathrm{cos}^{-1}((a_{xx} + a_{yy} + a_{zz} - 1)/2), \; 0 \le \theta \le \pi
+\theta = \mathrm{cos}^{-1}(a_{xx} + a_{yy} + a_{zz} - 1)/2), \; 0 \le \theta \le \pi  \tag{10}
 $$
-また
+また，
 $$
 \begin{eqnarray}
@@ 行 741: / 行 795: @@
 よって,$\mathrm{sin}\,\theta \ne 0$（すなわち$\theta \ne 0$ and $\theta \ne \pi）$の場合，
 $$
-n_x = (a_{yz} - a_{zy} ) / (2\,\mathrm{sin}\,\theta)
+n_x = (a_{yz} - a_{zy} ) / (2\,\mathrm{sin}\,\theta)　　\tag{11}
 $$
-同様に
+同様に，
 $$
 n_y = (a_{zx} - a_{xz} ) / (2\,\mathrm{sin}\,\theta)
 $$
 $$
 n_z = (a_{xy} - a_{yx} ) / (2\,\mathrm{sin}\,\theta)
 $$
-$ \theta = 0 $の場合は，無回転なので回転軸は存在しない．
-一方，$\theta = \pi$の場合は，式(9)に代入して，
-$$
-\left( \begin{array}{cc}
--1 + 2 n_x^2  &
-n_x n_y  &
-n_z n_x  \\
-n_x n_y  &
+実際には$\mathrm{sin}\,\theta$で割るより正規化するようが良い．しかし，$\mathrm{sin}\,\theta$が$0$に近いと式(11)は誤差が大きくなる．
--1 + 2 n_y^2 &
-n_y n_z \\
+$\theta=0$に近い場合は，回転角度も小さく回転軸の誤差は問題にならない．
+そもそも$\theta=0$の場合は無回転なので回転軸が存在しない．
+一方，$\theta = \pi$の場合は，この誤差が問題となる．そこで以下のように解いていく．
-n_z n_x &
-n_y n_z &
--1 + 2 n_z^2
-\end{array} \right)
 $$
-係数を比較すると
+\begin{eqnarray}
+a_{xx} - a_{yy} - a_{zz}
+&=& \mathrm{cos}\,\theta + n_x^2 (1 - \mathrm{cos}\,\theta)
+ - \mathrm{cos}\,\theta - n_y^2 (1 - \mathrm{cos}\,\theta)
+ - \mathrm{cos}\,\theta - n_z^2 (1 - \mathrm{cos}\,\theta) \\
+&=& (n_x^2 - n_y^2 - n_z^2)-\mathrm{cos}\,\theta (1 + n_x^2 - n_y^2 - n_z^2)\\
+&=& (n_x^2 + (n_x^2 - 1)) -\mathrm{cos}\,\theta (1 + n_x^2 + (n_x^2 - 1)) \\
+&=& 2n_x^2 - 1 - \mathrm{cos}\,\theta (2n_x^2) \\
+&=& 2n_x^2(1-\mathrm{cos}\,\theta ) -1
+\end{eqnarray}
 $$
--1 + 2 n_x^2 = a_{xx}
+よって
 $$
+n_x^2 = \frac{a_{xx} - a_{yy} - a_{zz} + 1 }{2(1-\mathrm{cos}\,\theta )}
 $$
--1 + 2 n_y^2 = a_{yy}
+この平方根を求める際には式(11)を基に正負を選ぶことになる．すなわち，
 $$
+n_x = \mathrm{sign}(a_{yz} - a_{zy} ) \sqrt{ \frac{a_{xx} - a_{yy} - a_{zz} + 1 }{2(1-\mathrm{cos}\,\theta )}} \tag{12}
 $$
--1 + 2 n_z^2 = a_{zz}
+同様に，
 $$
+n_y = \mathrm{sign}(a_{zx} - a_{xz} ) \sqrt{ \frac{-a_{xx} + a_{yy} - a_{zz} + 1 }{2(1-\mathrm{cos}\,\theta )}}
 $$
-n_x n_y = a_{xy} = a_{yx}
 $$
+n_z = \mathrm{sign}(a_{xy} - a_{yx} ) \sqrt{ \frac{- a_{xx} - a_{yy} * a_{zz} + 1 }{2(1-\mathrm{cos}\,\theta )}}
 $$
-n_y n_z = a_{yz} = a_{zy}
-$$
-$$
-n_z n_x = a_{zx} = a_{xz}
-$$
-よって
-$$
-n_x = \pm \sqrt{{1 + a_{xx}} \over 2}
-$$
-$ n_x \ne 0 $の場合，$\pi$回転はどちらに回しても同じなので正負はどちらを選んでもよい．
-他の成分はこれと整合を取るため以下のように計算する．
-$$
-n_y = a_{xy}/ 2 n_x
-$$
-$$
-n_z = a_{zx}/ 2 n_x
-$$
-$ n_x = 0 $の場合は他の成分から解けば良い．
-ここの説明では$ n_x $から解いたが実際には上３つの式を解いて
+この式(11)と式(12)の切り替えは，$\theta=\pi/2$にするのが良さそうに思われる
-一番絶対値の大きい成分から残りを計算すると精度的にも良い値が得られる．
+(([[articles:quaternion#直交行列から四元数へ|四元数]]の場合，この切り替えが不要なのも良い点である．
+さらに言えば四元数を求めてからロドリゲスを計算するほうが良いのかもしれない．)).
+この計算方法は$\theta$が大きいときにゼロ割が発生しないので安定して計算ができるが， 実際のプログラムでは計算誤差の影響で平方根の中の値が極めて小さいときに負になることがあるという点に注意を払う必要がある．
+===== プログラムと練習問題 =====
+[[articles:geo_basic#MATRIX|回転行列演算のプログラム]]
+[[articles:geo_manual#回転行列|練習問題]]