四元数公式推导

在看PhysX源码的时候，看到四元数公式，想知道怎么推导过来的，因为网上一大片帖子都是直接写上这个公式。最主要是纠结公式中角度的一半是如何来的

公式

参考[2] 10.4.3
$$\begin{array}{rl}q& =[cos(\theta /2),sin(\theta /2)\vec{n}]\\ & =[cos(\theta /2),sin(\theta /2)n_x,sin(\theta /2)n_y,sin(\theta /2)n_z]\end{array}$$

推导

参考[6]直接给出了证明，下面自己推导一遍

三维空间旋转公式

先抛开四元数，我们去求一个三位空间向量绕固定轴旋转之后的向量的公式是什么？也就是参考[2]书中提到的轴-角式旋转。而四元数是跟轴-角式的旋转相关的，先以这个为基础，再去推导四元数的公式会容易理解一些。
和参考[6]一样，用的右手坐标系，主要是用的绘图软件[9]，右手坐标系好看点。

设定：

• 任意向量$v\text{v}v$(向量用粗体小写字母表示)
• 绕经过原点的旋转轴$u\text{u}u={(x,y,z)}^T$
• 右手坐标系，绕向量$pmbu$从箭头到原点方向逆时针旋转角度$\theta$
• $u\text{u}u$是单位向量：$\Vert u\text{u}u\Vert =\sqrt{x^2+y^2+z^2}=1$

为了简单起见，看下图所示

1.向量分解

将$v\text{v}v$分解为平行于$u\text{u}u$的${v\text{v}v}_{\Vert }$，和垂直于$u\text{u}u$的${v\text{v}v}_{\perp }$
$$v\text{v}v={v\text{v}v}_{\Vert }+{v\text{v}v}_{\perp }$$
分别旋转这两个分向量，再把最后旋转的两个分量相加，得到旋转后的向量${v\text{v}v}^{{}^{\prime }}$
$${v\text{v}v}^{{}^{\prime }}={{v\text{v}v}_{\Vert }}^{{}^{\prime }}+{{v\text{v}v}_{\perp }}^{{}^{\prime }}$$

如下图所示：

根据正交投影公式
$$\begin{array}{rl}{v\text{v}v}_{\Vert }& =\frac{u\text{u}u.v\text{v}v}{u\text{u}u.u\text{u}u}u\text{u}u\\ & =(u\text{u}u.v\text{v}v)u\text{u}u(\Vert u\text{u}u\Vert =1)\end{array}$$

因为$v\text{v}v={v\text{v}v}_{\Vert }+{v\text{v}v}_{\perp }$，所以
$$\begin{array}{rl}{v\text{v}v}_{\perp }& =v\text{v}v-{v\text{v}v}_{\Vert }\\ & =v\text{v}v-(u\text{u}u.v\text{v}v)u\text{u}u\end{array}$$

接下来分别计算~

2.水平方向${v\text{v}v}_{\Vert }$的旋转

因为平行向量${v\text{v}v}_{\Vert }$绕$u\text{u}u$旋转任意角度之后还是自身，所以有：
$${{v\text{v}v}_{\Vert }}^{{}^{\prime }}={v\text{v}v}_{\Vert }$$

3.垂直方向${v\text{v}v}_{\perp }$的旋转

三维空间如图所示：

用2D的俯视图，会更容易看到清楚，对应于上图的下面一个虚线的圆

借助一个向量$w\text{w}w=u\text{u}u\times {v\text{v}v}_{\perp }$
注意：右手坐标系统，叉乘顺序
$$\begin{array}{rl}\Vert w\text{w}w\Vert & =\Vert u\text{u}u\times {v\text{v}v}_{\perp }\Vert \\ & =\Vert u\Vert .\Vert {v\text{v}v}_{\perp }\Vert .sin(\pi /2)\\ & =\Vert {v\text{v}v}_{\perp }\Vert \end{array}$$

