四元数（Quaternion）食用指南

四元数（Quaternion）食用指南

“这简直就是黑魔法！”

开发时，每次遇到旋转问题时总会心头一震，在欧拉角和四元数这两种处理方式的选择上犹豫不决，不知不觉就陷入了四元数的淤泥中…接下来，我决定发起总攻，好好理一理四元数。

阅读注意：

• 本文的图例以左手坐标系为主
• 空间中的向量采用列项量表示
• 四元数为Hamilton形式

1 左右手坐标系对旋转的影响在哪

我们处于三维世界中，一个旋转可以描述为由一个旋转轴 $\text{v}$ 和旋转角 $\theta$ 组成。这里其实隐含了一个问题，即 $\theta$ 的旋转究竟以逆时针为正方向，还是以顺时针为正方向。

左手坐标系下我们可以用左手绕轴，拇指指向正方向，手指旋向为顺时针，即顺时针为正方向。同理，右手坐标系下用右手绕轴，拇指指向正方向，手指旋向为逆时针，即逆时针为正方向。

这里也是关键所在，对于一个旋转角度$\theta$ ，在左手坐标系下解释为顺时针旋转$\theta$ ，而在右手坐标系下解释为逆时针旋转$\theta$。

也许你在之前还看过其他文章，不同文章在解决旋转矩阵推导、或四元数转旋转矩阵等问题时，结论公式内的正负符号会不一致，甚至还会分出左手坐标系下的旋转矩阵，或右手坐标系下的旋转矩阵的说法。其原因就是隐含的旋转正方向不一样，旋转角出现符号差异。例如：你处理左手坐标系下的旋转，却取逆时针方向为正方向，最后公式自然就有符号差异。

如果正方向随着左右手坐标系改变，最后应该是同一个公式，只是对角度旋向的解释不一样。所以，在以下的叙述中，只有在出现具体某个坐标系下时，我才会强调旋转的方向。

2 令人头疼的旋转

2.1 旋转矩阵

我们首先来看一下旋转矩阵，设有一个三维旋转矩阵$M$，如下所示：
$$M=\left(\begin{array}{ccc}{m}_{00}& m_{01}& m_{02}\\ m_{10}& m_{11}& m_{12}\\ m_{20}& m_{21}& m_{22}\end{array}\right)$$
旋转矩阵$M$中各元素与旋转轴和旋转角的对应关系，我们不能一眼直观得出。但这里简化一下，将维度降低到二维。

我们绕原点旋转一个点 $p(a,b)$ 到 $p^{{}^{\prime }}(c,d)$ ，我们设点 $p$ 到原点的距离为 $l$。那么就有：
$$\begin{gathered} a=l\cos \alpha \\ b=l\sin \alpha \\ c=l\cos (\alpha +\theta )=l\cos \alpha \cos \theta -l\sin \alpha \sin \theta =a\cos \theta -b\sin \theta \\ d=l\sin (\alpha +\theta )=l\sin \alpha \cos \theta +l\cos \alpha \sin \theta =a\sin \theta +b\cos \theta \end{gathered}$$
我们可以很快地写出一个二维的旋转矩阵$M_2$，他能让点绕原点逆时针旋转θ角度（注意：这里采用矩阵乘列向量的方式），如下所示：
$$M_2=\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)$$
事实上，三维旋转矩阵也能以类似的几何方式计算出矩阵内各个元素的值，我们将会在四元数部分一起进行推导。我们将上面的二维旋转矩阵扩展到三维，可以得出绕 $x$ 轴、$y$ 轴、$z$ 轴的旋转矩阵$R_x$、$R_y$ 和 $R_z$。

$$R_x=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & \sin \theta \\ 0& -\sin \theta & \cos \theta \end{array}\right)R_y=\left(\begin{array}{ccc}\cos \theta & 0& -\sin \theta \\ 0& 1& 0\\ \sin \theta & 0& \cos \theta \end{array}\right)R_z=\left(\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right)$$
上述推导在左手坐标系中进行，在扩展到三维的时候，要注意哪个轴是竖轴、哪个轴是横轴，按顺序来就好。

在实际应用中，旋转矩阵$R$大部分情况下会与缩放矩阵 $S$ 和平移矩阵 $T$ 组成一个变换矩阵$C=SRT$ 。如果单纯用矩阵来旋转计算成本是有点高的。

2.2 欧拉角

欧拉角简单地说就是由三个独立的旋转角度构成，每个分量记录绕各自轴旋转的角度，我们简单地写为：
$$EulerAngle=(\alpha ,\beta ,\gamma )$$
这其实是比较符合日常生活习惯的一种表达方式，例如，我们描述一个旋转时，会说它绕$x$轴转了30度，再绕$y$轴转了40度，最后绕$z$轴转了50度。这其中就有了一个先后旋转顺序X-Y-Z。 我们按照这个顺序，用旋转矩阵的方式写出来。
这个旋转矩阵有个致命的缺陷，这也是欧拉角的不足之处——万向锁问题。

