四元数（Quaternion）食用指南

“这简直就是黑魔法！”

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

阅读注意：

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

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

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

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

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

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

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

2 令人头疼的旋转

2.1 旋转矩阵

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

我们绕原点旋转一个点 p(a,b)p(a,b)p(a,b) 到 p′(c,d)p^{'}(c,d)p′(c,d) ，我们设点 ppp 到原点的距离为 lll。那么就有：
a=lcos⁡αb=lsin⁡αc=lcos⁡(α+θ)=lcos⁡αcos⁡θ−lsin⁡αsin⁡θ=acos⁡θ−bsin⁡θd=lsin⁡(α+θ)=lsin⁡αcos⁡θ+lcos⁡αsin⁡θ=asin⁡θ+bcos⁡θ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 a=lcosαb=lsinαc=lcos(α+θ)=lcosαcosθ−lsinαsinθ=acosθ−bsinθd=lsin(α+θ)=lsinαcosθ+lcosαsinθ=asinθ+bcosθ
我们可以很快地写出一个二维的旋转矩阵M2M_2M2，他能让点绕原点逆时针旋转θ角度（注意：这里采用矩阵乘列向量的方式），如下所示：
M2=(cos⁡θ−sin⁡θsin⁡θcos⁡θ)M_2 = \begin{pmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\\ \end{pmatrix} M2=(cosθsinθ−sinθcosθ)
事实上，三维旋转矩阵也能以类似的几何方式计算出矩阵内各个元素的值，我们将会在四元数部分一起进行推导。我们将上面的二维旋转矩阵扩展到三维，可以得出绕 xxx 轴、yyy 轴、zzz 轴的旋转矩阵RxR_xRx、RyR_yRy 和 RzR_zRz。

Rx=(1000cos⁡θsin⁡θ0−sin⁡θcos⁡θ)Ry=(cos⁡θ0−sin⁡θ010sin⁡θ0cos⁡θ)Rz=(cos⁡θsin⁡θ0−sin⁡θcos⁡θ0001)R_x=\begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta & \cos\theta \\ \end{pmatrix}\quad R_y=\begin{pmatrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta\\ \end{pmatrix} \quad R_z=\begin{pmatrix} \cos\theta & \sin\theta & 0\\ -\sin\theta & \cos\theta & 0\\ 0 & 0 & 1\\ \end{pmatrix} \quad Rx=⎝⎛1000cosθ−sinθ0sinθcosθ⎠⎞Ry=⎝⎛cosθ0sinθ010−sinθ0cosθ⎠⎞Rz=⎝⎛cosθ−sinθ0sinθcosθ0001⎠⎞
上述推导在左手坐标系中进行，在扩展到三维的时候，要注意哪个轴是竖轴、哪个轴是横轴，按顺序来就好。

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

2.2 欧拉角

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

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

看上去有点类似绕xyz轴的旋转矩阵，如果我们把这个矩阵乘以一个列向量 P(x,y,z)P(x,y,z)P(x,y,z)
P′=RzRy90RxP=[ysin⁡(γ+α)−zcos⁡(γ+α)ycos⁡(γ+α)+zsin⁡(γ+α)x]P^{'}=R_zR_{y90}R_xP = \begin{bmatrix} y\sin(\gamma + \alpha)-z\cos(\gamma + \alpha)\\ y\cos(\gamma + \alpha)+z\sin(\gamma + \alpha)\\ x\\ \end{bmatrix} P′=RzRy90RxP=⎣⎡ysin(γ+α)−zcos(γ+α)ycos(γ+α)+zsin(γ+α)x⎦⎤
这里可以看到无论怎么调整 γ\gammaγ 和 α\alphaα ，旋转后的点只会在XY平面转动，第一次旋转和最后一次旋转等价。

2.3 四元数

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

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

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

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

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

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

2.3.1 四元数相乘会发生什么

那么我们定义两个四元数 qa(xa,ya,za,wa)q_a(x_a,y_a,z_a,w_a)qa(xa,ya,za,wa) 和 qb(xb,yb,zb,wb)q_b(x_b,y_b,z_b,w_b)qb(xb,yb,zb,wb) ，可分别记为[wa,a][w_a,\textbf{a}][wa,a] 和 [wb,b][w_b,\textbf{b}][wb,b]，其中：
a=xai+yaj+zakb=xbi+ybj+zbk\textbf{a} = x_ai + y_aj + z_ak\\ \textbf{b} = x_bi + y_bj + z_bk a=xai+yaj+zakb=xbi+ybj+zbk
接下来，看看两个四元数相乘会发生什么。
qaqb=[wa,a][wb,b]=(wa+xai+yaj+zak)(wb+xbi+ybj+zbk)=(wawb−xaxb−yayb−zazb)+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k]\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}]\\ =& (w_a + x_ai + y_aj + z_ak)(w_b + x_bi + y_bj + z_bk)\\ =& (w_aw_b - x_ax_b - y_ay_b - z_az_b) \\ &+ (w_ax_b + w_bx_a + y_az_b-y_bz_a )[0,\textbf{i}] \\ &+ (w_ay_b + w_by_a + z_ax_b-z_bx_a )[0,\textbf{j}] \\ &+ (w_az_b + w_bz_a + x_ay_b-x_by_a )[0,\textbf{k}] \\ \end{aligned} qaqb===[wa,a][wb,b](wa+xai+yaj+zak)(wb+xbi+ybj+zbk)(wawb−xaxb−yayb−zazb)+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k]
换算过程中注意通过 ijk=−1ijk=-1ijk=−1 可以推出 ij=kij = kij=k 、ji=−kji = -kji=−k 等等式。注意，我们正在尝试通过q=[w,a]q = [w,\textbf{a}]q=[w,a]这种写法在计算过程消除虚根的影响。
qaqb=[wa,a][wb,b]=(wawb−xaxb−yayb−zazb)[1,0]+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k]=[wawb−xaxb−yayb−zazb,0]+[0,(waxb+wbxa+yazb−ybza)i]+[0,(wayb+wbya+zaxb−zbxa)j]+[0,(wazb+wbza+xayb−xbya)k]=[wawb−xaxb−yayb−zazb,wa(xai+yaj+zak)+wb(xbi+ybj+zbk)+(yazb−ybza)i+(zaxb−zbxa)j+(xayb−xbya)k]=[wawb−a⋅b,wab+wba+a×b]\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}]\\ =& (w_aw_b - x_ax_b - y_ay_b - z_az_b)[1,\textbf{0}] \\ &+ (w_ax_b + w_bx_a + y_az_b-y_bz_a )[0,\textbf{i}] \\ &+ (w_ay_b + w_by_a + z_ax_b-z_bx_a )[0,\textbf{j}] \\ &+ (w_az_b + w_bz_a + x_ay_b-x_by_a )[0,\textbf{k}] \\ =&[w_aw_b - x_ax_b - y_ay_b - z_az_b,\textbf{0}] \\ &+[0,(w_ax_b + w_bx_a + y_az_b-y_bz_a)\textbf{i}]\\ &+[0,(w_ay_b + w_by_a + z_ax_b-z_bx_a )\textbf{j}] \\ &+[0,(w_az_b + w_bz_a + x_ay_b-x_by_a )\textbf{k}] \\ =&[w_aw_b - x_ax_b - y_ay_b - z_az_b,\\ &w_a(x_a\textbf{i} + y_a\textbf{j} + z_a\textbf{k}) + w_b(x_b\textbf{i} + y_b\textbf{j} + z_b\textbf{k})\\ &+ (y_az_b-y_bz_a)\textbf{i} + (z_ax_b - z_bx_a)\textbf{j} + (x_ay_b - x_by_a)\textbf{k}]\\ =&[w_aw_b-\textbf{a}\cdot\textbf{b},w_a\textbf{b}+w_b\textbf{a} + \textbf{a}\times\textbf{b}] \end{aligned} qaqb=====[wa,a][wb,b](wawb−xaxb−yayb−zazb)[1,0]+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k][wawb−xaxb−yayb−zazb,0]+[0,(waxb+wbxa+yazb−ybza)i]+[0,(wayb+wbya+zaxb−zbxa)j]+[0,(wazb+wbza+xayb−xbya)k][wawb−xaxb−yayb−zazb,wa(xai+yaj+zak)+wb(xbi+ybj+zbk)+(yazb−ybza)i+(zaxb−zbxa)j+(xayb−xbya)k][wawb−a⋅b,wab+wba+a×b]
计算过程看上去很多，其实十分简单，我们有了最后化简的结果，四元数的魔法就变得清晰了起来。
qaqb=[wa,a][wb,b]=[wawb−a⋅b,wab+wba+a×b]\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}] = [w_aw_b-\textbf{a}\cdot\textbf{b},w_a\textbf{b}+w_b\textbf{a} + \textbf{a}\times\textbf{b}] \end{aligned} qaqb=[wa,a][wb,b]=[wawb−a⋅b,wab+wba+a×b]

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

