罗德里格旋转公式 (Rodrigues’ Rotation Formula)

关于三维空间中的旋转，我们以前提到过基于欧拉角的旋转表达矩阵，它们分别描述了围绕 x 轴、y 轴、z 轴旋转后坐标应当如何变化。事实上，我们可以更进一步，推导出一个通用的、围绕过原点的任意轴旋转的公式。

题设

这一节我们来描述我们已知的条件和待求的目标：

给定一个方向向量$\vec{u}$作为旋转轴，$\vec{v}$为待旋转的向量，我们希望得到$\vec{v}$围绕着$\vec{u}$逆时针旋转$\theta$角度之后的向量$\overrightarrow{v^{\prime }}$。注意，$\overrightarrow{v^{\prime }}$的表达式必须用已知条件$\vec{u}$、$\vec{v}$和$\theta$来表达。

思路

旋转前后向量的长度不会变化，反而是如何计算旋转后向量的方向成为了一个难题。我们采用分解向量的思路来解决向量旋转的问题——把$\vec{v}$分解为平行于$\vec{u}$的向量$\overrightarrow{v_{//}}$和垂直于$\vec{u}$的向量$\overrightarrow{v_{\perp }}$，分解的示意图如下：

这样定义向量的分解方式是有好处的，我们可以发现，旋转前后$\overrightarrow{v_{//}}$没有变化，而旋转前的$\overrightarrow{v_{\perp }}$、旋转后的$\overrightarrow{v_{\perp }^{\prime }}$都在一个平面内，同一个平面内的旋转比起三维旋转要好解决的多。

公式推导

$\overrightarrow{v_{//}}$实际上是$\vec{v}$在$\vec{u}$上的正交投影，因此我们可以得出：
$$\begin{gathered} \overrightarrow{v_{//}}=|\vec{v}|\frac{\vec{u}\cdot \vec{v}}{|\vec{u}|\cdot |\vec{v}|}\cdot \frac{\vec{u}}{|\vec{u}|} \\ =\frac{\vec{u}\cdot \vec{v}}{|\vec{u}{|}^2}\vec{u} \\ =(\vec{u}\cdot \vec{v})\vec{u} \end{gathered}$$
然后，我们运用向量减法给出$\overrightarrow{v_{\perp }}$的表达式：
$$\overrightarrow{v_{\perp }}=\vec{v}-\overrightarrow{v_{//}}$$
接下来，我们需要给出$\overrightarrow{v_{\perp }^{\prime }}$的表达式。给出一个直观的旋转俯视图：

先定义$\vec{w}$。引入这个向量是为了正交分解$\overrightarrow{v_{\perp }^{\prime }}$，因此它必须垂直于$\overrightarrow{v_{\perp }}$。如果你对叉乘很熟悉的话，很快就能想到我们可以用$\vec{u}\times \vec{v}$来得到具有这样的性质的向量。注意叉乘的顺序，根据旋转示意图和右手定则，$\vec{u}\times \overrightarrow{v_{\perp }}$的向量方向才是俯视图中的$\vec{w}$方向。

然后，我们把$\overrightarrow{v_{\perp }^{\prime }}$正交分解成平行于$\overrightarrow{v_{\perp }}$的$\overrightarrow{v_v^{\prime }}$和平行于$\vec{w}$的$\overrightarrow{v_w^{\prime }}$，并给出$\overrightarrow{v_{\perp }^{\prime }}$的表达式。
$$\begin{gathered} \overrightarrow{v_{\perp }^{\prime }}=\overrightarrow{v_v^{\prime }}+\overrightarrow{v_w^{\prime }} \\ \overrightarrow{v_v^{\prime }}=|\overrightarrow{v_{\perp }}|\cos \theta \cdot \frac{\overrightarrow{v_{\perp }}}{|\overrightarrow{v_{\perp }}|}=\overrightarrow{v_{\perp }}\cos \theta \\ \overrightarrow{v_w^{\prime }}=|\overrightarrow{v_{\perp }}|\sin \theta \cdot \frac{\vec{u}\times \overrightarrow{v_{\perp }}}{|\vec{u}\times \overrightarrow{v_{\perp }}|} \\ |\vec{u}\times \overrightarrow{v_{\perp }}|=|\vec{u}||\overrightarrow{v_{\perp }}|\sin (\pi /2)=|\overrightarrow{v_{\perp }}| \\ \overrightarrow{v_w^{\prime }}=(\vec{u}\times \overrightarrow{v_{\perp }})\sin \theta \\ \overrightarrow{v_{\perp }^{\prime }}=\overrightarrow{v_{\perp }}\cos \theta +(\vec{u}\times \overrightarrow{v_{\perp }})\sin \theta \end{gathered}$$
上面的式子可以进一步被化简，我们代入$\overrightarrow{v_{\perp }}$的表达式，并运用叉乘的分配律：
$$\begin{gathered} \overrightarrow{v_{\perp }^{\prime }}=(\vec{v}-\overrightarrow{v_{//}})\cos \theta +(\vec{u}\times (\vec{v}-\overrightarrow{v_{//}}))\sin \theta \\ 使用分配律，(\vec{u}\times (\vec{v}-\overrightarrow{v_{//}}))=\vec{u}\times \vec{v}-\vec{u}\times \overrightarrow{v_{//}} \\ 因为共线，\vec{u}\times \overrightarrow{v_{//}}=0 \\ \overrightarrow{v_{\perp }^{\prime }}=\vec{v}\cos \theta -\overrightarrow{v_{//}}\cos \theta +(\vec{u}\times \vec{v})\sin \theta \end{gathered}$$
最后，我们终于可以给出$\overrightarrow{v^{\prime }}$的表达式：
$$\begin{gathered} \overrightarrow{v^{\prime }}=\overrightarrow{v_{//}}+\overrightarrow{v_{\perp }^{\prime }} \\ \overrightarrow{v^{\prime }}=\overrightarrow{v_{//}}+\vec{v}\cos \theta -\overrightarrow{v_{//}}\cos \theta +(\vec{u}\times \vec{v})\sin \theta =\vec{v}\cos \theta +(1-\cos \theta )\overrightarrow{v_{//}}+(\vec{u}\times \vec{v})\sin \theta \\ \overrightarrow{v^{\prime }}=\vec{v}\cos \theta +(1-\cos \theta )(\vec{u}\cdot \vec{v})\vec{u}+(\vec{u}\times \vec{v})\sin \theta \end{gathered}$$
上式的结果即是作为标题而提及的 Rodrigues’ Rotation Formula。

化简为矩阵

我们需要进一步化简公式，得到其等价的矩阵表达形式，才方便代码的实现。

首先，我们需要知道向量三重积公式：
$$\vec{a}\times (\vec{b}\times \vec{c})=(\vec{a}\cdot \vec{c})\cdot \vec{b}-(\vec{a}\cdot \vec{b})\cdot \vec{c}$$
然后，我们还需要知道，向量的叉乘与矩阵之间的联系：
$$\vec{a}\times \vec{b}=\left(\begin{array}{c}{y}_a{z}_b-z_a{y}_b\\ z_a{x}_b-x_a{z}_b\\ x_a{y}_b-y_a{x}_b\end{array}\right)=A\cdot b=\left(\begin{array}{ccc}0& -z_a& y_a\\ z_a& 0& -x_a\\ -y_a& x_a& 0\end{array}\right)\left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)$$
向量的叉乘可以化为矩阵与向量的乘积，而且需要注意的是，矩阵只与左边的向量有关。

---

我们化简的目标是得到下述形式的表达式，其中M是矩阵。
$$\overrightarrow{v^{\prime }}=M\cdot \vec{v}$$
再次观察公式：
$$\overrightarrow{v^{\prime }}=\vec{v}\cos \theta +(1-\cos \theta )(\vec{u}\cdot \vec{v})\vec{u}+(\vec{u}\times \vec{v})\sin \theta$$
第一项和第三项都可以快速给出等价的矩阵形式，其中，$\vec{v}\cos \theta$可以化为：
$$\vec{v}\cos \theta =\left(\begin{array}{ccc}\cos \theta & 0& 0\\ 0& \cos \theta & 0\\ 0& 0& \cos \theta \end{array}\right)\cdot \vec{v}$$
正如前文所说，$(\vec{u}\times \vec{v})\sin \theta$也可以化成矩阵乘向量的形式，这里记向量$\vec{u}$形成的矩阵为$R_u$，可以得到$(\vec{u}\times \vec{v})\sin \theta =R_u\sin \theta \cdot \vec{v}$

比较难以化简的是第二项，处于外部的是向量$\vec{u}$而不是$\vec{v}$，这给我们带来了一些麻烦。观察$(\vec{u}\cdot \vec{v})\vec{u}$这一项和已知的三重积公式，或许可以想办法配凑另外一项，从而把点乘变为叉乘，再运用叉乘的性质化作矩阵。有好几种可能的叉乘式，最终我们选择配凑出这样的叉乘：$\vec{u}\times (\vec{u}\times \vec{v})$。其中一个理由是$\vec{u}$都在左边，我们可以复用前面提到的矩阵$R_u$；另一个理由是缺失的那一项很好配凑：
$$\vec{u}\times (\vec{u}\times \vec{v})=(\vec{u}\cdot \vec{v})\vec{u}-(\vec{u}\cdot \vec{u})\vec{v}=(\vec{u}\cdot \vec{v})\vec{u}-\vec{v}$$
明确了目标之后，开始化简：
$$\begin{gathered} \vec{v}\cos \theta +(1-\cos \theta )(\vec{u}\cdot \vec{v})\vec{u}=\vec{v}\cos \theta +\vec{v}-\vec{v}=\vec{v}-(1-\cos \theta )\vec{v}+(1-\cos \theta )(\vec{u}\cdot \vec{v})\vec{u} \\ =\vec{v}+(1-\cos \theta )((\vec{u}\cdot \vec{v})\vec{u}-\vec{v}) \\ =\vec{v}+(1-\cos \theta )(\vec{u}\times (\vec{u}\times \vec{v})) \end{gathered}$$
我们再次利用叉乘转换为矩阵的性质，可以得到：
$$\vec{u}\times (\vec{u}\times \vec{v})=\vec{u}\times (R_u\cdot \vec{v})=R_u^2\cdot \vec{v}$$
最后我们得到了一个比较复杂的矩阵：
$$\begin{gathered} \overrightarrow{v^{\prime }}=\vec{v}+(1-\cos \theta )\cdot R_u^2\cdot \vec{v}+R_u\sin \theta \cdot \vec{v} \\ 记I为单位矩阵 \\ \overrightarrow{v^{\prime }}=(I+(1-\cos \theta )\cdot R_u^2+R_u\sin \theta )\cdot \vec{v} \\ M=I+(1-\cos \theta )\cdot R_u^2+R_u\sin \theta \end{gathered}$$
M即为我们所求的矩阵。

Refer

罗德里格旋转公式（Rodrigues’ rotation formula）

四元数与三维旋转