当 $\beta =90^{°}$ 时，旋转矩阵会变成以下形式：

看上去有点类似绕xyz轴的旋转矩阵，如果我们把这个矩阵乘以一个列向量 $P(x,y,z)$
$$P^{{}^{\prime }}=R_z{R}_{y90}{R}_{x}P=\left[\begin{array}{c}y\sin (\gamma +\alpha )-z\cos (\gamma +\alpha )\\ y\cos (\gamma +\alpha )+z\sin (\gamma +\alpha )\\ x\end{array}\right]$$
这里可以看到无论怎么调整 $\gamma$ 和 $\alpha$ ，旋转后的点只会在XY平面转动，第一次旋转和最后一次旋转等价。

2.3 四元数

在进行四元数这部分，我准备从结论反向推导一波。

我们定义一个四元数 $q(x,y,z,w)$ ， $w$ 是实数部分， $i^2=j^2=k^2=ijk=-1$ ，$i$、$j$ 和 $k$ 为虚数，它们关系如下：
$$q=w+xi+yj+zk$$
四元数也可以用有序对记为 $q=[w,\text{v}]$，其中 $\text{v}$ 为向量$(x,y,z)$，代表 $\text{v}=xi+yj+zk$。

那么虚数就有对应的写法：

$$i=[0,\text{i}]j=[0,\text{j}]k=[0,\text{k}]$$
上面的向量 $\text{i}$ 相当于 $(1,0,0)$ ，向量 $\text{j}$ 相当于 $(0,1,0)$ ,向量 $\text{k}$ 相当于 $(0,0,1)$ 。

那么有几个特殊的四元数，我们需要注意一下：

• 纯四元数(Pure Quaternion) 定义为 $q(x,y,z,0)$ ，用有序对记为$[0,\text{v}]$。
• 单位四元数(Unit quaternion) 定义为 $q(x,y,z,w)$，其中 $x^2+y^2+z^2+w^2=1$，用有序对可以记为$[\cos \theta ,\sin \theta \text{n}]$ ，其中 $\text{n}$ 为单位向量。

2.3.1 四元数相乘会发生什么

那么我们定义两个四元数 $q_a(x_a,y_a,z_a,w_a)$ 和 $q_b(x_b,y_b,z_b,w_b)$ ，可分别记为$[w_a,\text{a}]$ 和 $[w_b,\text{b}]$，其中：
$$\begin{gathered} \text{a}=x_{a}i+y_{a}j+z_{a}k \\ \text{b}=x_{b}i+y_{b}j+z_{b}k \end{gathered}$$
接下来，看看两个四元数相乘会发生什么。
$$\begin{array}{rl}{q}_a{q}_b=& [w_a,\text{a}][w_b,\text{b}]\\ =& (w_a+x_{a}i+y_{a}j+z_{a}k)(w_b+x_{b}i+y_{b}j+z_{b}k)\\ =& (w_a{w}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b)\\ & +(w_a{x}_b+w_b{x}_a+y_a{z}_b-y_b{z}_a)[0,\text{i}]\\ & +(w_a{y}_b+w_b{y}_a+z_a{x}_b-z_b{x}_a)[0,\text{j}]\\ & +(w_a{z}_b+w_b{z}_a+x_a{y}_b-x_b{y}_a)[0,\text{k}]\end{array}$$
换算过程中注意通过 $ijk=-1$ 可以推出 $ij=k$ 、$ji=-k$ 等等式。注意，我们正在尝试通过$q=[w,\text{a}]$这种写法在计算过程消除虚根的影响。
$$\begin{array}{rl}{q}_a{q}_b=& [w_a,\text{a}][w_b,\text{b}]\\ =& (w_a{w}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b)[1,\text{0}]\\ & +(w_a{x}_b+w_b{x}_a+y_a{z}_b-y_b{z}_a)[0,\text{i}]\\ & +(w_a{y}_b+w_b{y}_a+z_a{x}_b-z_b{x}_a)[0,\text{j}]\\ & +(w_a{z}_b+w_b{z}_a+x_a{y}_b-x_b{y}_a)[0,\text{k}]\\ =& [w_a{w}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b,\text{0}]\\ & +[0,(w_a{x}_b+w_b{x}_a+y_a{z}_b-y_b{z}_a)\text{i}]\\ & +[0,(w_a{y}_b+w_b{y}_a+z_a{x}_b-z_b{x}_a)\text{j}]\\ & +[0,(w_a{z}_b+w_b{z}_a+x_a{y}_b-x_b{y}_a)\text{k}]\\ =& [w_a{w}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b,\\ & w_a(x_a\text{i}+y_a\text{j}+z_a\text{k})+w_b(x_b\text{i}+y_b\text{j}+z_b\text{k})\\ & +(y_a{z}_b-y_b{z}_a)\text{i}+(z_a{x}_b-z_b{x}_a)\text{j}+(x_a{y}_b-x_b{y}_a)\text{k}]\\ =& [w_a{w}_b-\text{a}\cdot \text{b},w_a\text{b}+w_b\text{a}+\text{a}\times \text{b}]\end{array}$$
计算过程看上去很多，其实十分简单，我们有了最后化简的结果，四元数的魔法就变得清晰了起来。
$$\begin{array}{rl}{q}_a{q}_b=& [w_a,\text{a}][w_b,\text{b}]=[w_a{w}_b-\text{a}\cdot \text{b},w_a\text{b}+w_b\text{a}+\text{a}\times \text{b}]\end{array}$$

