四元数 Quaternions

1. 数学定义

  在进行四元数对旋转的作用探讨前，我们先看一下四元数的定义，并不管三七二十一扔出一堆公式以供后续使用。首先四元数是一种超复数，与简单复数仅包含一个虚部$i$，四元数包含三个虚部$i,j,k$和一个实部，这里定义四元数入下：

$\hat{\textbf{q}}=(\textbf{q}_v, q_w)=iq_x+jq_y+kq_z+q_w \\ \textbf{q}_v=iq_x+jq_y+kq_z=(q_x,q_y,q_z)$ $i^2=j^2=k^2=-1, jk=-kj=i, ki=-ik=j,ij=-ji=k$

  于是可以有如下的运算：
Addition:    $\hat{\textbf{q}}+\hat{\textbf{r}}=(\textbf{q}_v, q_w)+(\textbf{r}_v, r_w) = (\textbf{q}_v+\textbf{r}_v,q_w+r_w)$

Multiplication: $\hat{\textbf{q}}\hat{\textbf{r}}=(iq_x+jq_y+kq_z+q_w)(ir_x+jr_y+kr_z+r_w) =(\textbf{q}_v\times\textbf{r}_v+r_w\textbf{q}_v+q_w\textbf{r}_v, q_wr_w-\textbf{q}_v\cdot\textbf{r}_v)$

Conjugate:   $\hat{\textbf{q}}^*=(-\textbf{q}_v, q_w)$

Norm:      $n(\hat{\textbf{q}})=\sqrt{\hat{\textbf{q}}\hat{\textbf{q}}^*}=\sqrt{\hat{\textbf{q}}^*\hat{\textbf{q}}}=\sqrt{\textbf{q}_v\cdot\textbf{q}_v+q_w^2}=\sqrt{q_x^2+q_y^2+q_z^2+q_w^2}$

Identity:     $\hat{\textbf{i}}=(\textbf{0}, 1)$

Inverse:    $\hat{\textbf{q}}^{-1}=\frac{1}{n(\hat{\textbf{q}})^2}\hat{\textbf{q}}^*$

  然后我们可以定义单位向量为：

$\hat{\textbf{q}}=(sin\phi\textbf{u}_q,cos\phi)=sin\phi\textbf{u}_q+cos\phi$ 其中$||\textbf{u}_q||=1$，可以验证$n(\hat{\textbf{q}})=1$，于是根据该形式我们可以将其改写为如下形式： $\hat{\textbf{q}}=sin\phi\textbf{u}_q+cos\phi=e^{\phi\textbf{u}_q}$

于是可以扩展出如下定义：
Logarithm:   $log(\hat{\textbf{q}})=\phi\textbf{u}_q$
Power:     $\hat{\textbf{q}}^t=e^{\phi t\textbf{u}_q}=sin(\phi t)\textbf{u}_q + cos(\phi t)$

2. 旋转

  现在我们有了四元数的定义，考虑有一个单位四元数$\hat{\textbf{q}}=(sin\phi \textbf{u}_q, cos\phi)$，和一个希望进行旋转的坐标$\textbf{p}=(p_x\ p_y\ p_z\ p_w)^T$，并将其装载为四元数$\hat{\textbf{p}}$，于是可以发现如下的公式：

$\hat{\textbf{q}}\hat{\textbf{p}}\hat{\textbf{q}}^{-1}$

那么这个公式做了什么事呢，先将绕轴旋转的图拿出来如下：

首先可以发现如下的结果：