我们设有一个单位四元数qunit(xu,yu,zu,wu)q_{unit}(x_u,y_u,z_u,w_u)qunit(xu,yu,zu,wu) 可记为 [cos⁡θ,sin⁡θn][\cos\theta,\sin\theta\textbf{n}][cosθ,sinθn]。设有一个向量 p(xa,ya,za)p(x_a,y_a,z_a)p(xa,ya,za)，我们将其看作一个纯四元数qp(xa,ya,za,0)q_p(x_a,y_a,z_a,0)qp(xa,ya,za,0) ，那么四元数和向量相乘就变成了两个四元数相乘。
p′=qunitp=[cos⁡θ,sin⁡θn][0,p]=[−sin⁡θn⋅p,cos⁡θp+sin⁡θn×p]\begin{aligned} p^{'}= q_{unit}p =&[\cos\theta,\sin\theta\textbf{n}][0,\textbf{p}]\\ =&[-\sin\theta\textbf{n}\cdot\textbf{p},\cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p}] \end{aligned} p′=qunitp==[cosθ,sinθn][0,p][−sinθn⋅p,cosθp+sinθn×p]
emmmm，这tm是啥？好像直接品不出啥几何关系。

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

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

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

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

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

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

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

2.3.3 四元数的模、共轭与逆

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

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

模长：我们称 qqq 的模长为∣q∣=w2+∣v∣2|q|=\sqrt{w^2+|\textbf{v}|^2}∣q∣=w2+∣v∣2 。
共轭：我们称 q∗(−x,−y,−z,w)q^*(-x,-y,-z,w)q∗(−x,−y,−z,w) 为qqq 的共轭，记为[w,−v][w,-\textbf{v}][w,−v]。那么就有：
q∗q=[w,−v][w,v]=[w2−v⋅(−v),wv−wv−v×v]=[w2+v2,0]=∣q∣2\begin{aligned} q^*q=&[w,-\textbf{v}][w,\textbf{v}]\\ =&[w^2-\textbf{v}\cdot(-\textbf{v}),w\textbf{v}-w\textbf{v}-\textbf{v}\times\textbf{v}]\\ =&[w^2+\textbf{v}^2,\textbf{0}]\\ =&|q|^2 \end{aligned} q∗q====[w,−v][w,v][w2−v⋅(−v),wv−wv−v×v][w2+v2,0]∣q∣2
逆：当 q−1q=1q^{-1}q = 1q−1q=1 时，称四元数 q−1q^{-1}q−1 为 qqq 的逆。那么就有：
q∗qq−1=q∗∣q∣2q−1=q∗q−1=q∗∣q∣2\begin{aligned} q^*qq^{-1}=&q^*\\ |q|^2q^{-1}=&q^*\\ q^{-1}=&\frac{q^*}{|q|^2}\end{aligned} q∗qq−1=∣q∣2q−1=q−1=q∗q∗∣q∣2q∗
如果 qqq 是一个单位四元数，那么就有：
qu−1=qu∗q_{u}^{-1}=q_{u}^* qu−1=qu∗

2.3.4 真·四元数旋转

我们设有一个单位四元数qu(xu,yu,zu,wu)q_{u}(x_u,y_u,z_u,w_u)qu(xu,yu,zu,wu) 可记为 [cos⁡θ,sin⁡θn][\cos\theta,\sin\theta\textbf{n}][cosθ,sinθn]。那么 qu−1q_u^{-1}qu−1 等于qu∗q_u^{*}qu∗可记为[cos⁡θ,−sin⁡θn][\cos\theta,-\sin\theta\textbf{n}][cosθ,−sinθn]。设有一个向量 p(xa,ya,za)p(x_a,y_a,z_a)p(xa,ya,za)，我们将其看作一个纯四元数p(xa,ya,za,0)p(x_a,y_a,z_a,0)p(xa,ya,za,0) 。

我们让 qupq_upqup 乘上 qu−1q_u^{-1}qu−1 。
qupqu−1=[cos⁡θ,sin⁡θn][0,p][cos⁡θ,−sin⁡θn]=[−sin⁡θn⋅p,cos⁡θp+sin⁡θn×p][cos⁡θ,−sin⁡θn]=[−sin⁡θcos⁡θn⋅p+sin⁡θcos⁡θn⋅p+sin⁡θsin⁡θ(n×p)⋅n,sin⁡θsin⁡θ(n⋅p)n+cos⁡θcos⁡θp+sin⁡θcos⁡θn×p−sin⁡θcos⁡θp×n−sin⁡θsin⁡θ(n×p)×n]\begin{aligned} q_upq_u^{-1} =& [\cos\theta,\sin\theta\textbf{n}][0,\textbf{p}][\cos\theta,-\sin\theta\textbf{n}]\\ =& [-\sin\theta\textbf{n}\cdot\textbf{p},\cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p}][\cos\theta,-\sin\theta\textbf{n}]\\ =& [-\sin\theta\cos\theta\textbf{n}\cdot\textbf{p}+\sin\theta\cos\theta\textbf{n}\cdot\textbf{p}+\sin\theta\sin\theta(\textbf{n}\times\textbf{p})\cdot\textbf{n},\\ & \sin\theta\sin\theta(\textbf{n}\cdot\textbf{p})\textbf{n}+\cos\theta\cos\theta\textbf{p}+\sin\theta\cos\theta\textbf{n}\times\textbf{p}\\ &-\sin\theta\cos\theta\textbf{p}\times\textbf{n} - \sin\theta\sin\theta(\textbf{n}\times\textbf{p})\times\textbf{n}] \end{aligned} qupqu−1===[cosθ,sinθn][0,p][cosθ,−sinθn][−sinθn⋅p,cosθp+sinθn×p][cosθ,−sinθn][−sinθcosθn⋅p+sinθcosθn⋅p+sinθsinθ(n×p)⋅n,sinθsinθ(n⋅p)n+cosθcosθp+sinθcosθn×p−sinθcosθp×n−sinθsinθ(n×p)×n]
这里注意：
(n×p)⋅n=0(\textbf{n}\times\textbf{p})\cdot\textbf{n} = 0 (n×p)⋅n=0
上面的结果还涉及到向量三重积 ：
(n×p)×n=n×(p×n)−p×(n×n)=n×(p×n)n×(p×n)=p(n⋅n)−n(n⋅p)(\textbf{n}\times\textbf{p})\times\textbf{n} = \textbf{n}\times(\textbf{p}\times\textbf{n})-\textbf{p}\times(\textbf{n}\times\textbf{n}) =\textbf{n}\times(\textbf{p}\times\textbf{n})\\ \\ \textbf{n}\times(\textbf{p}\times\textbf{n})=\textbf{p}(\textbf{n}\cdot\textbf{n})-\textbf{n}(\textbf{n}\cdot\textbf{p}) (n×p)×n=n×(p×n)−p×(n×n)=n×(p×n)n×(p×n)=p(n⋅n)−n(n⋅p)
所以，继续化简：
qupqu−1=[0,2sin⁡θsin⁡θ(n⋅p)n+cos⁡θcos⁡θp+2sin⁡θcos⁡θn×p−sin⁡θsin⁡θp(n⋅n)]=[0,2sin⁡2θ(n⋅p)n+cos⁡2θp−sin⁡2θp+2sin⁡θcos⁡θn×p]=[0,(1−cos⁡2θ)(n⋅p)n+cos⁡2θp+sin⁡2θn×p]=[0,(n⋅p)n+cos⁡2θ(p−(n⋅p)n)+sin⁡2θn×p]\begin{aligned} q_upq_u^{-1} =& [0, 2\sin\theta\sin\theta(\textbf{n}\cdot\textbf{p})\textbf{n}+\cos\theta\cos\theta\textbf{p}+2\sin\theta\cos\theta\textbf{n}\times\textbf{p}\\ &- \sin\theta\sin\theta\textbf{p}(\textbf{n}\cdot\textbf{n})]\\ =& [0, 2\sin^2\theta(\textbf{n}\cdot\textbf{p})\textbf{n}\\ &+\cos^2\theta\textbf{p}- \sin^2\theta\textbf{p}\\ &+2\sin\theta\cos\theta\textbf{n}\times\textbf{p}]\\ =&[0,(1-\cos2\theta)(\textbf{n}\cdot\textbf{p})\textbf{n} + \cos2\theta\textbf{p}+\sin2\theta\textbf{n}\times\textbf{p}]\\ =&[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos2\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin2\theta\textbf{n}\times\textbf{p}] \end{aligned} qupqu−1====[0,2sinθsinθ(n⋅p)n+cosθcosθp+2sinθcosθn×p−sinθsinθp(n⋅n)][0,2sin2θ(n⋅p)n+cos2θp−sin2θp+2sinθcosθn×p][0,(1−cos2θ)(n⋅p)n+cos2θp+sin2θn×p][0,(n⋅p)n+cos2θ(p−(n⋅p)n)+sin2θn×p]
我们令α=2θ\alpha = 2\thetaα=2θ。
qupqu−1=[0,(n⋅p)n+cos⁡α(p−(n⋅p)n)+sin⁡αn×p]q_upq_u^{-1}=[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p}] qupqu−1=[0,(n⋅p)n+cosα(p−(n⋅p)n)+sinαn×p]
好家伙，这tm又是啥啊。没办法我们硬着头皮画图一波。