2.3.2 单位四元数和向量相乘会旋转吗

我们设有一个单位四元数$q_{unit}(x_u,y_u,z_u,w_u)$ 可记为 $[\cos \theta ,\sin \theta \text{n}]$。设有一个向量 $p(x_a,y_a,z_a)$，我们将其看作一个纯四元数$q_p(x_a,y_a,z_a,0)$ ，那么四元数和向量相乘就变成了两个四元数相乘。
$$\begin{array}{rl}{p}^{{}^{\prime }}=q_{unit}p=& [\cos \theta ,\sin \theta \text{n}][0,\text{p}]\\ =& [-\sin \theta \text{n}\cdot \text{p},\cos \theta \text{p}+\sin \theta \text{n}\times \text{p}]\end{array}$$
emmmm，这tm是啥？好像直接品不出啥几何关系。

仔细想一想，我们的预期是通过四元数乘向量实现某种变换，得到一个新的向量 $p^{{}^{\prime }}(x_b,y_b,z_b)$ ，或者说一个新的纯四元数$(x_b,y_b,z_b,0)$。那么，就需要变换的结果实部为0，即$-\sin \theta \text{n}\cdot \text{p}=0$ 。

假设$\sin \theta =0$ 没有推导下去的意义，好像只有当向量 $\text{n}$ 和向量 $\text{p}$ 垂直时，这个结果有点意思：
$$[-\sin \theta \text{n}\cdot \text{p},\cos \theta \text{p}+\sin \theta \text{n}\times \text{p}]=[0,\cos \theta \text{p}+\sin \theta \text{m}]$$
其中 $\text{m}=\text{n}\times \text{p}$ , 由于$\text{n}$是单位向量，向量$\text{m}$的模长和向量$\text{p}$的模长一致，且他们三个向量互相垂直。

这个结果可以说是十分清晰了：

注意，我们在左手坐标系下谈论此问题，不要弄错$\text{n}\times \text{p}$的方向。

当向量$\text{n}$ 和向量$\text{p}$ 垂直时，单位四元数 $[\cos \theta ,\sin \theta \text{n}]$乘向量$\text{p}$时，会使$\text{p}$在左手坐标系下以向量$\text{n}$ 为旋转轴，绕轴顺时针旋转 $\theta$ 个角度！！！（左手坐标系下绕轴顺时针是正方向，上右图是顺着n方向看才会这样，可以自己用左手大拇指顺着n方向握住，看看手指的旋向。如果是右手坐标系，此处应解释为逆时针旋转$\theta$ ）

等等，如果我们不假设向量$\text{n}$ 和向量$\text{p}$ 垂直，忽略结果的实部的影响，直接看虚部$\cos \theta \text{p}+\sin \theta \text{n}\times \text{p}$ 实际上也是在绕某个轴旋转$\theta$角度，只不过旋转后的模长不一定和原本保持一致。

唉，这个旋转不完美，如果有某种方式，能够自己干掉$-\sin \theta \text{n}\cdot \text{p}$ 就好了。

2.3.3 四元数的模、共轭与逆

在继续前进之前，我们插播一下三个概念~

我们定义一个四元数 $q(x,y,z,w)$，记为 $[w,\text{v}]$。

• 模长：我们称 $q$ 的模长为$|q|=\sqrt{w^2+|\text{v}{|}^2}$ 。

• 共轭：我们称 $q^{\ast }(-x,-y,-z,w)$ 为$q$ 的共轭，记为$[w,-\text{v}]$。那么就有：
  $$\begin{array}{rl}{q}^{\ast }q=& [w,-\text{v}][w,\text{v}]\\ =& [w^2-\text{v}\cdot (-\text{v}),w\text{v}-w\text{v}-\text{v}\times \text{v}]\\ =& [w^2+{\text{v}}^2,\text{0}]\\ =& |q{|}^2\end{array}$$

• 逆：当 $q^{-1}q=1$ 时，称四元数 $q^{-1}$ 为 $q$ 的逆。那么就有：
  $$\begin{array}{rl}{q}^{\ast }q{q}^{-1}=& q^{\ast }\\ |q{|}^2{q}^{-1}=& q^{\ast }\\ q^{-1}=& \frac{q^{\ast }}{|q{|}^2}\end{array}$$
  如果 $q$ 是一个单位四元数，那么就有：
  $$q_u^{-1}=q_u^{\ast }$$

