四元數和旋轉(Quaternion & rotation)
本篇文章主要講述3D空間中的旋轉和四元數之間的關系。其中會涉及到矩陣、向量運算，旋轉矩陣，四元數，旋轉的四元數表示，四元數表示的旋轉如何轉化為旋轉矩陣。層層鋪墊，可能文章有點長。基礎好的同學，可以直接跳到四元數表示旋轉部分，見下文公式(18)和公式(21)。
1 向量的點積和叉積
1.1 點積
給定兩個n維向量\(\mathbf{P}, \mathbf{Q}\)，則它們的點積(dot product，又稱為內積)為：
\[\mathbf{P}\cdot \mathbf{Q} = \left\|\mathbf{P}\right\| \left\|\mathbf{Q}\right\|\cos\alpha \qquad(1), \]
其中\(\alpha\)是兩向量之間的夾角。
圖1 向量P在Q上的投影
如上面圖1，向量\(\mathbf{P}\)在\(\mathbf{Q}\)上的投影為:
\[proj_Q \mathbf{P} = \frac{\mathbf{P}\cdot \mathbf{Q}}{\left\|\mathbf{Q}\right\|^2}\mathbf{Q} \]
向量\(\mathbf{P}\)垂直於\(\mathbf{Q}\)的分量為：
\[\begin{aligned} perp_Q \mathbf{P} &= \mathbf{P} - proj_Q \mathbf{P}\\ &=\mathbf{p} - \frac{\mathbf{P}\cdot \mathbf{Q}}{\left\|\mathbf{Q}\right\|^2}\mathbf{Q} \end{aligned} \]
其中，向量\(\mathbf{P}\)在\(\mathbf{Q}\)上的投影可以看作\(\mathbf{P}\)的線性變換，可以寫成矩陣向量積：
\[proj_Q \mathbf{P} = \frac{1}{\left\|\mathbf{Q}\right\|^2}\left[ \begin{matrix} \mathcal{Q}_x^2 & \mathcal{Q}_x\mathcal{Q}_y & \mathcal{Q}_x\mathcal{Q}_z\\ \mathcal{Q}_x\mathcal{Q}_y & \mathcal{Q}_y^2 & \mathcal{Q}_y\mathcal{Q}_z \\ \mathcal{Q}_x\mathcal{Q}_z & \mathcal{Q}_y\mathcal{Q}_z & \mathcal{Q}_z^2 \end{matrix} \right] \left[ \begin{matrix} P_x\\ P_y\\ P_z \end{matrix} \right] \qquad (2) \]
1.2 叉積(cross product)
給定兩個3D向量\(\mathbf{P}, \mathbf{Q}\)，則它們的叉積(又稱為向量積，vector product)是一個向量:
\[\mathbf{P}\times \mathbf{Q} = \left \langle P_yQ_z - P_zQ_y, P_zQ_x - P_xQ_z, P_xQ_y - P_yQ_x \right \rangle. \]
叉積的模:
\[\left \| \mathbf{P}\times \mathbf{Q}\right \| = \left \| \mathbf{P} \right \| \left \| Q \right \| \sin\alpha \]
叉積也可以寫成矩陣向量相乘的形式：
\[\mathbf{P}\times \mathbf{Q} = \left[ \begin{matrix} 0 & -P_z & P_y \\ P_z & 0 & -P_x \\ -P_y & P_x & 0 \end{matrix} \right] \left[ \begin{matrix} \mathcal{Q}_x \\ \mathcal{Q}_y \\ \mathcal{Q}_z \end{matrix} \right] \qquad (3) \]
先看二維空間中的旋轉。
圖2 x-y平面中向量P旋轉90度
如上面圖2，在x-y平面上把向量\(\mathbf{P} = \left \langle x, y \right \rangle\)逆時針旋轉90度，變成了向量\(\mathbf{Q} = \left \langle -P_y, P_x \right \rangle = \left \langle -y, x \right \rangle\)。向量\(\mathbf{P}, \mathbf{Q}\)形成了x-y平面的一個正交基，因此我們可以用這兩個向量表示x-y平面的任意向量。
圖3 x-y平面中向量P旋轉theta度
如上面圖3，向量\(\mathbf{Q}\)是向量\(\mathbf{P}\)逆時針旋轉90度后得到的向量，向量\(\mathbf{P'}\)是向量\(\mathbf{P}\)旋轉\(\theta\)度得到的向量，則：
\[\mathbf{P'} = \mathbf{P}\cos\theta + \mathbf{Q}\sin\theta . \]
即
\[P'_x = P_x\cos\theta - P_y\sin\theta \\ P'_y = P_y\cos\theta + P_x\sin\theta \]
寫成矩陣形式，即：
\[\mathbf{P'}= \left[ \begin{matrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{matrix} \right] \mathbf{P} \]
上面的旋轉矩陣可以擴展為三維空間中繞z軸旋轉\(\theta\)角度的矩陣(只要保證旋轉后z坐標不變)：
\[\mathbf{R}_z(\theta) = \left[ \begin{matrix} \cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{matrix} \right] \]
類似地，也可以推出三維空間中繞x軸、y軸的旋轉矩陣：
\[\mathbf{R}_x(\theta) = \left[ \begin{matrix} 1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{matrix} \right] \]
\[\mathbf{R}_y(\theta)=\left[ \begin{matrix} \cos\theta & 0 & \sin\theta\\ 0 & 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{matrix} \right] \]
繞任意軸的旋轉
假設我們想要繞任意一個軸\(\mathbf{A}\)對向量\(\mathbf{P}\)旋轉\(\theta\)度(其中\(\mathbf{A}\)是單位向量)。
因為旋轉軸\(\mathbf{A}\)是單位向量，所以\(\mathbf{P}\)在\(\mathbf{A}\)上的投影可以簡化為:
\[proj_{\mathbf{A}} \mathbf{P} = (\mathbf{A}\cdot \mathbf{P})\mathbf{A} \]
則\(\mathbf{P}\)垂直於\(\mathbf{A}\)的分量為：
\[perp_{\mathbf{A}} \mathbf{P}=\mathbf{P}-(\mathbf{A}\cdot \mathbf{P})\mathbf{A} \]
圖4. 繞任意軸旋轉示意圖
與旋轉軸平行的分量\(proj_{\mathbf{A}} \mathbf{P}\)不受旋轉的影響，我們只需要考慮垂直旋轉軸的分量\(perp_{\mathbf{A}} \mathbf{P}\)。要注意的是，最后計算出旋轉后的\(perp_{\mathbf{A}} \mathbf{P}\)要加上與旋轉軸平行的分量，才能得到最終的旋轉后的向量。
垂直分量繞\(\mathbf{A}\)的旋轉發生於垂直於旋轉軸\(\mathbf{A}\)的平面上。像之前一樣，我們可以把旋轉后的向量表示成向量\(perp_{\mathbf{A}} \mathbf{P}\)和\(perp_{\mathbf{A}} \mathbf{P}\)繞\(\mathbf{A}\)逆時針旋轉90度后的向量(即下文說的\(\mathbf{A}\times \mathbf{P}\))的線性組合(linear combination)。
如圖4所示，\(perp_{\mathbf{A}} \mathbf{P}\)的長度顯然為\(\left \|\mathbf{P}\right \|\sin\alpha\)，又旋轉軸\(\mathbf{A}\)是單位向量，則\(\mathbf{A}\times \mathbf{P}\)剛好長度和\(相同，且垂直於\)\mathbf{A}\(和\)\mathbf{P}$，正是我們想要的向量。
現在，按照上面的論述，\(perp_{\mathbf{A}} \mathbf{P}\)繞\(\mathbf{A}\)旋轉\(\theta\)后的向量可以表示為:
\[\left[\mathbf{P} - (\mathbf{A}\cdot \mathbf{P})\mathbf{A} \right]\cos\theta + (\mathbf{A}\times\mathbf{P})\sin\theta \]
再加上不受旋轉影響的分量\(proj_{\mathbf{A}} \mathbf{P}\)，就得到了向量\(\mathbf{P}\)繞旋轉軸\(\mathbf{A}\)旋轉\(\theta\)角度后的向量\(\mathbf{P'}\)
\[\mathbf{P'} = \mathbf{P}\cos\theta + (\mathbf{A}\times \mathbf{P})\sin\theta + \mathbf{A}(\mathbf{A}\cdot\mathbf{P})(1-\cos\theta) \qquad (4) \]
把\(\mathbf{A}\times \mathbf{P}\)和\(\mathbf{A}(\mathbf{A}\cdot\mathbf{P})\)換成對應的矩陣形式(參考上面的公式3和公式2)。我們可以得到：
\[\mathbf{P'} = \left[ \begin{matrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{matrix} \right]\mathbf{P}\cos\theta + \left[ \begin{matrix} 0 & -A_z & A_y\\ A_z & 0 & -A_x\\ -A_y & A_x & 0 \end{matrix} \right]\mathbf{P}\sin\theta \\+ \left[ \begin{matrix} A_x^2 & A_xA_y & A_xA_z\\ A_xA_y & A_y^2 & A_yA_z\\ A_xA_z & A_yA_z & A_z^2 \end{matrix} \right]\mathbf{P}(1-\cos\theta) \qquad(5) \]
把公式(5)中的各項結合起來(即提取出\(\mathbf{P}\))，同時設\(c=\cos\theta, s=\sin\theta\)，則繞任意旋轉軸\(\mathbf{A}\)旋轉一個向量\(\theta\)角度的旋轉矩陣為:
\[\mathbf{R}_{\mathbf{A}}(\theta)=\left[ \begin{matrix} c+(1-c)A_x^2 & (1-c)A_xA_y-sA_z & (1-c)A_xA_z+sA_y\\ (1-c)A_xA_y+sA_z & c+(1-c)A_y^2 & (1-c)A_yA_z-sA_x\\ (1-c)A_xA_z-sA_y & (1-c)A_yA_z+sA_x & c+(1-c)A_z^2 \end{matrix} \right] \quad (6) \]
3 四元數
3.1 四元數定義
四元數(quaternion)可以看作中學時學的復數的擴充，它有三個虛部。形式如下：
\(\mathbf{q}=<w, x, y, z> = w + xi + yj + zk\)，可以寫成\(\mathbf{q}=s+\mathbf{v}\)
具有如下性質：
\(i^2 = j^2 = k^2 = -1\)
\(ij = -ji = k\)
\(jk = -kj = i\)
\(ki = -ik = j\)
設\(\mathbf{q}_1 = s_1 + \mathbf{v}_1\)，\(\mathbf{q}_2 = s_2 + \mathbf{v}_2\)，則
\(\mathbf{q}_1\mathbf{q}_2 = s_1s_2 - \mathbf{v}_1\cdot \mathbf{v}_2 + s_1\mathbf{v}_2 + s_2\mathbf{v}_1 + \mathbf{v}_1\times \mathbf{v}_2\)
3.2 共 軛四元數
一個四元數\(\mathbf{q} = s + \mathbf{v}\)的共軛(用\(\bar{\mathbf{q}}\)表示)為\(\bar{\mathbf{q}} = s - \mathbf{v}\)
一個四元數和它的共軛的積等於該四元數與自身的點乘，也等於該四元數長度的平方。即，
\(\mathbf{q}\bar{\mathbf{q}} = \bar{\mathbf{q}} \mathbf{q}=\mathbf{q}\cdot\mathbf{q} = ||\mathbf{q}||^2=q^2\)
3.3 四元數的逆
一個非零四元數\(\mathbf{q}\)的逆為\(\mathbf{q^{-1}=\frac{\bar{\mathbf{q}}}{q^2}}\)。
顯然\(\mathbf{q}\mathbf{q^{-1}}=1\)
3.4 四元數表示旋轉
終於到正題了。
三維空間中的旋轉可以被認為是一個函數\(\phi\)，從\(\mathbb{R}^3\)到自身的映射。函數\(\phi\)要想表示一個旋轉，必須在旋轉過程中保持向量長度(lengths)、向量夾角(angles)和handedness不變。
(handedness和左右手坐標系有關，例如左手坐標系中向量旋轉后，仍要符合左手坐標系規則)
長度保持不變要滿足：
\[\left \|\phi(\mathbf{P}) \right \| = \left \|\mathbf{P} \right\| \qquad (7) \]
角度不變要滿足：
\[\phi(\mathbf{P_1})\cdot \phi(\mathbf{P_2}) = \mathbf{P_1}\cdot \mathbf{P_2} \qquad (8) \]
最后，handedness保持不變要滿足：
\[\phi(\mathbf{P_1})\times \phi(\mathbf{P_2})=\phi(\mathbf{P_1}\times\mathbf{P_2}) \qquad(9) \]
擴展函數\(\phi\)成為一個從\(\mathbb{H}\)到其自身的映射，要求\(\phi(s+\mathbf{V})=s+\phi(\mathbf{V})\)，這使得我們可以重寫上面的方程(7)：
\[\phi(\mathbf{P_1})\cdot\phi(\mathbf{P_2})=\phi(\mathbf{P_1}\cdot\mathbf{P_2}) \qquad (10) \]
注意，這里把\(\mathbf{P_1}\)和\(\mathbf{P_2}\)當作標量部分(scalar part)為0的四元數(其實就是三維空間中的點)，所以根據\(\phi(s+\mathbf{V})=s+\phi(\mathbf{V})\)可得到公式(10)。我們可以把方程(9)、(10)合寫成一個公式(角度保持不變、handedness保持不變):
\[\phi(\mathbf{P_1})\phi(\mathbf{P_2})=\phi(\mathbf{P_1P_2}) \qquad (11) \]
方程(11)兩向量的乘其實既點乘、又是叉乘，二者合二為一，用一種形式寫出來了。
一個滿足方程(11)的函數\(\phi\)被稱為同態(homomorphism，異質同形)。
重點來了：
由下面公式(12)給出的一類函數滿足方程(7)和方程(11)，可以表示旋轉。
\[\phi_{\mathbf{q}}(\mathbf{P})=\mathbf{q}\mathbf{P}\mathbf{q^{-1}} \qquad (12) \]
這里\(\mathbf{q}\)是一個非零四元數，函數的參數\(\mathbf{P}\)可以看作三維空間中的點(即一個實部或標量部分為0的四元數)。
首先證明方程(12)中的函數滿足方程(7):
\[\left \|\phi_{\mathbf{q}}(\mathbf{P})\right \| = \left \|\mathbf{qPq^{-1}}\right \|=\left \|q\right \| \left \|\mathbf{P}\right \| \left\| q^{-1} \right \| = \left \|\mathbf{P}\right \| \frac{\left \|\mathbf{q}\right \|\left \|\bar{\mathbf{q}}\right \|}{q^2}=\left \|\mathbf{P}\right \| \qquad (13) \]
更進一步地，\(\phi_{\mathbf{q}}\)是一個homomorphism，即滿足方程(11)，因為：
\[\phi_{\mathbf{q}}(\mathbf{P_1})\phi_{\mathbf{q}}(\mathbf{P_2})=\mathbf{qP_1q^{-1}qP_2q^{-1}}=\mathbf{qP_1P_2q^{-1}}=\phi_{\mathbf{q}}(\mathbf{P_1P_2}) \qquad (14) \]
接下來我們要找到一個四元數\(\mathbf{q}\)，使它對應於一個繞旋轉軸\(\mathbf{A}\)旋轉\(\theta\)的旋轉變換。可以很容易證明對於任意非零標量\(a\)，\(\phi_{a\mathbf{q}}=\phi_{\mathbf{q}}\)成立。為了使事情簡單點，我們可以選擇單位四元數\(\mathbf{q}\)。
設\(\mathbf{q}=s + \mathbf{v}\)為一個單位四元數，則它的逆為\(\mathbf{q^{-1}} = s - \mathbf{v}\)。再給一個三維空間中的點\(\mathbf{P}\)(相當於一個標量部分為0的四元數)，就有：
\[\begin{aligned} \mathbf{qPq^{-1}} &= (s + \mathbf{v})\mathbf{P}(s-\mathbf{v})\\ &= (-\mathbf{v}\cdot \mathbf{P} + s\mathbf{P} + \mathbf{v}\times\mathbf{P})(s-\mathbf{v})\\ &= -s\mathbf{p}\times\mathbf{v}-\mathbf{v}\times\mathbf{p}\times\mathbf{v}+s^2\mathbf{P}+s\mathbf{v}\times\mathbf{P}+\mathbf{v}\mathbf{P}\mathbf{v}-\mathbf{v}\mathbf{P}s+s\mathbf{P}\mathbf{v}+\mathbf{v}\times\mathbf{P}\cdot\mathbf{V} \\ &=s^2\mathbf{P}+2s\mathbf{v}\times \mathbf{P}+(\mathbf{v}\cdot\mathbf{P})\mathbf{v}-\mathbf{v}\times\mathbf{P}\times\mathbf{v} \end{aligned} \qquad (15) \]
定理：給定任意兩個3D向量\(\mathbf{P}, \mathbf{Q}\)，則：
\(\mathbf{P}\times(\mathbf{Q}\times\mathbf{P})=\mathbf{P}\times\mathbf{Q}\times\mathbf{P}=P^2\mathbf{Q}-(\mathbf{P}\cdot\mathbf{Q})\mathbf{P}\)
所以，對方程(15)中的\(\mathbf{v}\times\mathbf{P}\times\mathbf{v}\)施加上述定理，方程(15)就變成了：
\[\mathbf{qPq^{-1}}=(s^2-\mathbf{v}^2)\mathbf{P}+2s\mathbf{v}\times\mathbf{P}+2(\mathbf{v}\cdot\mathbf{P})\mathbf{v} \qquad (16) \]
設\(\mathbf{v}=t\mathbf{A}\)，此處\(\mathbf{A}\)是一個單位向量，即旋轉軸，重寫方程(16)為：
\[\mathbf{qPq^{-1}}=(s^2-t^2)\mathbf{P}+2st\mathbf{A}\times\mathbf{P}+2t^2(\mathbf{A}\cdot\mathbf{P})\mathbf{A} \qquad (17) \]
把方程(17)和上面說過的繞任意軸旋轉的公式(4)進行比較。
上面說過的公式(4)😒\mathbf{P'} = \mathbf{P}\cos\theta + (\mathbf{A}\times \mathbf{P})\sin\theta + \mathbf{A}(\mathbf{A}\cdot\mathbf{P})(1-\cos\theta) \qquad $
我們可以推出以下等式：
\[\begin{aligned} s^2 - t^2 &= \cos\theta\\ 2st &= \sin\theta\\ 2t^2 &= 1 - \cos\theta \end{aligned} \]
由第三個等式可以解出:
\[t=\sqrt{\frac{1-\cos\theta}{2}}=\sin\frac{\theta}{2} \]
由第一和第三個等式可推出\(s^2+t^2=1\)，所以\(s=\cos\frac{\theta}{2}\)。
所以，我們找到了一個單位四元數\(\mathbf{q}\)，其對應於繞旋轉軸\(\mathbf{A}\)旋轉\(\theta\)角度的旋轉變換(重要結論)：
\[\begin{aligned} \mathbf{q} &=s+\mathbf{v}\\ &=s+t\mathbf{A}\\ &=\cos\frac{\theta}{2} + \mathbf{A}\sin\frac{\theta}{2} \end{aligned} \qquad (18) \]
這里順便證明一下對於任意非零\(a\)(如，\(a=-1\))，\(a\mathbf{q}\)和\(\mathbf{q}\)表示同一個旋轉，因為
\[(a\mathbf{q})\mathbf{P}(a\mathbf{q})^{-1}=a\mathbf{qP}\frac{\mathbf{q^{-1}}}{a}=\mathbf{qPq^{-1}} \]
兩個四元數\(\mathbf{q_1}\)和\(\mathbf{q_2}\)的乘積也表示一個旋轉。例如，\(\mathbf{q_1q_2}\)表示施加旋轉\(\mathbf{q_2}\)，再施加旋轉\(\mathbf{q_1}\)。因為：
\[\mathbf{q_1(q_2Pq_2^{-1})q_1^{-1}}=\mathbf{(q_1q_2)P(q_1q_2)^{-1}} \]
我們可以連接任意多個表示旋轉的四元數，形成一個單一的四元數。
兩個四元數相乘需要16個乘加運算(multiply-add)，然而兩個\(3\times 4\)矩陣相乘需要27個乘加運算。因此在某些情況下(比如要對同一個對象施加多個旋轉變換時)，用四元數表示旋轉，計算效率會高些。
總之，通過一個四元數\(\mathbf{q}\)對一個點\(\mathbf{P}\)施加旋轉變換，只需要做如下計算:
\[\mathbf{P'}=\mathbf{qPq^{-1}} \]
3.5 四元數轉換為旋轉矩陣
有時需要把四元數轉換為\(3\times 3\)的旋轉矩陣，例如，使用某些3D圖形庫時。
我們可以根據方程(2)和方程(3)，把方程(17)寫程矩陣形式，來找到四元數\(\mathbf{q}=s+t\mathbf{A}\)對應的旋轉矩陣:
\[\mathbf{qPq^{-1}}=\left[ \begin{matrix} s^2-t^2 & 0 & 0\\ 0 & s^2-t^2 & 0\\ 0 & 0 & s^2-t^2 \end{matrix} \right]\mathbf{P}+ \left[ \begin{matrix} 0 & -2stA_z & 2stA_y\\ 2stA_z & 0 & -2stA_x\\ -2stA_y & 2stA_x &0 \end{matrix} \right]\mathbf{P}\\ +\left[ \begin{matrix} 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_y & 2t^2A_yA_z & 2t^2A_z^2 \end{matrix} \right]\mathbf{P} \qquad (19) \]
把四元數\(\mathbf{q}\)寫成四維向量的形式\(\mathbf{q}=\left \langle w, x, y, z \right\rangle\)，其中\(w=s, x=tA_x, y=tA_y, z=tA_z\)。因為\(\mathbf{A}\)是單位向量，所以：
\[x^2 + y^2 + z^2 = t^2 \quad A^2=t^2 \]
用\(w, x, y, z\)的形式來重寫方程(19)：
\[\mathbf{qPq^{-1}}=\left[ \begin{matrix} 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{matrix} \right]\mathbf{P}\\ +\left[ \begin{matrix} 0 & -2wz & 2wy\\ 2wz & 0 & -2wx\\ -2wy & 2wx & 0 \end{matrix} \right]\mathbf{P}+ \left[ \begin{matrix} 2x^2 & 2xy & 2xz\\ 2xy & 2y^2 & 2yz\\ 2xz & 2yz & 2z^2 \end{matrix} \right]\mathbf{P} \qquad (20) \]
因為\(\mathbf{q}\)是一個單位四元數，則\(w^2+x^2+y^2+z^2=1\)，所以
\[w^2 - x^2 - y^2 - z^2 = 1 -2x^2 - 2y^2 - 2z^2 \]
結合上面這個等式及方程(20)中的三個矩陣，我們可以得到對應於四元數\(\mathbf{q}\)的旋轉矩陣：
\[\mathbf{R_q}=\left[ \begin{matrix} 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{matrix} \right] \qquad (21) \]
原文，見知乎專欄文章:https://zhuanlan.zhihu.com/p/78987582
參考文獻:
- Mathematics for 3D Game Programming and Computer Graphics 英文版第三版，第2、3、4章。
