将三维坐标 \((x,y,z)\) 转化成四维齐次坐标 \((x,y,z,1)\)，可以允许我们用矩阵乘法来表示平移、旋转和缩放。平移和缩放的矩阵都比较简单，绕 x、y、z 轴的旋转也比较简单。

但绕任意轴旋转的场景，对应的矩阵变换比较复杂，本篇文章专门来介绍它。

1. 罗德里格旋转公式

绕任意轴旋转已经有罗德里格旋转公式可以利用，它给定一个单位旋转向量 \(k\) 和一个旋转角 \(\theta\)，任何向量 \(v\) 绕 \(k\) 旋转 \(\theta\) 角后，得到

\(v_{rot}=v \text{cos}\theta + (k\times v)\text{sin}\theta + k(k\cdot v)(1-\text{cos}\theta)\)

1.1 引

在推导罗德里格旋转公式之前，我们先看一个特殊情况，如图 1 所示，就是 \(v\) 和旋转轴 \(k\) 是垂直的情况。

\(k\) 和 \(v\) 以及它们的叉乘 \(k\times v\) 可以组成一个笛卡尔坐标系，它们两两垂直。所以此时我们可以在“俯视图”角度思考问题。

如图 2 所示，\(v_{rot}\) 在 \(v\) 方向上分量的长度是 \(\lvert v \rvert \text{cos}\theta\)，在 \(k\times v\) 方向上分量的长度是 \(\lvert v \rvert \text{sin}\theta\)。

我们需要知道 \(k\times v\) 的大小，等于

\(\lvert k \rvert \lvert v \rvert \text{sin}\langle k,v \rangle\)

因为 \(k\) 和 \(v\) 垂直，且 \(k\) 是单位向量，所以其大小也是 \(\lvert v \rvert\)。

这样，我们就能得到这种特殊情况下的解：

\(v_{rot}=v\text{cos}\theta + (k\times v)\text{sin}\theta \)

---

引子介绍完了，我们开始推导罗德里格旋转公式。它的核心思路是，先把 \(v\) 分解成平行于旋转轴 \(k\) 的分量和垂直于 \(k\) 的分量。

平行于 \(k\) 的分量，绕 \(k\) 旋转是不变的。所以只需要考虑垂直于 \(k\) 的分量，就变成了 1.1 引 这个问题。

首先我们通过点乘，求得 \(v\) 在 \(k\) 方向的投影向量，即平行向量

\(v_\parallel=(v\cdot k)k\)

垂直向量为

\(v_\perp=v-(v\cdot k)k\)

因为 \(v_\perp\) 和 \(k\) 垂直，按照 1.1 引 的结论得

\(v_{\perp rot}=v_\perp\text{cos}\theta + (k\times v_\perp)\text{sin}\theta\)

组合 \(v_\parallel\) 分量求出 \(v_{rot}\)

\(v_{rot}=v_\parallel+v_{\perp rot}\)

\(=(v\cdot k)k+v_\perp\text{cos}\theta + (k\times v_\perp)\text{sin}\theta\)

\(=(v\cdot k)k+(v-(v\cdot k)k)\text{cos}\theta + (k\times v_\perp)\text{sin}\theta\)

\(=v \text{cos}\theta + (k\times v_\perp)\text{sin}\theta + k(k\cdot v)(1-\text{cos}\theta)\)

以上我们没有把叉乘这项里的 \(v_\perp\) 用等式替换，主要是太“琐碎”了。不过现在除了这项，其他项已经和标准的罗德里格旋转公式一样了。

其实，这一项是等价的，我们来证明一下。

\(k\times v = k\times (v_\parallel + v_\perp)\)

\(= k\times v_\parallel + k\times v_\perp\)

\(= 0 + k\times v_\perp = k\times v_\perp\)

所以最终，\(v_{rot}=v \text{cos}\theta + (k\times v)\text{sin}\theta + k(k\cdot v)(1-\text{cos}\theta)\)。

2. 罗德里格旋转公式的矩阵形式

罗德里格旋转公式可以推导出一个旋转矩阵。有一种简单的方法，分别令

\( v= \begin{pmatrix} 1 \\ 0 \\ 0 \end{pmatrix}, v= \begin{pmatrix} 0 \\ 1 \\ 0 \end{pmatrix}, v= \begin{pmatrix} 0 \\ 0 \\ 1 \end{pmatrix} \)

代入公式，可以依次求得旋转矩阵的第一列、第二列和第三列。

我们这边再采用直接构造的方式进行。

针对 \(v\text{cos}\theta\) 项，我们有矩阵

\( \begin{pmatrix} \text{cos}\theta & 0 & 0 \\ 0 & \text{cos}\theta & 0 \\ 0 & 0 & \text{cos}\theta \end{pmatrix} \)

针对 \(k\times v\) 项，我们有叉积矩阵

\( K= \begin{pmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{pmatrix} \)

针对 \(k(k\cdot v)\)，我们有外积矩阵 \(kk^T\)。

所以最终，我们有旋转矩阵

\(R=\text{cos}\theta I + \text{sin}\theta K + (1-\text{cos}\theta)kk^T\)

2.1 代码实现

为了方便程序实现，我们把矩阵 \(R\) 展开。

\( R= \begin{pmatrix} k_x^2(1-\text{cos}\theta)+\text{cos}\theta & k_xk_y(1-\text{cos}\theta)-k_z\text{sin}\theta & k_xk_z(1-\text{cos}\theta)+k_y\text{sin}\theta \\ k_xk_y(1-\text{cos}\theta)+k_z\text{sin}\theta & k_y^2(1-\text{cos}\theta)+\text{cos}\theta & k_yk_z(1-\text{cos}\theta)-k_x\text{sin}\theta \\ k_xk_z(1-\text{cos}\theta)-k_y\text{sin}\theta & k_yk_z(1-\text{cos}\theta)+k_x\text{sin}\theta & k_z^2(1-\text{cos}\theta)+\text{cos}\theta \end{pmatrix} \)

代码实现如清单 1 所示，只要小心一点，依次把各项填入即可。

1. template<typename T>
2. Matrix<T, 4, 4> RotationMatrix(Vector3<T> axis, T angle_rad)
3. {
4.     axis.Normalize();
5.  
6.     T c = cos(angle_rad);
7.     T s = sin(angle_rad);
8.     T one_minus_c = 1 - c;
9.  
10.     T x = axis.x(), y = axis.y(), z = axis.z();
11.     return Matrix<T, 4, 4>({
12.         x * x * one_minus_c + c, x * y * one_minus_c - z * s, x * z * one_minus_c + y * s, 0,
13.         x * y * one_minus_c + z * s, y * y * one_minus_c + c, y * z * one_minus_c - x * s, 0,
14.         x * z * one_minus_c - y * s, y * z * one_minus_c + x * s, z * z * one_minus_c + c, 0,
15.         0, 0, 0, 1
16.         });
17. }