2.3.4 真·四元数旋转

我们设有一个单位四元数$q_u(x_u,y_u,z_u,w_u)$ 可记为 $[\cos \theta ,\sin \theta \text{n}]$。那么 $q_u^{-1}$ 等于$q_u^{\ast }$可记为$[\cos \theta ,-\sin \theta \text{n}]$。设有一个向量 $p(x_a,y_a,z_a)$，我们将其看作一个纯四元数$p(x_a,y_a,z_a,0)$ 。

我们让 $q_{u}p$ 乘上 $q_u^{-1}$ 。
$$\begin{array}{rl}{q}_{u}p{q}_u^{-1}=& [\cos \theta ,\sin \theta \text{n}][0,\text{p}][\cos \theta ,-\sin \theta \text{n}]\\ =& [-\sin \theta \text{n}\cdot \text{p},\cos \theta \text{p}+\sin \theta \text{n}\times \text{p}][\cos \theta ,-\sin \theta \text{n}]\\ =& [-\sin \theta \cos \theta \text{n}\cdot \text{p}+\sin \theta \cos \theta \text{n}\cdot \text{p}+\sin \theta \sin \theta (\text{n}\times \text{p})\cdot \text{n},\\ & \sin \theta \sin \theta (\text{n}\cdot \text{p})\text{n}+\cos \theta \cos \theta \text{p}+\sin \theta \cos \theta \text{n}\times \text{p}\\ & -\sin \theta \cos \theta \text{p}\times \text{n}-\sin \theta \sin \theta (\text{n}\times \text{p})\times \text{n}]\end{array}$$
这里注意：
$$(\text{n}\times \text{p})\cdot \text{n}=0$$
上面的结果还涉及到向量三重积 ：
$$\begin{gathered} (\text{n}\times \text{p})\times \text{n}=\text{n}\times (\text{p}\times \text{n})-\text{p}\times (\text{n}\times \text{n})=\text{n}\times (\text{p}\times \text{n}) \\ \text{n}\times (\text{p}\times \text{n})=\text{p}(\text{n}\cdot \text{n})-\text{n}(\text{n}\cdot \text{p}) \end{gathered}$$
所以，继续化简：
$$\begin{array}{rl}{q}_{u}p{q}_u^{-1}=& [0,2\sin \theta \sin \theta (\text{n}\cdot \text{p})\text{n}+\cos \theta \cos \theta \text{p}+2\sin \theta \cos \theta \text{n}\times \text{p}\\ & -\sin \theta \sin \theta \text{p}(\text{n}\cdot \text{n})]\\ =& [0,2{\sin }^2\theta (\text{n}\cdot \text{p})\text{n}\\ & +{\cos }^2\theta \text{p}-{\sin }^2\theta \text{p}\\ & +2\sin \theta \cos \theta \text{n}\times \text{p}]\\ =& [0,(1-\cos 2\theta )(\text{n}\cdot \text{p})\text{n}+\cos 2\theta \text{p}+\sin 2\theta \text{n}\times \text{p}]\\ =& [0,(\text{n}\cdot \text{p})\text{n}+\cos 2\theta (\text{p}-(\text{n}\cdot \text{p})\text{n})+\sin 2\theta \text{n}\times \text{p}]\end{array}$$
我们令$\alpha =2\theta$。
$$q_{u}p{q}_u^{-1}=[0,(\text{n}\cdot \text{p})\text{n}+\cos \alpha (\text{p}-(\text{n}\cdot \text{p})\text{n})+\sin \alpha \text{n}\times \text{p}]$$
好家伙，这tm又是啥啊。没办法我们硬着头皮画图一波。

图中：
$$\begin{array}{rl}AB=& (\text{n}\cdot \text{p})\text{n}\\ BD=& \text{n}\times \text{p}\\ BC=& \text{p}-(\text{n}\cdot \text{p})\text{n}\\ BE=& \cos \alpha (\text{p}-(\text{n}\cdot \text{p})\text{n})+\sin \alpha \text{n}\times \text{p}\\ AE=& AB+BE\\ =& (\text{n}\cdot \text{p})\text{n}+\cos \alpha (\text{p}-(\text{n}\cdot \text{p})\text{n})+\sin \alpha \text{n}\times \text{p}\end{array}$$
芜湖！单位四元数$q_u=[\cos \theta ,\sin \theta \text{n}]$ 通过运算 $q_{u}p{q}_u^{-1}$ 让 向量$p$ ，在左手坐标系下绕 $\text{n}$轴顺时针旋转了$2\theta$ ！！！（如果是右手坐标系，应解释为逆时针旋转$2\theta$ ）