图中：
AB=(n⋅p)nBD=n×pBC=p−(n⋅p)nBE=cos⁡α(p−(n⋅p)n)+sin⁡αn×pAE=AB+BE=(n⋅p)n+cos⁡α(p−(n⋅p)n)+sin⁡αn×p\begin{aligned} AB=&(\textbf{n}\cdot\textbf{p})\textbf{n}\\ BD=&\textbf{n}\times\textbf{p}\\ BC=&\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n}\\ BE =&\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p}\\ AE=&AB+BE\\ =&(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p} \end{aligned} AB=BD=BC=BE=AE==(n⋅p)nn×pp−(n⋅p)ncosα(p−(n⋅p)n)+sinαn×pAB+BE(n⋅p)n+cosα(p−(n⋅p)n)+sinαn×p
芜湖！单位四元数qu=[cos⁡θ,sin⁡θn]q_u=[\cos\theta,\sin\theta\textbf{n}]qu=[cosθ,sinθn] 通过运算 qupqu−1q_upq_u^{-1}qupqu−1 让向量ppp ，在左手坐标系下绕 n\textbf{n}n轴顺时针旋转了2θ2\theta2θ ！！！（如果是右手坐标系，应解释为逆时针旋转2θ2\theta2θ ）

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

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

2.3.5 我们跳过的东西

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

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

