四元数による回転

四元数による回転の要件

${\boldsymbol\psi}_{{\mathbb R}^3}:{\mathbb R}^3\rightarrow{\mathbb R}^3$ が回転を表すためには、長さと角度と掌性を保存しなければなりません。
長さが保存されるのは、 ${\bf p}\in{\mathbb R}^3$ について以下の等式が成り立つときです。

$\begin{eqnarray} ||{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p})||=||{\bf p}||\tag{1} \end{eqnarray}$

原点から2点 ${\bf p}_1\in{\mathbb R}^3$ と ${\bf p}_2\in{\mathbb R}^3$ へ結んだ線分の角が保存されるのは、以下の等式が成り立つときです。

$\begin{eqnarray} {\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_1)^\top{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_2)={\bf p}_1^\top{\bf p}_2\tag{2} \end{eqnarray}$

掌性が保存されるのは、以下の等式が成り立つときです。

$\begin{eqnarray} {\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_1)\times{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_2)={\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_1\times{\bf p}_2)\tag{3} \end{eqnarray}$

ここで、 ${\boldsymbol\psi}_{\mathbb H}:{\mathbb H}\rightarrow{\mathbb H}$ と拡張して、 ${\boldsymbol\psi}_{\mathbb H}\left((s,{\bf v})\right)=\left(s,{\boldsymbol\psi}_{{\mathbb R}^3}({\bf v})\right)$ が成り立つとします。
$s\in{\mathbb R},{\bf v}\in{\mathbb R}^3$ です。

${\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)(0, {\bf p}_2)\right)$ を変形します。

$\begin{eqnarray} {\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)(0, {\bf p}_2)\right)&=&{\boldsymbol\psi}_{\mathbb H}\left((-{\bf p}_1^\top{\bf p}_2,{\bf p}_1\times{\bf p}_2)\right)\\ &=&\left(-{\bf p}_1^\top{\bf p}_2,{\boldsymbol\psi}_{{\mathbb R}^3}\left({\bf p}_1\times{\bf p}_2\right)\right)\tag{4} \end{eqnarray}$

${\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)\right){\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_2)\right)$ を変形します。

$\begin{eqnarray} {\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)\right){\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_2)\right) &=&\left(0,{\boldsymbol\psi}_{{\mathbb R}^3}\left({\bf p}_1\right)\right)\left(0,{\boldsymbol\psi}_{{\mathbb R}^3}\left({\bf p}_2\right)\right)\\ &=&\left(-{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_1)^\top{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_2),{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_1)\times{\boldsymbol\psi}_{{\mathbb R}^3}({\bf p}_2)\right)\tag{5} \end{eqnarray}$

$(2),(3),(4),(5)$ より、角度と掌性の保存の要件は、以下の1つの式で表せます。

$\begin{eqnarray} {\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)\right){\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_2)\right)={\boldsymbol\psi}_{\mathbb H}\left((0, {\bf p}_1)(0, {\bf p}_2)\right)\tag{6} \end{eqnarray}$

この式 $(6)$ を満たす関数を準同型写像と言います。

四元数による回転は、以下の式で表されます。

$\begin{eqnarray} {\boldsymbol\psi}_q( (0, {\bf p} ) )=q(0, {\bf p})q^{-1}\tag{7} \end{eqnarray}$

$q\in{\mathbb H}$ とし、 $0$ でないとします。
式 $(7)$ で表される写像 ${\boldsymbol\psi}_q$ が要件 $(1),(6)$ を満たすことを示します。

まずは、 ${\boldsymbol\psi}_q$ が長さを保存すること、すなわち、 $(1)$ を示します。

$\begin{eqnarray} || {\boldsymbol\psi}_q( (0, {\bf p} ) ) ||&=&||q(0, {\bf p})q^{-1}||\\ &=&||q||\cdot||(0, {\bf p})||\cdot||q^{-1}||\\ &=&||q||\cdot||{\bf p}||\cdot\frac{||q||}{||q||^2}\\ &=&||{\bf p}||\tag{8} \end{eqnarray}$

$(8)$ の変形には、 $(24),(25)$ を用いています。
${\boldsymbol\psi}_q( (0, {\bf p} ) )={\boldsymbol\psi}_{{\mathbb R}^3}({\bf p} ) )$ と $(8)$ より、 $(1)$ が示せました。

次に、 ${\boldsymbol\psi}_q$ が角度、掌性を保存すること、すなわち、 $(6)$ を示します。