为了让旋转角刚好为 $\theta$ ，我们的单位四元数应该是 $q_u=[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\text{n}]$ 。

到此为止我们弄清了四元数让向量旋转的运算原理。

2.3.5 我们跳过的东西

上述内容，基本处于从结论反推的状态。我们忽略了虚数在旋转中发挥的作用等更加底层的原理 。如果你想知道这些原理，你可以在《Quaternions for Computer Graphics》一书中找到比本文更加详细的内容（凭大家强大的能力一定能找到它的pdf）。

此外，我们这里对以上的内容进行一个补充：

我们定义一个单位四元数$q_u(x_u,y_u,z_u,w_u)$ 可记为 $[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\text{n}]$。设有一个向量 $p(x_a,y_a,z_a)$，我们将其看作一个纯四元数$p(x_a,y_a,z_a,0)$ 。那么：

• $q_u^{-1}p{q}_u$ ：绕$\text{n}$轴旋转$-\theta$。

当 $\text{n}\perp \text{p}$ 时，则有：

• $p{q}_u$ ：绕$\text{n}$轴旋转$-\frac{\theta }{2}$。
• $q_u^{-1}p$ ：绕$\text{n}$轴旋转$-\frac{\theta }{2}$。
• $p{q}_u^{-1}$：绕$\text{n}$轴旋转$\frac{\theta }{2}$。

所以，从上面来看$q_{u}p{q}_u^{-1}$ 可拆为两个部分 $q_{u}p$ 和 $p{q}_u^{-1}$ ，分别承当旋转$\frac{\theta }{2}$的任务，合起来一共旋转$\theta$。

除上面之外，我们还应该关注一下$-q_u$ 对旋转影响：
$$\begin{array}{rl}-q_u=& [-\cos \frac{\theta }{2},-\sin \frac{\theta }{2}\text{n}]\\ =& [\cos (\frac{\theta }{2}+\pi ),\sin (\frac{\theta }{2}+\pi )\text{n}]\\ =& [\cos (\frac{\theta +2\pi }{2}),\sin (\frac{\theta +2\pi }{2})\text{n}]\end{array}$$
可以看出$-q_u$ 绕轴旋转了$\theta +2\pi$ ，仅仅从结果上看，$-q_u$ 和 $q_u$ 的旋转效果应该是一样的。

3 啊转转转

3.1 旋转矩阵的具体写法

在1.3.4节中我们对四元数运算的原理进行了推导，在推导过程其实也是旋转矩阵的推导内容的一部分。我们回顾一下条件：我们设有一个向量为 $p(x_p,y_p,z_p)$，并有一个单位四元数$q_u(x_u,y_u,z_u,w_u)$ 可记为 $[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\text{n}]$ ，对其进行旋转。它的旋转效果等效为旋转矩阵$M$。

然后，我们直接从以下这个等式开始：
$$Mp=q_{u}p{q}_u^{-1}=[0,(\text{n}\cdot \text{p})\text{n}+\cos \theta (\text{p}-(\text{n}\cdot \text{p})\text{n})+\sin \theta \text{n}\times \text{p}]$$
我们设旋转轴$\text{n}$ 为$(x_n,y_n,z_n)$ ，对上式各个部分进行矩阵化。

首先第一部分：
$$\begin{array}{rl}(\text{n}\cdot \text{p})\text{n}=& (x_n{x}_p+y_n{y}_p+x_n{x}_p)\text{n}\\ =& \left[\begin{array}{ccc}{x}_n^2& x_n{y}_n& x_n{z}_n\\ x_n{y}_n& y_n^2& y_n{z}_n\\ x_n{z}_n& y_n{z}_n& z_n^2\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]\end{array}$$

第二部分：
$$\begin{array}{rl}\cos \theta (\text{p}-(\text{n}\cdot \text{p})\text{n})=& \cos \theta \left[\begin{array}{ccc}1-x_n^2& -x_n{y}_n& -x_n{z}_n\\ -x_n{y}_n& 1-y_n^2& -y_n{z}_n\\ -x_n{z}_n& -y_n{z}_n& 1-z_n^2\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]\end{array}$$
最后一部分：
$$\begin{array}{rl}\sin \theta \text{n}\times \text{p}=& \sin \theta (y_n{z}_p-z_n{y}_p,z_n{x}_p-x_n{z}_p,x_n{y}_p-y_n{x}_p)\\ =& \sin \theta \left[\begin{array}{ccc}0& -z_n& y_n\\ z_n& 0& -x_n\\ -y_n& x_n& 0\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]\end{array}$$
把它们合起来：
$$\begin{array}{rl}Mp=& q_{u}p{q}_u^{-1}\\ =& \left[\begin{array}{ccc}\cos \theta +(1-\cos \theta )x_n^2& (1-\cos \theta )x_n{y}_n-z_n\sin \theta & (1-\cos \theta )x_n{z}_n+y_n\sin \theta \\ (1-\cos \theta )x_n{y}_n+z_n\sin \theta & \cos \theta +(1-\cos \theta )y_n^2& (1-\cos \theta )y_n{z}_n-x_n\sin \theta \\ (1-\cos \theta )x_n{z}_n-y_n\sin \theta & (1-\cos \theta )y_n{z}_n+x_n\sin \theta & \cos \theta +(1-\cos \theta )z_n^2\end{array}\right]\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]\end{array}$$
我们获得了旋转矩阵的具体写法，我们只要知道旋转轴和旋转角度就可以构建出对应的旋转矩阵。上面的 $\theta$ 在左手坐标系下顺时针为正方向，在右手坐标系下逆时针为正方向。