我们定义一个单位四元数qu(xu,yu,zu,wu)q_{u}(x_u,y_u,z_u,w_u)qu(xu,yu,zu,wu) 可记为 [cos⁡θ2,sin⁡θ2n][\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}][cos2θ,sin2θn]。设有一个向量 p(xa,ya,za)p(x_a,y_a,z_a)p(xa,ya,za)，我们将其看作一个纯四元数p(xa,ya,za,0)p(x_a,y_a,z_a,0)p(xa,ya,za,0) 。那么：

qu−1pquq_u^{-1}pq_uqu−1pqu ：绕n\textbf{n}n轴旋转−θ-\theta−θ。

当 n⊥p\textbf{n}\perp \textbf{p}n⊥p 时，则有：

pqupq_upqu ：绕n\textbf{n}n轴旋转−θ2-\frac{\theta}{2}−2θ。
qu−1pq_u^{-1}pqu−1p ：绕n\textbf{n}n轴旋转−θ2-\frac{\theta}{2}−2θ。
pqu−1pq_u^{-1}pqu−1：绕n\textbf{n}n轴旋转θ2\frac{\theta}{2}2θ。

所以，从上面来看qupqu−1q_upq_u^{-1}qupqu−1 可拆为两个部分 qupq_upqup 和 pqu−1pq_u^{-1}pqu−1 ，分别承当旋转θ2\frac{\theta}{2}2θ的任务，合起来一共旋转θ\thetaθ。

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