将${v\text{v}v}_{\perp }^{{}^{\prime }}$分解到$w\text{w}w$和${v\text{v}v}_{\perp }$上，可以简单算出来下面的等式
$$\begin{array}{rl}{v\text{v}v}_{\perp }^{{}^{\prime }}& ={v\text{v}v}_v^{{}^{\prime }}+{v\text{v}v}_w^{{}^{\prime }}\\ & =cos(\theta ){v\text{v}v}_{\perp }+sin(\theta )w\text{w}w\\ & =cos(\theta ){v\text{v}v}_{\perp }+sin(\theta )(u\text{u}u\times {v\text{v}v}_{\perp })\end{array}$$
书上说提到用一点三角学的公式，其实主要是下面的这个三个向量的模都相等。按照投影或者分解，本应该是${v\text{v}v}_v^{{}^{\prime }}=cos(\theta ){v\text{v}v}_{\perp }^{{}^{\prime }}$和${v\text{v}v}_w^{{}^{\prime }}=sin(\theta ){v\text{v}v}_{\perp }^{{}^{\prime }}$。简单换算一下就是上面的等式了
$$\begin{array}{rl}{v\text{v}v}_v^{{}^{\prime }}& =cos(\theta ){v\text{v}v}_{\perp }^{{}^{\prime }}=cos(\theta )\frac{{v\text{v}v}_{\perp }}{\Vert {v\text{v}v}_{\perp }\Vert }\Vert {v\text{v}v}_{\perp }^{{}^{\prime }}\Vert =cos(\theta ){v\text{v}v}_{\perp }\\ {v\text{v}v}_w^{{}^{\prime }}& =sin(\theta ){v\text{v}v}_{\perp }^{{}^{\prime }}=sin(\theta )\frac{w\text{w}w}{\Vert w\text{w}w\Vert }\Vert {v\text{v}v}_{\perp }^{{}^{\prime }}\Vert =sin(\theta )w\text{w}w=sin(\theta )(u\text{u}u\times {v\text{v}v}_{\perp })\end{array}$$

4.向量$v\text{v}v$的旋转公式

再由旋转后的向量组合起来得到
$$\begin{array}{rl}{v\text{v}v}^{{}^{\prime }}& ={v\text{v}v}_{\Vert }^{{}^{\prime }}+{v\text{v}v}_{\perp }^{{}^{\prime }}\\ & ={v\text{v}v}_{\Vert }+cos(\theta ){v\text{v}v}_{\perp }+sin(\theta )(u\text{u}u\times {v\text{v}v}_{\perp })\end{array}$$
因为叉乘遵守分配律，有
$$\begin{array}{rl}u\text{u}u\times {v\text{v}v}_{\perp }& =u\text{u}u\times (v\text{v}v-{v\text{v}v}_{\Vert })\\ & =u\text{u}u\times v\text{v}v-u\text{u}u\times {v\text{v}v}_{\Vert }\\ & =u\text{u}u\times v\text{v}v(u\text{u}u平行于{v\text{v}v}_{\Vert })\end{array}$$
最后，将${v\text{v}v}_{\Vert }=(u\text{u}u.v\text{v}v).u\text{u}u$和${v\text{v}v}_{\perp }=v\text{v}v-(u\text{u}u.v\text{v}v).u\text{u}u$代入，得到
$$\begin{array}{r}{v\text{v}v}^{{}^{\prime }}=(u\text{u}u.v\text{v}v).u\text{u}u+cos(\theta )(v\text{v}v-(u\text{u}u.v\text{v}v).u\text{u}u)+sin(\theta )(u\text{u}u\times v\text{v}v)\\ =cos(\theta )v\text{v}v+(1-cos(\theta ))(u\text{u}u.v\text{v}v).u\text{u}u+sin(\theta )(u\text{u}u\times v\text{v}v)\end{array}$$

注意这个公式，参考[6]中叫做叫做「Rodrigues’ Rotation Formula」

四元数的3D旋转公式

接下来，我们要慢慢推导四元数的公式了，那么先要搞清楚四元数喝旋转之前的关系。下面的内容把四元数相关性质省略了，想看的话，看参考[6]或者其他书籍吧。

参考上一节，还是走相同的流程：三维空间旋转公式，也是分解向量$v$。但是，这次不是用的向量的三维表示，而是再上一个维度，用四元数去表示。高一维度能完全秒杀低一维度的生物，所以想想还是挺厉害的。

先放两个引理，后面还会遇到几个引理，都是为了方便推导公式：