如果你想在左手坐标系下，仍然取逆时针为正方向，那么公式中$\sin \theta$ 应该替换为$-\sin \theta$ ；如果你想在右手坐标系下，取顺时针为正方向，同理公式中的$\sin \theta$ 应该替换为$-\sin \theta$ 。

3.2 四元数转旋转矩阵

我们接着上一节，继续推导。一个单位四元数$q_u(x_u,y_u,z_u,w_u)$ 可记为 $[\cos \frac{\theta }{2},\sin \frac{\theta }{2}\text{n}]$ ，且旋转轴$\text{n}$ 为$(x_n,y_n,z_n)$。那么它们的关系为：
$$\begin{gathered} x_u=x_n\sin \frac{\theta }{2}{y}_u=y_n\sin \frac{\theta }{2} \\ {z}_u=z_n\sin \frac{\theta }{2}{w}_u=\cos \frac{\theta }{2} \end{gathered}$$
我们利用和角公式就有：
$$\begin{gathered} \cos \theta =2w_u^2-1 \\ \sin \theta =2w_u\sin \frac{\theta }{2} \\ 1-\cos \theta =2{\sin }^2\frac{\theta }{2} \end{gathered}$$
然后我们对旋转矩阵的结果进行一个替换：
$$\begin{array}{rl}M=& \left[\begin{array}{ccc}2w_u^2-1+2x_n^2{\sin }^2\frac{\theta }{2}& 2x_n{y}_n{\sin }^2\frac{\theta }{2}-2z_n{w}_u\sin \frac{\theta }{2}& 2x_n{z}_n{\sin }^2\frac{\theta }{2}+2y_n{w}_u\sin \frac{\theta }{2}\\ 2x_n{y}_n{\sin }^2\frac{\theta }{2}+2z_n{w}_u\sin \frac{\theta }{2}& 2w_u^2-1+2y_n^2{\sin }^2\frac{\theta }{2}& 2y_n{z}_n{\sin }^2\frac{\theta }{2}-2x_n{w}_u\sin \frac{\theta }{2}\\ 2x_n{z}_n{\sin }^2\frac{\theta }{2}-2y_n{w}_u\sin \frac{\theta }{2}& 2y_n{z}_n{\sin }^2\frac{\theta }{2}+2x_n{w}_u\sin \frac{\theta }{2}& 2w_u^2-1+2z_n^2{\sin }^2\frac{\theta }{2}\end{array}\right]\\ & \\ =& \left[\begin{array}{ccc}2(w_u^2+x_u^2)-1& 2(x_u{y}_u-z_u{w}_u)& 2(x_u{z}_u+y_u{w}_u)\\ 2(x_u{y}_u+z_u{w}_u)& 2(w_u^2+y_u^2)-1& 2(y_u{z}_u-x_u{w}_u)\\ 2(x_u{z}_u-y_u{w}_u)& 2(y_u{z}_u+x_u{w}_u)& 2(w_u^2+z_u^2)-1\end{array}\right]\end{array}$$
由于是单位四元数，旋转矩阵也可以写为：
$$\begin{array}{rl}M=& \left[\begin{array}{ccc}1-2(y_u^2+z_u^2)& 2(x_u{y}_u-z_u{w}_u)& 2(x_u{z}_u+y_u{w}_u)\\ 2(x_u{y}_u+z_u{w}_u)& 1-2(x_u^2+z_u^2)& 2(y_u{z}_u-x_u{w}_u)\\ 2(x_u{z}_u-y_u{w}_u)& 2(y_u{z}_u+x_u{w}_u)& 1-2(x_u^2+y_u^2)\end{array}\right]\end{array}$$
通过一些替换，现在旋转矩阵和四元数的关系已经对应了起来。

在左手坐标系下，如果你需要以逆时针为正方向，那么公式中的$x_u$、$y_u$、$z_u$ 应该替换为$-x_u$、$-y_u$、$-z_u$ 。

在右手坐标系下，如果你需要以顺时针为正方向，那么公式中的$x_u$、$y_u$、$z_u$ 也应该替换为$-x_u$、$-y_u$、$-z_u$ 。