3 啊转转转

3.1 旋转矩阵的具体写法

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

然后，我们直接从以下这个等式开始：
Mp=qupqu−1=[0,(n⋅p)n+cos⁡θ(p−(n⋅p)n)+sin⁡θn×p]Mp=q_upq_u^{-1}=[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\theta\textbf{n}\times\textbf{p}] Mp=qupqu−1=[0,(n⋅p)n+cosθ(p−(n⋅p)n)+sinθn×p]
我们设旋转轴n\textbf{n}n 为(xn,yn,zn)(x_n,y_n,z_n)(xn,yn,zn) ，对上式各个部分进行矩阵化。

首先第一部分：
(n⋅p)n=(xnxp+ynyp+xnxp)n=[xn2xnynxnznxnynyn2ynznxnznynznzn2][xpypzp]\begin{aligned} (\textbf{n}\cdot\textbf{p})\textbf{n} =&(x_nx_p+y_ny_p+x_nx_p)\textbf{n}\\ =&\begin{bmatrix} x_n^2 & x_ny_n & x_nz_n \\ x_ny_n & y_n^2 & y_nz_n \\ x_nz_n & y_nz_n & z_n^2\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned} (n⋅p)n==(xnxp+ynyp+xnxp)n⎣⎡xn2xnynxnznxnynyn2ynznxnznynznzn2⎦⎤⎣⎡xpypzp⎦⎤