• 引理一 纯四元数
  简单来讲呢？现在要用一个四维的向量来表示空间的旋转，表示方法就有了一些对应的变化

如果一个四元数能写成这样的形式，
$$v=[0,v\text{v}v]$$
那么我们称$v$为一个纯四元数

注意：这里用非粗体字母表示四元数

• 引理二 Graßmann 积

直接列出来，如果看推理，参考[6]

对任意的四元数$q_1=[s,v\text{v}v],q_2=[t,u\text{u}u],q_1{q}_2的结果是$
$$q_1{q}_2=[st-v\text{v}v.u\text{u}u,su\text{u}u+tv\text{v}v+v\text{v}v\times u\text{u}u]$$

按照引理一，列出所有的四元数表示
$$\begin{array}{rlr}& v=[0,v\text{v}v]& v^{{}^{\prime }}=[0,{v\text{v}v}^{{}^{\prime }}]\\ & v_{\perp }=[0,{v\text{v}v}_{\perp }]& v_{\perp }^{{}^{\prime }}=[0,{v\text{v}v}_{\perp }^{{}^{\prime }}]\\ & v_{\Vert }=[0,{v\text{v}v}_{\Vert }]& v_{\Vert }^{{}^{\prime }}=[0,{v\text{v}v}_{\Vert }^{{}^{\prime }}]\\ & u=[0,u\text{u}u]& \end{array}$$
并且有：
$$v=v_{\perp }+v_{\Vert }{v}^{{}^{\prime }}=v_{\perp }^{{}^{\prime }}+v_{\Vert }^{{}^{\prime }}$$

1.水平方向$v_{\Vert }$的旋转

同理，这个旋转没有发生任何变化
$$v_{\Vert }^{{}^{\prime }}=v_{\Vert }$$

2.垂直方向$v_{\perp }$的旋转

之前推导有
$${v\text{v}v}_{\perp }^{{}^{\prime }}=cos(\theta ){v\text{v}v}_{\perp }+sin(\theta )(u\text{u}u\times {v\text{v}v}_{\perp })$$
把向量换成四元数，就可以有四元数表示的公式了，但是向量的叉乘怎么和四元数什么乘积对应起来呢？看下面，可以看到是Graßmann 积

根据引理二的Graßmann 积，假设有两个四元数$v=[0,v\text{v}v],u=[0,u\text{u}u]$，那么$vu=[-v\text{v}v.u\text{u}u,v\text{v}v\times u\text{u}u]$。由此，那么可以算出来推理一中的四元数
$$\begin{array}{rl}u{v}_{\perp }& =[-u\text{u}u.{v\text{v}v}_{\perp },u\text{u}u\times {v\text{v}v}_{\perp }]\\ & =[0,u\text{u}u\times {v\text{v}v}_{\perp }](u\text{u}u.{v\text{v}v}_{\perp }=0)\end{array}$$
结果还是一个四元数！所以，上面向量${v\text{v}v}_{\perp }^{{}^{\prime }}$的表示，用四元数表示为：
$$v_{\perp }^{{}^{\prime }}=cos(\theta )v_{\perp }+sin(\theta )(u{v}_{\perp })$$
四元数遵循乘法分配律，得到
$$\begin{array}{rl}{v}_{\perp }^{{}^{\prime }}& =cos(\theta )v_{\perp }+sin(\theta )(u{v}_{\perp })\\ & =(cos(\theta )+sin(\theta )u)v_{\perp }\end{array}$$
接下来做一些数学上常用的技巧。

如果将$(cos(\theta )+sin(\theta )u)$看成一个旋转四元数，我们就能将旋转写成四元数的乘积了。到此为止，我们已经将旋转与四元数的积联系起来了

令：$q=cos(\theta )+sin(\theta )u$，得到
$$v_{\perp }^{{}^{\prime }}=q{v}_{\perp }$$
对q进行变形
$$\begin{array}{rl}q& =cos(\theta )+sin(\theta )u\\ & =[cos(\theta ,0\text{0}0)]+[0,sin(\theta )u\text{u}u]\\ & =[cos(\theta ),sin(\theta )u\text{u}u]\end{array}$$

到这里，就和最开始的公式有点像了，但是还差个2倍的关系