$\begin{eqnarray} {\boldsymbol\psi}_q( (0, {\bf p}_1 ) ){\boldsymbol\psi}_q( (0, {\bf p}_2 ) ) &=&q(0,{\bf p}_1)q^{-1}q(0,{\bf p}_2)q^{-1}\\ &=&q(0,{\bf p}_1)(0,{\bf p}_2)q^{-1}\\ &=&{\boldsymbol\psi}_q( (0, {\bf p}_1 )(0, {\bf p}_2 ) )\tag{9} \end{eqnarray}$

$(9)$ より、 $(6)$ が示せました。
以上より、 $(7)$ が回転であることが示せました。

$q$ を $a\in{\mathbb R}$ 倍した $aq$ による写像 ${\boldsymbol\psi}_{aq}( (0, {\bf p} ) )=(aq)(0, {\bf p})(aq)^{-1}$ を変形してみます。

$\begin{eqnarray} {\boldsymbol\psi}_{aq}( (0, {\bf p} ) )&=&(aq)(0, {\bf p})(aq)^{-1}\\ &=&aq(0, {\bf p})q^{-1}a^{-1}\\ &=&q(0, {\bf p})q^{-1}\\ &=&{\boldsymbol\psi}_q( (0, {\bf p} ) )\tag{10} \end{eqnarray}$

$(10)$ より、 ${\boldsymbol\psi}_{aq}( (0, {\bf p} ) )={\boldsymbol\psi}_q( (0, {\bf p} ) )$ なので、単純にするために単位四元数のみを考えることにします。

$q=(s,{\bf v})\in{\mathbb H},s\in{\mathbb R},{\bf v}\in{\mathbb R}^3$ とします。 $||q||=1$ より、 $q^{-1}=\bar{q}=(s,-{\bf v})$ です。
$(7)$ を計算していきます。

$\begin{eqnarray} {\boldsymbol\psi}_q( (0, {\bf p} ) )&=&q(0, {\bf p})q^{-1}\\ &=&(s,{\bf v})(0,{\bf p})(s,-{\bf v})\\ &=&(-{\bf v}^\top{\bf p},s{\bf p}+{\bf v}\times{\bf p})(s,-{\bf v})\\ &=&\left( (-{\bf v}^\top{\bf p})s-(s{\bf p}+{\bf v}\times{\bf p})^\top(-{\bf v}),\ (-{\bf v}^\top{\bf p})(-{\bf v})+s(s{\bf p}+{\bf v}\times{\bf p})+(s{\bf p}+{\bf v}\times{\bf p})\times(-{\bf v})\right)\\ &=&\left( (-s{\bf v}^\top{\bf p})+s{\bf p}^\top{\bf v}+({\bf v}\times{\bf p})^\top{\bf v},\ ({\bf v}^\top{\bf p}){\bf v}+s^2{\bf p}+s({\bf v}\times{\bf p})-s({\bf p}\times{\bf v})-({\bf v}\times{\bf p})\times{\bf v}\right)\\ &=&\left(0,\ s^2{\bf p}+2s({\bf v}\times{\bf p})+({\bf v}^\top{\bf p}){\bf v}-({\bf v}\times{\bf p})\times{\bf v}\right)\\ &=&\left(0,\ s^2{\bf p}+2s({\bf v}\times{\bf p})+({\bf v}^\top{\bf p}){\bf v}+{\bf v}\times({\bf v}\times{\bf p})\right)\\ &=&\left(0,\ s^2{\bf p}+2s({\bf v}\times{\bf p})+({\bf v}^\top{\bf p}){\bf v}+({\bf v}^\top{\bf p}){\bf v}-({\bf v}^\top{\bf v}){\bf p}\right)\\ &=&\left(0,\ (s^2-||{\bf v}||^2){\bf p}+2s({\bf v}\times{\bf p})+2({\bf v}^\top{\bf p}){\bf v}\right)\tag{11} \end{eqnarray}$

$(11)$ の導出で、ベクトル3重積の公式 $(21)$ を使いました。

単位ベクトル ${\bf a}\in{\mathbb R}^3$ を用いて ${\bf v}=t{\bf a}$ とおくと、 $(11)$ は以下のようになります。

$\begin{eqnarray} {\boldsymbol\psi}_q( (0, {\bf p} ) )&=&\left(0,\ (s^2-t^2){\bf p}+2st({\bf a}\times{\bf p})+2t^2({\bf a}^\top{\bf p}){\bf a}\right)\tag{12} \end{eqnarray}$

任意軸まわりの回転 - ロドリゲスの回転公式の記事でで求めた式 $(11)$ は以下でした。