第二部分：
cos⁡θ(p−(n⋅p)n)=cos⁡θ[1−xn2−xnyn−xnzn−xnyn1−yn2−ynzn−xnzn−ynzn1−zn2][xpypzp]\begin{aligned} \cos\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n}) =& \cos\theta \begin{bmatrix} 1-x_n^2 & -x_ny_n & -x_nz_n \\ -x_ny_n & 1-y_n^2 & -y_nz_n \\ -x_nz_n & -y_nz_n & 1-z_n^2\end{bmatrix} \begin{bmatrix} x_p \\y_p \\ z_p\end{bmatrix} \end{aligned} cosθ(p−(n⋅p)n)=cosθ⎣⎡1−xn2−xnyn−xnzn−xnyn1−yn2−ynzn−xnzn−ynzn1−zn2⎦⎤⎣⎡xpypzp⎦⎤
最后一部分：
sin⁡θn×p=sin⁡θ(ynzp−znyp,znxp−xnzp,xnyp−ynxp)=sin⁡θ[0−znynzn0−xn−ynxn0][xpypzp]\begin{aligned} \sin\theta\textbf{n}\times\textbf{p} =& \sin\theta(y_nz_p-z_ny_p,z_nx_p-x_nz_p,x_ny_p-y_nx_p)\\ =&\sin\theta\begin{bmatrix} 0 & -z_n & y_n \\ z_n & 0 & -x_n \\ -y_n & x_n & 0\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned} sinθn×p==sinθ(ynzp−znyp,znxp−xnzp,xnyp−ynxp)sinθ⎣⎡0zn−yn−zn0xnyn−xn0⎦⎤⎣⎡xpypzp⎦⎤
把它们合起来：
Mp=qupqu−1=[cos⁡θ+(1−cos⁡θ)xn2(1−cos⁡θ)xnyn−znsin⁡θ(1−cos⁡θ)xnzn+ynsin⁡θ(1−cos⁡θ)xnyn+znsin⁡θcos⁡θ+(1−cos⁡θ)yn2(1−cos⁡θ)ynzn−xnsin⁡θ(1−cos⁡θ)xnzn−ynsin⁡θ(1−cos⁡θ)ynzn+xnsin⁡θcos⁡θ+(1−cos⁡θ)zn2][xpypzp]\begin{aligned} M p =& q_upq_u^{-1}\\=&\begin{bmatrix} \cos\theta+(1-\cos\theta)x_n^2 & (1-\cos\theta)x_ny_n-z_n\sin\theta & (1-\cos\theta)x_nz_n+y_n\sin\theta \\ (1-\cos\theta)x_ny_n+z_n\sin\theta & \cos\theta+(1-\cos\theta)y_n^2 & (1-\cos\theta)y_nz_n-x_n\sin\theta \\ (1-\cos\theta)x_nz_n-y_n\sin\theta & (1-\cos\theta)y_nz_n + x_n\sin\theta& \cos\theta+(1-\cos\theta)z_n^2\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned} Mp==qupqu−1⎣⎡cosθ+(1−cosθ)xn2(1−cosθ)xnyn+znsinθ(1−cosθ)xnzn−ynsinθ(1−cosθ)xnyn−znsinθcosθ+(1−cosθ)yn2(1−cosθ)ynzn+xnsinθ(1−cosθ)xnzn+ynsinθ(1−cosθ)ynzn−xnsinθcosθ+(1−cosθ)zn2⎦⎤⎣⎡xpypzp⎦⎤
我们获得了旋转矩阵的具体写法，我们只要知道旋转轴和旋转角度就可以构建出对应的旋转矩阵。上面的 θ\thetaθ 在左手坐标系下顺时针为正方向，在右手坐标系下逆时针为正方向。

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

3.2 四元数转旋转矩阵