注意：这里对一个向量${v\text{v}v}_{\perp }$绕旋转轴$u\text{u}u$旋转左乘四元数$q$就可以了。但是，这里的一个前提条件是向量和旋转轴垂直的情况
所以，接下来，我们再回到一般情况

3.四元数$v$的旋转公式

回到一般情况
$$\begin{array}{rl}{v}^{{}^{\prime }}& =v_{\perp }^{{}^{\prime }}+v_{\Vert }^{{}^{\prime }}\\ & =v_{\Vert }+q{v}_{\perp }\end{array}$$
这里，我把步骤写下来，完全参考[6]，对于中间的证明，可以翻阅参考[6]，非常的详细
接下来就是等式各种演变了

• 引理三

如果$q=[cos(\theta ),sin(\theta )u\text{u}u]$，如果$u\text{u}u$为单位向量，$q^2=qq=[cos(2\theta ),sin(2\theta )u\text{u}u]$

我要找的2倍角出现了，有点苗头了.

$$\begin{array}{rl}{v}^{{}^{\prime }}& =v_{\perp }^{{}^{\prime }}+v_{\Vert }^{{}^{\prime }}\\ & =v_{\Vert }+q{v}_{\perp }\\ & =1.v_{\Vert }+q{v}_{\perp }\\ & =p{p}^{-1}{v}_{\Vert }+pp{v}_{\perp }(令q=p^2,则p=[cos(1/2\theta ),sin(1/2\theta )u\text{u}u])\end{array}$$
因为$p$也是单位四元数，即$\Vert p\Vert =1$，则有
$$p^{-1}=p^{\star }$$
继续做等式变换
$$\begin{array}{rl}{v}^{{}^{\prime }}& =p{p}^{-1}{v}_{\Vert }+pp{v}_{\perp }\\ & =p{p}^{\star }{v}_{\Vert }+pp{v}_{\perp }\end{array}$$

• 引理四

假设$v_{\Vert }=[0,{v\text{v}v}_{\Vert }]$是一个纯四元数，而$q=[\alpha ,\beta u\text{u}u]$，其中$u\text{u}u$是一个单位向量，$\alpha ,\beta \in \mathbb{R}$。在这种情况下，如果${v\text{v}v}_{\Vert }$平行于$u\text{u}u$，那么$q{v}_{\Vert }=v_{\Vert }q$

• 引理五

假设 $v_{\perp }=[0,{v\text{v}v}_{\perp }]$ 是一个纯四元数，而$q=[\alpha ,\beta u\text{u}u]$，其中 $u\text{u}u$ 是一个单位向量，$\alpha ,\beta \in \mathbb{R}$．在这种条件下，如果 ${v\text{v}v}_{\perp }$ 正交于 $u\text{u}u$，那么 $q{v}_{\perp }=v_{\perp }{q}^{\star }$

则等式可以继续化简
$$\begin{array}{rl}{v}^{{}^{\prime }}& =p{p}^{\star }{v}_{\Vert }+pp{v}_{\perp }\\ & =p{v}_{\Vert }{p}^{\star }+p{v}_{\perp }{p}^{\star }\\ & =p(v_{\Vert }+v_{\perp })p^{\star }\end{array}$$
可以看到$(v_{\Vert }+v_{\perp })=v$，则
$$v^{{}^{\prime }}=pv{p}^{\star }=pv{p}^{-1}$$
其中，$q=[cos(\theta /2),sin(\theta /2)u\text{u}u]$

小结

• 可以看到如果用四元数去旋转一个向量的话，并不是四元数左乘或者右乘一次就行了，而是后面还要乘以一个四元数的逆

到此，这个公式的证明就结束了，参考[6]说四元数的表示和3D旋转向量表示的「Rodrigues’ Rotation Formula」公式是等价的，虽然没有证明，但是其实证明就是各种三角函数的带入。而且文章中也给出了一个关键的叉乘公式，下面自己就手动证明一下，可以跳过。

两种公式等价证明