$\hat{\textbf{q}}^{-1}=\hat{\textbf{q}}^*=(-sin\phi \textbf{u}_q, cos\phi)$ $\textbf{u}\times\textbf{p}=\textbf{y}$ $\textbf{y}\times\textbf{u}=\textbf{x}$ $\textbf{p}\cdot\textbf{u}=||\textbf{u'}||$

接下来即可以尝试将上述公式展开
：

$$\require{enclose} \begin{align*} \hat{\textbf{q}}\hat{\textbf{p}}\hat{\textbf{q}}^{-1} &= (sin\phi \textbf{u}_q, cos\phi)(\textbf{p}_v, p_w)(-sin\phi \textbf{u}_q, cos\phi) \\ &= (sin\phi \textbf{u}_q\times\textbf{p}_v + p_wsin\phi \textbf{u}_q+cos\phi\textbf{p}_v, p_wcos\phi-sin\phi(\textbf{u}_q\cdot\textbf{p}_v))(-sin\phi \textbf{u}_q, cos\phi) \\ &=(sin\phi \textbf{y} + p_wsin\phi \textbf{u}_q+cos\phi\textbf{p}_v, p_wcos\phi-sin\phi||\textbf{u'}||))(-sin\phi \textbf{u}_q, cos\phi) \\ &= ((-sin^2\phi\textbf{y}\times\textbf{u}_q - \enclose{horizontalstrike}{p_wsin^2\phi\textbf{u}_q\times\textbf{u}_q} - sin\phi cos\phi\textbf{p}_v\times\textbf{u}_q) \\ &\ \ \ \ + (sin\phi cos\phi \textbf{y} + \enclose{horizontalstrike}{p_wsin\phi cos\phi\textbf{u}_q} + cos^2\textbf{p}_v) \\ &\ \ \ \ +(-\enclose{horizontalstrike}{p_wsin\phi cos\phi\textbf{u}_q}+sin^2\phi||\textbf{u'}||\textbf{u}_q), \\ &\ \ \ \ p_wcos^2\phi-\enclose{horizontalstrike}{sin\phi cos\phi||\textbf{u'}||} - (-\enclose{horizontalstrike}{sin^2\phi(\textbf{y}\cdot\textbf{u}_q)}-p_wsin^2\phi(\textbf{u}_q\cdot\textbf{u}_q)-\enclose{horizontalstrike}{sin\phi cos\phi(\textbf{p}_v\cdot\textbf{u}_q)}))\\ &=(-sin^2\phi\textbf{x}+2sin\phi cos\phi \textbf{y} + cos^2\phi \textbf{p}_v + sin^2\phi\textbf{u'},p_w) \\ &=(-sin^2\phi\textbf{x}+2sin\phi cos\phi \textbf{y} + cos^2\phi (\textbf{p}_v - \textbf{u'}) + \textbf{u'},p_w) \\ &=((cos^2\phi-sin^2\phi)\textbf{x}+2sin\phi cos\phi \textbf{y} + \textbf{u'},p_w) \\ &=(cos(2\phi)\textbf{x}+sin(2\phi)\textbf{y}+\textbf{u'},p_w) \end{align*} $$

  显然，该式实部未改变$p_w$的值，而虚部刚好使$(p_x, p_y, p_z)^T$按照单位轴$\textbf{u}_q$旋转了$2\phi$，如下图所示：

  并且容易验证$\hat{\textbf{q}}$和$-\hat{\textbf{q}}$表示的是同种旋转。
  所以最终我们可以将空间内绕轴$\textbf{u}_q$旋转$\phi$角度的四元数表示为如下形式：

$\hat{\textbf{q}}=(sin\frac{\phi}{2}\textbf{u}_q, cos\frac{\phi}{2})$



3. 四元数->旋转矩阵

   接下来我们需要将四元数表示为方便计算的旋转矩阵形式。上一步中我们已经展示了$\hat{\textbf{q}}\hat{\textbf{p}}\hat{\textbf{q}}^{*} $能够将$\textbf{p}$进行旋转，要得到旋转矩阵，我们尝试将$\textbf{p}_v$从中剥离出来，进行这一步前，先对如下条件进行说明：

$\hat{\textbf{q}} = \left[\begin{array}{cccc} q_x \\ q_y \\ q_z \\ q_w \end{array}\right] = \left[\begin{array}{cccc} u_xsin\phi \\ u_ysin\phi \\ u_zsin\phi \\ cos\phi \end{array}\right] \\ \textbf{p}=\textbf{Ip} \\ (\textbf{p}\cdot\textbf{u})\textbf{u} = \textbf{u}(\textbf{u}^T\textbf{p})=\left[\begin{array}{cccc} u_x^2 & u_xu_y & u_xu_z \\ u_xu_y & u_y^2 & u_yu_z \\ u_xu_z & u_yu_z & u_z^2\end{array}\right]\textbf{p}\\ \textbf{u}\times\textbf{p}=\left[\begin{array}{cccc} u_y p_z - u_z p_y \\ u_z p_x - u_x p_z \\ u_x p_y - u_y p_x \end{array}\right] = \left[\begin{array}{cccc} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{array}\right]\textbf{p}$

然后开始进行剥离：

$$ \begin{align*} \textbf{Rq}_v&=(cos^2\phi-sin^2\phi)\textbf{x}+2sin\phi cos\phi \textbf{y} + \textbf{u'}\\ &=(cos^2\phi-sin^2\phi)(\textbf{p}_v - \textbf{u'}) + 2sin\phi cos\phi \textbf{u}_q\times\textbf{p}_v+(cos^2\phi+sin^2\phi)\textbf{u'} \\ & = (2cos^2\phi - 1)\textbf{p}_v+ 2sin\phi cos\phi \textbf{u}_q\times\textbf{p}_v + 2sin^2\phi (\textbf{p}\cdot\textbf{u}_q)\textbf{u}_q \\ &= ((2q^2_w-1)\textbf{I}+2q_wsin\phi\left[\begin{array}{cccc} 0 & -u_z & u_y \\ u_z & 0 & -u_x \\ -u_y & u_x & 0 \end{array}\right]+2sin^2\phi\left[\begin{array}{cccc} u_x^2 & u_xu_y & u_xu_z \\ u_xu_y & u_y^2 & u_yu_z \\ u_xu_z & u_yu_z & u_z^2\end{array}\right])\textbf{p}_v\\ &=(\left[\begin{array}{cccc} 2q^2_w-1 & 0 & 0 \\ 0 & 2q^2_w-1 & 0 \\ 0 & 0 & 2q^2_w-1 \end{array}\right]+2q_w\left[\begin{array}{cccc} 0 & -q_z & q_y \\ q_z & 0 & -q_x \\ -q_y & q_x & 0 \end{array}\right]+2\left[\begin{array}{cccc} 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{array}\right])\textbf{p}_v \end{align*} $$ 综上，可以有旋转矩阵： $$ \begin{align*} \textbf{R} &= \left[\begin{array}{cccc} 2(q_x^2 + q^2_w)-1& 2(q_xq_y-q_zq_w) & 2(q_xq_z+q_yq_w) \\ 2(q_xq_y +q_zq_w)& 2(q_y^2 + q^2_w)-1 & 2(q_yq_z -q_xq_w)\\ 2(q_xq_z-q_yq_w) & 2(q_yq_z+q_xq_w) & 2(q_z^2 + q^2_w)-1\end{array}\right]\\ &=\left[\begin{array}{cccc} 1 - 2(q_y^2 + q^2_z)& 2(q_xq_y-q_zq_w) & 2(q_xq_z+q_yq_w) \\ 2(q_xq_y +q_zq_w)& 1-2(q_x^2 + q^2_z) & 2(q_yq_z -q_xq_w)\\ 2(q_xq_z-q_yq_w) & 2(q_yq_z+q_xq_w) & 1-2(q_x^2 + q^2_y) \end{array}\right] \end{align*}$$

   当然，另一种直接基于矩阵的计算方式或许更直观，将四元数的左乘直接写作如下形式：

$\begin{align*} \hat{\textbf{q}}\hat{\textbf{p}} &= \textbf{q}_v\times\textbf{p}_v+p_w\textbf{q}_v+q_w\textbf{p}_v+q_wp_w-\textbf{p}_v\cdot\textbf{q}_v \\ &= (\left[\begin{array}{cccc} 0 & -q_z & q_y & 0\\ q_z & 0 & -q_x & 0\\ -q_y & q_x & 0 & 0 \\ 0 & 0 & 0 & 0\end{array}\right] + \left[\begin{array}{cccc} 0 & 0 & 0 & q_x\\ 0 & 0 & 0 & q_y\\ 0 & 0 & 0 & q_z \\ 0 & 0 & 0 & 0\end{array}\right] + \left[\begin{array}{cccc} q_w & 0 & 0 & 0\\ 0 & q_w & 0 & 0\\ 0 & 0 & q_w & 0 \\ 0 & 0 & 0 & 0\end{array}\right]+ \left[\begin{array}{cccc} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 \\ -q_x & -q_y & -q_z & q_w\end{array}\right]) \left[\begin{array}{cccc} p_x \\ p_y \\ p_z \\ p_w \end{array}\right] \\ &= \left[\begin{array}{cccc} q_w & -q_z & q_y & q_x\\ q_z & q_w & -q_x & q_y\\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w\end{array}\right]\left[\begin{array}{cccc} p_x \\ p_y \\ p_z \\ p_w \end{array}\right] = \textbf{L}(\textbf{q})\textbf{p} \end{align*}$

同样地，右乘也可以表示为：

$\begin{align*} \hat{\textbf{p}}\hat{\textbf{q}} &= \textbf{p}_v\times\textbf{q}_v+p_w\textbf{q}_v+q_w\textbf{p}_v+q_wp_w-\textbf{p}_v\cdot\textbf{q}_v \\ &= (\left[\begin{array}{cccc} 0 & q_z & -q_y & 0\\ -q_z & 0 & q_x & 0\\ q_y & -q_x & 0 & 0 \\ 0 & 0 & 0 & 0\end{array}\right] + \left[\begin{array}{cccc} 0 & 0 & 0 & q_x\\ 0 & 0 & 0 & q_y\\ 0 & 0 & 0 & q_z \\ 0 & 0 & 0 & 0\end{array}\right] + \left[\begin{array}{cccc} q_w & 0 & 0 & 0\\ 0 & q_w & 0 & 0\\ 0 & 0 & q_w & 0 \\ 0 & 0 & 0 & 0\end{array}\right]+ \left[\begin{array}{cccc} 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 \\ -q_x & -q_y & -q_z & q_w\end{array}\right]) \left[\begin{array}{cccc} p_x \\ p_y \\ p_z \\ p_w \end{array}\right] \\ &= \left[\begin{array}{cccc} q_w & q_z & -q_y & q_x\\ -q_z & q_w & q_x & q_y\\ q_y & -q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w\end{array}\right]\left[\begin{array}{cccc} p_x \\ p_y \\ p_z \\ p_w \end{array}\right] = \textbf{R}(\textbf{q})\textbf{p} \end{align*}$ 于是旋转矩阵即可表示为： $\begin{align*} \textbf{M}&=\hat{\textbf{q}}\hat{\textbf{p}}\hat{\textbf{q}}^{-1}=\textbf{R}(\textbf{q}^{-1})\textbf{L}(\textbf{q})\textbf{p} \\ &=\left[\begin{array}{cccc} q_w & -q_z & q_y & -q_x\\ q_z & q_w & -q_x & -q_y\\ -q_y & q_x & q_w & -q_z \\ q_x & q_y & q_z & q_w\end{array}\right]\left[\begin{array}{cccc} q_w & -q_z & q_y & q_x\\ q_z & q_w & -q_x & q_y\\ -q_y & q_x & q_w & q_z \\ -q_x & -q_y & -q_z & q_w\end{array}\right]\left[\begin{array}{cccc} p_x \\ p_y \\ p_z \\ p_w \end{array}\right] \\ &=\left[\begin{array}{cccc} \textbf{R} & \textbf{0} \\ \textbf{0} & 1 \end{array}\right] \end{align*}$

4. 旋转矩阵->四元数

  那么当我们拿到如下的旋转矩阵:

$\textbf{M}=\left[\begin{array}{cccc} \textbf{R} & \textbf{0} \\ \textbf{0} & 1 \end{array}\right]$ 要怎么转换为四元数形式呢？我们首先有如下的发现（公式1）： $ m_{21}-m_{12}=4q_xq_w \\ m_{02}-m_{20}=4q_yq_w \\ m_{10}-m_{01}=4q_zq_w $ 而要得到$q_w$，则需要利用$\textbf{M}$的迹（trace）： $tr(\textbf{M})=4-4(q^2_x+q^2_y+q^2_z)=4q^2_w$ 综上即有（公式2）： $\begin{array}{cccc} q_w = \frac{1}{2}\sqrt{tr(\textbf{M})} & q_x = \frac{m_{21}-m_{12}}{4q_w} \\ q_y = \frac{m_{02}-m_{20}}{4q_w} & q_z = \frac{m_{10}-m_{01}}{4q_w} \end{array} $   显然上述计算方式有两个问题，首先$q_w > 0$，这个问题我们之前的计算发现对于$q_w=cos\phi$,实际旋转角度为$2\phi$，当我们限制$\phi\in(-\frac{\pi}{2},\frac{\pi}{2}]$时，旋转角度刚好覆盖$2\pi$，并且$q_w$一定大于0。另一个问题是在$q_w$接近时结果不稳定，所以在Real-Time Rendering (4th Edition)中提到了更稳定的算法， 首先令$t=q^2_w - q^2_x-q^2_y-q^2_z$，则有： $ m_{00}=t+2q^2_x \\ m_{11}=t+2q^2_y \\ m_{22}=t+2q^2_z \\ u = m_{00}+m{11}+m{22}=t+2q^2_w $ 于是有如下的计算方式： 1. 根据$m_{00}, m{11}, m{22}, u$判断$q^2_x,q^2_y,q^2_z,q^2_w$的大小 2. 若q_w最大，则按照公式1进行计算 3. 反之，选择最大的值，按照如下的方式计算结果，并进而用公式1计算剩余值 $ 4q^2_x=+ m_{00} - m_{11} - m_{22} + m_{33} \\ 4q^2_y=- m_{00} + m_{11} - m_{22} + m_{33} \\ 4q^2_z=-m_{00} - m_{11} + m_{22} + m_{33} \\ $

5. 球面线性插值

  我们可以对任意两个四元数进行插值，简单地表示如下：

$\textbf{q}(t)=\textbf{q}_0^{1-t}\textbf{q}_1^t$
即两个旋转的线性组合，但要表示为更容易计算的方式，则是四维单位球上进行球面线性插值其思想是以起点构造基坐标，然后进行施密特正交化，于是即可以如下地得到线性插值结果：

其中$cos\phi=\textbf{q}_0\cdot\textbf{q}_1$。
  可以看到相较于欧拉角，任意两个四元数都可以得到稳定的插值结果。

6. 向量旋转

  很多时候我们希望将向量$\textbf{s}$旋转到向量$\textbf{t}$，并且希望旋转的路径越小越好（即暗示了路径是一条优弧）。那么根据轴向角的方式就很容易描述这个过程了，如下图所示：

首先有$\textbf{u}=\frac{\textbf{s}\times\textbf{t}}{||\textbf{s}\times\textbf{t}||}=\frac{\textbf{s}\times\textbf{t}}{sin\theta}$，$e=\textbf{s}\cdot\textbf{t}=cos\theta$，于是可以用四元数表示旋转：

$\hat{\textbf{q}}=(sin\frac{\theta}{2}\textbf{u}, cos\frac{\theta}{2}) = (\frac{sin\frac{\theta}{2}}{sin\theta}\textbf{s}\times\textbf{t}, cos\frac{\theta}{2})=(\frac{1}{\sqrt{2(1+e)}}\textbf{s}\times\textbf{t}, \sqrt{\frac{1+e}{2}})$
需要注意的是仅当$\textbf{s}$和$\textbf{t}$相反时，上述计算会出现问题。

参考文献

Real-Time Rendering (4th Edition)