同时注意：以上的推导在四元数和旋转矩阵均位于同一个左手（或右手）坐标系的情况下进行。

3.2 旋转矩阵转四元数

我看到有大佬写了优化这个算法的文章 ：Converting a Rotation Matrix to a Quaternion

我觉很棒，强烈建议看一遍！不过需要注意的文章中的向量是行向量，涉及到的矩阵存在转置。

以下我将依据链接文章，大致翻译一下并修改为列向量的版本：

根据我们的旋转矩阵(第二个版本)，我们可以得出以下四种关系式：
$$\begin{gathered} 1+m_{00}-m_{11}-m_{22}=4x^2 \\ {m}_{10}+m_{01}=4xy \\ {m}_{20}+m_{02}=4xz \\ {m}_{21}-m_{12}=4xw \end{gathered}$$
由此关系我们可以解出$x$，就可以根据其他等式解出其他值。

这样做就有一个问题，如果x是0咋办？如果是0，上面四个等式的值都会是0，造成一个没有意义的结果。即使这些表达式的值不是0，但是很接近0，那么它们可能会受到灾难性抵消（catastrophic cancellation），从而产生一个不稳定的结果。（catastrophic cancellation是指当减去两个大小大致相同的测量值时发生的错误尖峰。这种效应通常被称为重要性的损失或减法抵消，不管测量多么精确，这种效应很容易使实验结果变得毫无价值。）

我们可以通过以下观察来解决这个问题。关于单位四元数，我们可以肯定的一件事是，总会存在至少一个不接近0的分量，而且一定会存在一个大小至少为1/2的分量。这些上述用到的表达式都被4x缩放过了，但是用其他类似的处理方式（指利用矩阵M以类似的方式产生表达式，例如：1 + m₀₀+ m₁₁+ m₂₂= 4w² ），也会产生缩放因子4y、4z和4w。如果我们准备以这四种缩放因子的其中一种入手，我们总是能够选出一个数据比较稳定的方向。

以前的代码

在网上你可以找到一段相当标准的代码，也许有一些细微的不同，可以将写成如下（已改为列向量）：

trace = m00 + m11 + m22;
if (trace > 0.0f)
{
    
    
    k = 0.5f / sqrt(1.0f + trace);
    q = quat( k * (m21 - m12), k * (m02 - m20), k * (m10 - m01), 0.25f / k );
}
else if ((m00 > m11) && (m00 > m22))
{
    
    
    k = 0.5f / sqrt(1.0f + m00 - m11 - m22);
    q = quat( 0.25f / k, k * (m10 + m01), k * (m20 + m02), k * (m21 - m12) );
}
else if (m11 > m22)
{
    
    
    k = 0.5f / sqrt(1.0f + m11 - m00 - m22);
    q = quat( k * (m10 + m01), 0.25f / k, k * (m21 + m12), k * (m02 - m20) );
}
else
{
    
    
    k = 0.5f / sqrt(1.0f + m22 - m00 - m11);
    q = quat( k * (m20 + m02), k * (m21 + m12), 0.25f / k, k * (m10 - m01) );
}

这里有不太理想的几点：

1. 它最开始计算了m₀₀+m₁₁+m₂₂的和，但在四种情况中的三种里面都没有用到。
2. 对于四个分量有着不对称的处理方式：第一种方式是在$|w|>1/2$的 情况下进行，但如果不是，它将选取$|x|$，$|y|$，$|z|$中最大值的那条分支。
3. 总是对一个分量执行除法。

新的处理方式

我们来看看如何处理这些不如意的地方。首先，我们要问：什么时候每个对角元素都是负的？

因为q是一个单位四元数，有$x^2+y^2+z^2+w^2=1$的性质。所以，举个栗子，当x和y一起贡献了超过四元数一半的模长时，m₂₂就是一个负数，这说明x或y足够大，可以避免数值上的不稳定。我们只需要测试哪个可以用（也有可能两个都可以，但我们需要一个保证）。

现在我们考虑对角元素的两两之差：
以及两两之和：
因此，对于任意两个分量的选择，我们可以将一个对角元素与另外一个对角元素（或另外一个的负数）进行比较，来选择分量更大的那一个。例如：

修改框架

总之，通过以上这些观察结果，这里建议使用分而治之的策略来选择四个代码路径中的一条。此外，我们可以只通过比较对角元素来进行选择。这里是其中一种实现方式：

if (m22 < 0) // is |(x,y)| bigger than |(z,w)|?
{
    
    
    if (m00 > m11) // is |x| bigger than |y|?
        // use x-form
    else
        // use y-form
}
else
{
    
    
    if (m00 < -m11) // is |z| bigger than |w|?
        // use z-form
    else
        // use w-form
}

这个框架解决了代码中缺少对称性的问题。而且，由于我们不再需要预先计算矩阵的迹，所以我们也不会丢弃已经计算过的值——迹只能被四条分支的其中一个中得到计算（w-form分支）。在其他三个分支中，将计算它们自己需要的表达式。

