罗德里格斯公式（Rodrigues Formula）

罗德里格斯公式（Rodrigues’s Formula）

摘要：罗德里格斯公式推导过程。主要参考Krasjet的文档，写的非常详细，如果想看更加详细的过程，建议去看看文档。这里主要做了简洁浓缩，方便快速查看。

一、轴角（Axis-Angle）的介绍

采用一个旋转轴u和一个旋转角度θ来刻画旋转。

比如：
我们有一个经过原点的（如果旋转轴不经过原点我们可以先将旋转轴平移到原点，进行旋转，再平移回原处）旋转轴 $u=(x,y,z{)}^T$ ，我们希望将一个向量 $v$，沿着这个旋转轴旋转 $\theta$ 度，变换到 $v\prime$ ：

为了消除旋转轴 u 模长这个多余的自由度，我们可以规定旋转轴 u 的模长为 $\Vert u\Vert =\sqrt{x2+y2+z2}=1$，也就是说 u 是一个单位向量。

二、旋转的分解

我们可以将$v$分解为平行与旋转轴，以及垂直于旋转轴的两个分量，$v\Vert$和 $v\perp$。
$$v=v\Vert +v\perp$$
然后分别旋转这两个分向量，再将它们旋转的结果相加，获得旋转后的向量。
$$v\prime =v\prime \Vert +v\prime \perp$$

2.1 平行量的旋转

平行量的旋转，前后不变
$$v\prime \Vert =v\Vert$$
$v\Vert$ 其实就是 v 在 u 上的正交投影 (Orthogonal Projection)，根据正交投影的公式，我们可以得出：
$$v\Vert =(u\cdot v)u$$

2.2 垂直量的旋转

$v\perp$正交于$u$ 的 ，这个旋转可以看做是平面内的一个旋转。因为旋转不改变 v ⊥ 的长度，所以路径是
一个圆。下面是这个旋转的示意图，右侧的为俯视图。

现在，3D 的旋转就被我们转化为了 2D 平面上的旋转。由于在这个平面上我们只有一个向量 $v\perp$ ，用它来表示一个旋转是不够的，我们还需要构造一个同时正交于 $u$ 和 $v\perp$ 的向量 $w$，这个可以通过叉乘来获得：
$$\begin{gathered} w=u\times v\perp \\ \Vert w\Vert =\Vert u\times v\perp \Vert =\Vert u\Vert .\Vert v\perp \Vert .sin(\pi /2)=\Vert v\perp \Vert \end{gathered}$$
也就是说，$w$ 和 $v\perp$ 的模长是相同的，所以，w 也位于圆上。
有了这个新的向量 $w$，就相当于我们在平面内有了两个坐标轴。
我们现在可以把 $v\prime \perp$ 投影到 $w$ 和 $v\perp$ 上，将其分解为 $v_v\prime$ 和 $v_w\prime$ 。使用一点三角学的知识我们就能得到：
$$v\prime \perp =v_v\prime +v_w\prime =cos(\theta )v\perp +sin(\theta )w=cos(\theta )v\perp +sin(\theta )(u\times v\perp )$$

三、旋转的合成

将上面的两个结果组合就可以获得：
$$v\prime =v\prime \Vert +v\prime \perp =v\Vert +cos(\theta )v\perp +sin(\theta )(u\times v\perp )$$
由之前的结论，知：
$$v\Vert =(u\cdot v)u$$
由$v\Vert$可以得到$v\perp$：
$$v\perp =v-v\Vert =v-(u\cdot v)u$$
变换叉乘$u\times v\perp$得：
$$u\times v\perp =u\times (v-v\Vert )=u\times v-u\times v\Vert =u\times v$$

最终组合结果可以变化为：
$$v\prime =cos(\theta )v+(1-cos(\theta ))(u\cdot v)u+sin(\theta )(u\times v)$$
这也就是罗德里格斯公式（Rodrigues’s Formula）：
3D 空间中任意一个 $v$ 沿着单位向量 $u$ 旋转$\theta$ 角度之后的 $v\prime$ 为：
$$v\prime =cos(\theta )v+(1-cos(\theta ))(u\cdot v)u+sin(\theta )(u\times v)$$