我们接着上一节，继续推导。一个单位四元数qu(xu,yu,zu,wu)q_{u}(x_u,y_u,z_u,w_u)qu(xu,yu,zu,wu) 可记为 [cos⁡θ2,sin⁡θ2n][\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}][cos2θ,sin2θn] ，且旋转轴n\textbf{n}n 为(xn,yn,zn)(x_n,y_n,z_n)(xn,yn,zn)。那么它们的关系为：
xu=xnsin⁡θ2yu=ynsin⁡θ2zu=znsin⁡θ2wu=cos⁡θ2x_u = x_n\sin\frac{\theta}{2}\quad\quad y_u = y_n\sin\frac{\theta}{2}\\ z_u = z_n\sin\frac{\theta}{2}\quad\quad w_u = \cos\frac{\theta}{2} xu=xnsin2θyu=ynsin2θzu=znsin2θwu=cos2θ
我们利用和角公式就有：
cos⁡θ=2wu2−1sin⁡θ=2wusin⁡θ21−cos⁡θ=2sin⁡2θ2\cos\theta = 2w_u^2 - 1\\ \sin\theta = 2w_u\sin\frac{\theta}{2}\\ 1-\cos\theta=2\sin^2\frac{\theta}{2} cosθ=2wu2−1sinθ=2wusin2θ1−cosθ=2sin22θ
然后我们对旋转矩阵的结果进行一个替换：
M=[2wu2−1+2xn2sin⁡2θ22xnynsin⁡2θ2−2znwusin⁡θ22xnznsin⁡2θ2+2ynwusin⁡θ22xnynsin⁡2θ2+2znwusin⁡θ22wu2−1+2yn2sin⁡2θ22ynznsin⁡2θ2−2xnwusin⁡θ22xnznsin⁡2θ2−2ynwusin⁡θ22ynznsin⁡2θ2+2xnwusin⁡θ22wu2−1+2zn2sin⁡2θ2]=[2(wu2+xu2)−12(xuyu−zuwu)2(xuzu+yuwu)2(xuyu+zuwu)2(wu2+yu2)−12(yuzu−xuwu)2(xuzu−yuwu)2(yuzu+xuwu)2(wu2+zu2)−1]\begin{aligned} M =&\begin{bmatrix} 2w_u^2 - 1+2x_n^2\sin^2\frac{\theta}{2} & 2x_ny_n\sin^2\frac{\theta}{2}-2z_nw_u\sin\frac{\theta}{2} & 2x_nz_n\sin^2\frac{\theta}{2}+2y_nw_u\sin\frac{\theta}{2} \\ 2x_ny_n\sin^2\frac{\theta}{2}+2z_nw_u\sin\frac{\theta}{2} & 2w_u^2 - 1+2y_n^2\sin^2\frac{\theta}{2} & 2y_nz_n\sin^2\frac{\theta}{2}-2x_nw_u\sin\frac{\theta}{2} \\ 2x_nz_n\sin^2\frac{\theta}{2}-2y_nw_u\sin\frac{\theta}{2} & 2y_nz_n\sin^2\frac{\theta}{2} + 2x_nw_u\sin\frac{\theta}{2}& 2w_u^2 - 1+2z_n^2\sin^2\frac{\theta}{2}\end{bmatrix} \\ \\ =&\begin{bmatrix} 2(w_u^2 + x_u^2 )- 1 & 2(x_uy_u-z_uw_u) & 2(x_uz_u+y_uw_u) \\ 2(x_uy_u+z_uw_u) & 2(w_u^2+y_u^2) - 1 & 2(y_uz_u-x_uw_u) \\ 2(x_uz_u-y_uw_u) & 2(y_uz_u + x_uw_u) & 2(w_u^2+z_u^2) - 1 \end{bmatrix} \end{aligned} M==⎣⎡2wu2−1+2xn2sin22θ2xnynsin22θ+2znwusin2θ2xnznsin22θ−2ynwusin2θ2xnynsin22θ−2znwusin2θ2wu2−1+2yn2sin22θ2ynznsin22θ+2xnwusin2θ2xnznsin22θ+2ynwusin2θ2ynznsin22θ−2xnwusin2θ2wu2−1+2zn2sin22θ⎦⎤⎣⎡2(wu2+xu2)−12(xuyu+zuwu)2(xuzu−yuwu)2(xuyu−zuwu)2(wu2+yu2)−12(yuzu+xuwu)2(xuzu+yuwu)2(yuzu−xuwu)2(wu2+zu2)−1⎦⎤
由于是单位四元数，旋转矩阵也可以写为：
M=[1−2(yu2+zu2)2(xuyu−zuwu)2(xuzu+yuwu)2(xuyu+zuwu)1−2(xu2+zu2)2(yuzu−xuwu)2(xuzu−yuwu)2(yuzu+xuwu)1−2(xu2+yu2)]\begin{aligned} M =&\begin{bmatrix} 1-2(y_u^2 + z_u^2 ) & 2(x_uy_u-z_uw_u) & 2(x_uz_u+y_uw_u) \\ 2(x_uy_u+z_uw_u) & 1-2(x_u^2+z_u^2) & 2(y_uz_u-x_uw_u) \\ 2(x_uz_u-y_uw_u) & 2(y_uz_u + x_uw_u) & 1-2(x_u^2+y_u^2) \end{bmatrix} \end{aligned} M=⎣⎡1−2(yu2+zu2)2(xuyu+zuwu)2(xuzu−yuwu)2(xuyu−zuwu)1−2(xu2+zu2)2(yuzu+xuwu)2(xuzu+yuwu)2(yuzu−xuwu)1−2(xu2+yu2)⎦⎤
通过一些替换，现在旋转矩阵和四元数的关系已经对应了起来。

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

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

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

3.2 旋转矩阵转四元数

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

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

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