最后，关于除法问题可按如下方式解决。原本的代码会在每条分支中进行计算：

k = 0.5f / sqrt(t);

t是一些项的和，然后让四元数的一个分量等于$0.25/k$。（这是我们关注的第二个除法；第一个是平方根的倒数运算，通常不使用除法直接执行。）但是请注意：

因此，除法可被代替为乘法。编译器可能会执行这种优化，但我们必须自己确保它执行。

船新版本（已改为列向量）

if (m22 < 0)
{
    
    
    if (m00 > m11)
    {
    
    
        t = 1 + m00 - m11 - m22;
        q = quat( t, m01+m10, m20+m02, m21-m12 );
    }
    else
    {
    
    
        t = 1 - m00 + m11 - m22;
        q = quat( m01+m10, t, m12+m21, m02-m20 );
    }
}
else
{
    
    
    if (m00 < -m11)
    {
    
    
        t = 1 - m00 - m11 + m22;
        q = quat( m20+m02, m12+m21, t, m10-m01 );
    }
    else
    {
    
    
        t = 1 + m00 + m11 + m22;
        q = quat( m21-m12, m02-m20, m10-m01, t );
    }
}
q *= 0.5 / Sqrt(t);

3.3 欧拉角转四元数

由于欧拉角带有旋转先后顺序，我们这里随便来个旋转顺序YXZ，并仍在在左手坐标系下推导。

我们设有一个欧拉角为$(\alpha ,\beta ,\gamma )$ ，那么我们就可以构造三个分别绕轴旋转的单位四元数 $q_x=[\cos \frac{\alpha }{2},\sin \frac{\alpha }{2}\text{i}]$、$q_y=[\cos \frac{\beta }{2},\sin \frac{\beta }{2}\text{j}]$ 和 $q_z=[\cos \frac{\gamma }{2},\sin \frac{\gamma }{2}\text{k}]$ 。

那么对应的四元数为：
$$\begin{array}{rl}{q}_{yxz}=& q_z{q}_x{q}_y\\ =& [\cos \frac{\gamma }{2},\sin \frac{\gamma }{2}\text{k}][\cos \frac{\alpha }{2},\sin \frac{\alpha }{2}\text{i}][\cos \frac{\beta }{2},\sin \frac{\beta }{2}\text{j}]\\ =& [\cos \frac{\gamma }{2}\cos \frac{\alpha }{2},\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\text{i}+\cos \frac{\alpha }{2}\sin \frac{\gamma }{2}\text{k}+\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\text{j}][\cos \frac{\beta }{2},\sin \frac{\beta }{2}\text{j}]\\ =& [\cos \frac{\gamma }{2}\cos \frac{\alpha }{2}\cos \frac{\beta }{2}-\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\sin \frac{\beta }{2},\\ & (\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\cos \frac{\beta }{2}-\sin \frac{\gamma }{2}\cos \frac{\alpha }{2}\sin \frac{\beta }{2})\text{i}\\ & (\cos \frac{\gamma }{2}\cos \frac{\alpha }{2}\sin \frac{\beta }{2}+\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\cos \frac{\beta }{2})\text{j}\\ & (\sin \frac{\gamma }{2}\cos \frac{\alpha }{2}\cos \frac{\beta }{2}+\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\sin \frac{\beta }{2})\text{k}]\end{array}$$
换种写法：
$$q_{yxz}=\left[\begin{array}{c}\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\cos \frac{\beta }{2}-\sin \frac{\gamma }{2}\cos \frac{\alpha }{2}\sin \frac{\beta }{2}\\ \cos \frac{\gamma }{2}\cos \frac{\alpha }{2}\sin \frac{\beta }{2}+\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\cos \frac{\beta }{2}\\ \sin \frac{\gamma }{2}\cos \frac{\alpha }{2}\cos \frac{\beta }{2}+\cos \frac{\gamma }{2}\sin \frac{\alpha }{2}\sin \frac{\beta }{2}\\ \cos \frac{\gamma }{2}\cos \frac{\alpha }{2}\cos \frac{\beta }{2}-\sin \frac{\gamma }{2}\sin \frac{\alpha }{2}\sin \frac{\beta }{2}\end{array}\right]$$

3.4 四元数转欧拉角

累了累了，这玩意换算算着有点难写啊。

我们直接来看看别人咋弄的吧~ 下面的文章里面各种旋转顺序的欧拉角都有的哦~
不过需要注意人家是在右手坐标系下推导，且实部是放在四元数的第一位的哦~

Euler angles, quaternions, and transformation matrices for space shuttle analysis

（水平有限，有缘再补充吧~）

---

参考文档：

[1] Quaternions for Computer Graphics

[2] Converting a Rotation Matrix to a Quaternion

[3] Euler angles, quaternions, and transformation matrices for space shuttle analysis