$\begin{eqnarray} {\bf p}'=\cos\theta{\bf p}+\sin\theta\left({\bf a}\times{\bf p}\right)+(1-\cos\theta)\left({\bf a}^\top{\bf p}\right){\bf a}\tag{13}\\ \end{eqnarray}$

$(12),(13)$ を係数比較します。

$\begin{eqnarray} \left\{ \begin{array}{l} s^2-t^2=\cos\theta\\ 2st=\sin\theta\\ 2t^2=1-\cos\theta \end{array} \right.\tag{14} \end{eqnarray}$

$(14)$ の第三式より

$\begin{eqnarray} t=\sqrt{\dfrac{1-\cos\theta}{2}}=\sin\frac{\theta}{2}\tag{15} \end{eqnarray}$

が得られます。
また、 $(14)$ の第一式と第三式より、 $s^2+t^2=1$ であることが分かります。
したがって、 $s=\cos\dfrac{\theta}{2}$ でなければならないことが分かります。

以上より、単位ベクトル $\bf a$ まわりの角度 $\theta$ 回転に対応する単位四元数 $q$ が求まりました。

$\begin{eqnarray} q=\left(\cos\dfrac{\theta}{2},\ \sin\dfrac{\theta}{2}{\bf a}\right)\tag{16} \end{eqnarray}$

任意軸まわりの回転行列

四元数による回転を表す式 $(12)$ の虚部 $(s^2-t^2){\bf p}+2st({\bf a}\times{\bf p})+2t^2({\bf a}^\top{\bf p}){\bf a}$ を行列で表したいと思います。
${\bf a}\times{\bf p}$ と $({\bf a}^\top{\bf p}){\bf a}$ を $(22),(23)$ を用いて、同等の行列で置き換えます。

$\begin{eqnarray} &&(s^2-t^2){\bf p}+2st({\bf a}\times{\bf p})+2t^2({\bf a}^\top{\bf p}){\bf a}\\ &=& \begin{pmatrix} s^2-t^2 & 0 & 0\\ 0 & s^2-t^2 & 0\\ 0 & 0 & s^2-t^2 \end{pmatrix} {\bf p} + \begin{pmatrix} 0 & -2sta_z & 2sta_y\\ 2sta_z & 0 & -2sta_x\\ -2sta_y & 2sta_x & 0 \end{pmatrix} {\bf p} + \begin{pmatrix} 2t^2a_x^2 & 2t^2a_xa_y & 2t^2a_xa_z\\ 2t^2a_xa_y & 2t^2a_y^2 & 2t^2a_ya_z\\ 2t^2a_xa_z & 2t^2a_ya_z & 2t^2a_z^2 \end{pmatrix} {\bf p}\tag{17} \end{eqnarray}$

ここで、四元数 $q$ を $q=(w,(x,y,z))$ として書くと、 $w=s,x=ta_x,y=ta_y,z=ta_z$ となります。
また、 ${\bf a}$ は単位ベクトルであるので、 $x^2+y^2+z^2=t^2||{\bf a}||^2=t^2$ です。
これらを用いて、 $(17)$ を書き直します。

$\begin{eqnarray} &&(s^2-t^2){\bf p}+2st({\bf a}\times{\bf p})+2t^2({\bf a}^\top{\bf p}){\bf a}\\ &=& \begin{pmatrix} w^2-x^2-y^2-z^2 & 0 & 0\\ 0 & w^2-x^2-y^2-z^2 & 0\\ 0 & 0 & w^2-x^2-y^2-z^2\\ \end{pmatrix} {\bf p} +\begin{pmatrix} 0 & -2wz & 2wy\\ 2wz & 0 & -2wz\\ -2wy & 2wx & 0 \end{pmatrix} {\bf p} + \begin{pmatrix} 2x^2 & 2xy & 2xz\\ 2xy & 2y^2 & 2yz\\ 2xz & 2yz & 2z^2 \end{pmatrix} {\bf p}\\ &=& \begin{pmatrix} w^2+x^2-y^2-z^2 & 2(xy-wz) & 2(xz+wy)\\ 2(xy+wz) & w^2-x^2+y^2-z^2 & 2(yz-wx)\\ 2(xy-wy) & 2(yz+wx) & w^2-x^2-y^2+z^2\\ \end{pmatrix} {\bf p}\tag{18} \end{eqnarray}$

また、 $q$ は単位四元数であるから、 $w^2+x^2+y^2+z^2=1$ を満たします。
したがって、

$\begin{eqnarray} w^2-x^2-y^2-z^2=1-2x^2-2y^2-2z^2\tag{19} \end{eqnarray}$

と書けます。

$(18),(19)$ より、四元数 $q$ に対応する回転行列 ${\bf R}_q$ が得られます。

$\begin{eqnarray} {\bf R}_q= \begin{pmatrix} 1-2y^2-2z^2 & 2xy-2wz & 2xz+2wy\\ 2xy+2wz & 1-2x^2-2z^2 & 2yz-2wx\\ 2xz-2wy & 2yz+2wx & 1-2x^2-2y^2 \end{pmatrix}\tag{20} \end{eqnarray}$

なお、 $(20)$ を変形すると、任意軸まわりの回転 - ロドリゲスの回転公式の記事の $(13)$ に一致します。

ベクトル3重積の公式

$\begin{eqnarray} {\bf a}\times({\bf b}\times{\bf c})=({\bf a}^\top{\bf c}){\bf b}-({\bf a}^\top{\bf b}){\bf c}\tag{21} \end{eqnarray}$

p の q 上への射影を表す行列

$\begin{eqnarray} {\rm proj}_{\bf q}{\bf p}=\dfrac{{\bf p}^\top{\bf q}}{||{\bf q}||^2}{\bf q} = \dfrac{1}{||{\bf q}||^2} \begin{pmatrix} q_x^2 & q_xq_y & q_xq_z\\ q_xq_y & q_y^2 & q_yq_z\\ q_xq_z & q_yq_z & q_z^2 \end{pmatrix} \begin{pmatrix} p_x\\p_y\\p_z\tag{22} \end{pmatrix} \end{eqnarray}$

外積を表す行列

$\begin{eqnarray} {\bf p}\times{\bf q}=\begin{pmatrix} 0 & -p_z & p_y\\ p_z & 0 & -p_x\\ -p_y & p_x & 0 \end{pmatrix} \begin{pmatrix} q_x\\q_y\\q_z\tag{23} \end{pmatrix} \end{eqnarray}$

四元数のスカラー倍のノルム

$||a(s,v)||=|a|\cdot ||(s, {\bf v})||\ \cdots(24)$ であることを示します。ただし、 $a,s\in{\mathbb R},{\bf v}\in{\mathbb R}^3$ とします。

$\begin{eqnarray} ||a(s,v)||&=&||(a,{\bf 0})(s, {\bf v})||\\ &=&||(as, a{\bf v})||\\ &=&\left(a^2s^2+a^2||{\bf v}||^2\right)^\frac{1}{2}\\ &=&|a|\left(s^2+||{\bf v}||^2\right)^\frac{1}{2}\\ &=&|a|\cdot ||(s, {\bf v})|| \end{eqnarray}$

四元数の積のノルムとノルムの積

$||(s_1,(x_1,y_1,z_1))(s_2,(x_2,y_2,z_2))||=||(s_1,(x_1,y_1,z_1))||\cdot ||(s_2,(x_2,y_2,z_2))||\ \cdots(25)$ であることをSympyを使って示します。

import sympy
sympy.init_printing()
sympy.var('s_1,x_1,y_1,z_1,s_2,x_2,y_2,z_2')
# || (s_1,x_1, y_1, z_1)(s_2, x_2, y_2, z_2) || ** 2
s = s_1 * s_2 - x_1 * x_2 - y_1 * y_2 - z_1 * z_2
x = s_1 * x_2 + s_2 * x_1 + y_1 * z_2 - y_2 * z_1
y = s_1 * y_2 + s_2 * y_1 - x_1 * z_2 + x_2 * z_1
z = s_1 * z_2 + s_2 * z_1 + x_1 * y_2 - x_2 * y_1
nrm_pow2 = s ** 2 + x ** 2 + y ** 2 + z ** 2

# (|| (s_1,x_1, y_1, z_1)|| || (s_2, x_2, y_2, z_2) ||) ** 2
nrm1_pow2 = s_1 ** 2 + x_1 ** 2 + y_1 ** 2 + z_1 ** 2
nrm2_pow2 = s_2 ** 2 + x_2 ** 2 + y_2 ** 2 + z_2 ** 2
multi_nrm_pow2 = nrm1_pow2 * nrm2_pow2

nrm_pow2 = sympy.expand(nrm_pow2)
multi_nrm_pow2 = sympy.expand(multi_nrm_pow2)

display(nrm_pow2)
display(multi_nrm_pow2)
display(nrm_pow2.equals(multi_nrm_pow2))

参考文献

ゲームプログラミングのための3Dグラフィックス数学 p69-p72