根据我们的旋转矩阵(第二个版本)，我们可以得出以下四种关系式：
1+m00−m11−m22=4x2m10+m01=4xym20+m02=4xzm21−m12=4xw1+m_{00}-m_{11}-m_{22}=4x^2\\ m_{10}+m_{01}=4xy\\ m_{20}+m_{02}=4xz\\ m_{21}-m_{12}=4xw 1+m00−m11−m22=4x2m10+m01=4xym20+m02=4xzm21−m12=4xw
由此关系我们可以解出xxx，就可以根据其他等式解出其他值。

这样做就有一个问题，如果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) );
}

这里有不太理想的几点：

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

新的处理方式

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

因为q是一个单位四元数，有x2+y2+z2+w2=1x^2+y^2+z^2+w^2=1x2+y2+z2+w2=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-formelse// use y-form
}
else
{if (m00 < -m11) // is |z| bigger than |w|?// use z-formelse// use w-form
}

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

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

k = 0.5f / sqrt(t);

t是一些项的和，然后让四元数的一个分量等于0.25/k0.25/k0.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)(α,β,γ) ，那么我们就可以构造三个分别绕轴旋转的单位四元数 qx=[cos⁡α2,sin⁡α2i]q_x=[\cos\frac{\alpha}{2},\sin\frac{\alpha}{2}\textbf{i}]qx=[cos2α,sin2αi]、qy=[cos⁡β2,sin⁡β2j]q_y=[\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{j}]qy=[cos2β,sin2βj] 和 qz=[cos⁡γ2,sin⁡γ2k]q_z=[\cos\frac{\gamma}{2},\sin\frac{\gamma}{2}\textbf{k}]qz=[cos2γ,sin2γk] 。

那么对应的四元数为：
qyxz=qzqxqy=[cos⁡γ2,sin⁡γ2k][cos⁡α2,sin⁡α2i][cos⁡β2,sin⁡β2j]=[cos⁡γ2cos⁡α2,cos⁡γ2sin⁡α2i+cos⁡α2sin⁡γ2k+sin⁡γ2sin⁡α2j][cos⁡β2,sin⁡β2j]=[cos⁡γ2cos⁡α2cos⁡β2−sin⁡γ2sin⁡α2sin⁡β2,(cos⁡γ2sin⁡α2cos⁡β2−sin⁡γ2cos⁡α2sin⁡β2)i(cos⁡γ2cos⁡α2sin⁡β2+sin⁡γ2sin⁡α2cos⁡β2)j(sin⁡γ2cos⁡α2cos⁡β2+cos⁡γ2sin⁡α2sin⁡β2)k]\begin{aligned} q_{yxz}=&q_zq_xq_y\\ =&[\cos\frac{\gamma}{2},\sin\frac{\gamma}{2}\textbf{k}][\cos\frac{\alpha}{2},\sin\frac{\alpha}{2}\textbf{i}][\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{j}]\\ =&[\cos\frac{\gamma}{2}\cos\frac{\alpha}{2},\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\textbf{i} + \cos\frac{\alpha}{2}\sin\frac{\gamma}{2}\textbf{k}+\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\textbf{j}][\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{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})\textbf{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})\textbf{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})\textbf{k}]\\ \end{aligned} qyxz====qzqxqy[cos2γ,sin2γk][cos2α,sin2αi][cos2β,sin2βj][cos2γcos2α,cos2γsin2αi+cos2αsin2γk+sin2γsin2αj][cos2β,sin2βj][cos2γcos2αcos2β−sin2γsin2αsin2β,(cos2γsin2αcos2β−sin2γcos2αsin2β)i(cos2γcos2αsin2β+sin2γsin2αcos2β)j(sin2γcos2αcos2β+cos2γsin2αsin2β)k]
换种写法：
qyxz=[cos⁡γ2sin⁡α2cos⁡β2−sin⁡γ2cos⁡α2sin⁡β2cos⁡γ2cos⁡α2sin⁡β2+sin⁡γ2sin⁡α2cos⁡β2sin⁡γ2cos⁡α2cos⁡β2+cos⁡γ2sin⁡α2sin⁡β2cos⁡γ2cos⁡α2cos⁡β2−sin⁡γ2sin⁡α2sin⁡β2]q_{yxz}=\begin{bmatrix} \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{bmatrix} qyxz=⎣⎢⎢⎡cos2γsin2αcos2β−sin2γcos2αsin2βcos2γcos2αsin2β+sin2γsin2αcos2βsin2γcos2αcos2β+cos2γsin2αsin2βcos2γcos2αcos2β−sin2γsin2αsin2β⎦⎥⎥⎤

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