为了书写方便，先假设$c=cos(\theta /2),s=sin(\theta /2)$。已知$v=[0,v\text{v}v]，q=[c,su\text{u}u]$，Graßmann 积算前面两个
$$\begin{array}{rl}qv& =[0-su\text{u}u.v\text{v}v,cv\text{v}v+0\text{0}0+su\text{u}u\times v\text{v}v]\\ & =[-su\text{u}u.v\text{v}v,cv\text{v}v+su\text{u}u\times v\text{v}v]\end{array}$$
按照四元数性质，可以很容易知道$q^{-1}=q^{\star }=[c,-su\text{u}u]$，继续使用Graßmann 积推导公式
$$qv{q}^{-1}=[-scu\text{u}u.v\text{v}v-(cv\text{v}v+su\text{u}u\times v\text{v}v).(-su\text{u}u),-su\text{u}u.v\text{v}v.(-su\text{u}u)+c(cv\text{v}v+su\text{u}u\times v\text{v}v)+(cv\text{v}v+su\text{u}u\times v\text{v}v)\times (-su\text{u}u)]$$
分两部算逗号前面的，这样会清洗一点，不然公式写的老长

• 1.逗号前面四元数的实数部分
  $$\begin{array}{rlr}& =-scu\text{u}u.v\text{v}v-(cv\text{v}v+su\text{u}u\times v\text{v}v).(-su\text{u}u)& \\ & =-scu\text{u}u.v\text{v}v+scv\text{v}v.u\text{u}u-su\text{u}u\times v\text{v}v.(-su\text{u}u)& \\ & =s^{2}u\text{u}u\times v\text{v}v.u\text{u}u& (向量点积满足交换律：a\text{a}a.b\text{b}b=b\text{b}b.a\text{a}a)\\ & =0& (u\text{u}u\times v\text{v}v和u\text{u}u垂直)\end{array}$$

• 2.逗号后面四元数的虚数部分
  $$\begin{array}{rl}& =-su\text{u}u.v\text{v}v.(-su\text{u}u)+c(cv\text{v}v+su\text{u}u\times v\text{v}v)+(cv\text{v}v+su\text{u}u\times v\text{v}v)\times (-su\text{u}u)\\ & =s^2(u\text{u}u.v\text{v}v)u\text{u}u+c^{2}v\text{v}v+csu\text{u}u\times v\text{v}v-csv\text{v}v\times u\text{u}u-s^{2}u\text{u}u\times v\text{v}v\times u\text{u}u\\ & =s^2(u\text{u}u.v\text{v}v)u\text{u}u+c^{2}v\text{v}v+2csu\text{u}u\times v\text{v}v-s^{2}u\text{u}u\times v\text{v}v\times u\text{u}u(向量叉乘：a\text{a}a\times b\text{b}b=-b\text{b}b\times a\text{a}a)\\ & =s^2(u\text{u}u.v\text{v}v)u\text{u}u+c^{2}v\text{v}v+2csu\text{u}u\times v\text{v}v-s^2[(u\text{u}u.u\text{u}u)v\text{v}v-(u\text{u}u.v\text{v}v)u\text{u}u][(a\text{a}a\times b\text{b}b)\times c\text{c}c=(a\text{a}a.c\text{c}c)b\text{b}b-(a\text{a}a.b\text{b}b)c\text{c}c]\\ & =s^2(u\text{u}u.v\text{v}v)u\text{u}u+s^2(u\text{u}u.v\text{v}v)u\text{u}u+c^{2}v\text{v}v-s^{2}v\text{v}v+2csu\text{u}u\times v\text{v}v\\ & =cos(\theta )v\text{v}v+(1-cos(\theta ))(u\text{u}u.v\text{v}v)u\text{u}u+sin(\theta )u\text{u}u\times v\text{v}v(三角函数两倍角公式)\end{array}$$

• 3.再合在一起
  $$qv{q}_{-1}=[0,cos(\theta )v\text{v}v+(1-cos(\theta ))(u\text{u}u.v\text{v}v)u\text{u}u+sin(\theta )u\text{u}u\times v\text{v}v]$$
  这样就完成证明了

参考

[1]Maths - AxisAngle to Quaternion
[2]3D数学基础图形与游戏开发
[4]四元数与欧拉角（RPY角）的相互转换
[5]Converting to Euler & Tait-Bryan
[6]四元数旋转公式推导
[7]eater.net
[8]四元数与三位旋转
[9]https://www.geogebra.org/classic
[10]geogebra