1. 为什么需要轴角表示

在三维空间里描述"物体转了一下"，最直观的说法就是：绕着某一根轴 $u$，转过一个角度 $\theta$。比如一头玩具奶牛绕着穿过它身体的一根轴 $u$ 转了 $\theta$ 度，这就是最符合直觉的旋转描述方式，比旋转矩阵（9个数、还带约束）或者欧拉角（万向锁问题）都更"轻量"。
轴角表示用两个量来刻画一次旋转：

• 旋转轴：$u\in {\mathbb{R}}^3$，且 $\Vert u\Vert =1$（必须是单位向量，只表示方向）
• 旋转角度：$\theta \in \mathbb{R}$（绕轴转过的角度，正负表示方向，符合右手定则）
  把这两个量合并成一个三维向量，就得到旋转向量（也叫指数坐标）：
  $$\mathbf{\theta }=\theta u\in {\mathbb{R}}^3$$
  这个向量的方向就是转轴，长度就是转角。用一个三维向量就把旋转轴和旋转角度都装进去了，这是轴角表示最省空间的地方（旋转矩阵要用9个数，四元数要用4个数，而它只要3个数）。
  接下来的核心问题是：已知一个向量 $v$，绕着轴 $u$ 转过角度 $\theta$ 之后，新的向量 $v_{\text{rot}}$ 到底怎么算？这就引出了下面的罗德里格斯旋转公式。

2. 把向量 $v$ 分解成"轴向分量"和"垂直分量"

核心思路：一个向量绕轴旋转的时候，沿着轴方向的那部分是"纹丝不动"的（因为它本来就在轴上），真正发生旋转的只是垂直于轴的那部分。所以第一步永远是把 $v$ 拆成两块：
$$v=v_{\parallel }+v_{\perp }$$
其中：

• $v_{\parallel }$：$v$ 在轴 $u$ 方向上的投影分量（旋转时保持不变）
• $v_{\perp }$：$v$ 垂直于轴 $u$ 的分量（旋转时真正转动的部分）
  因为 $u$ 是单位向量，$v$ 在 $u$ 方向上的投影长度就是点积 $v\cdot u$，所以：
  $$v_{\parallel }=(v\cdot u)u$$
  垂直分量自然就是剩下的部分：
  $$v_{\perp }=v-v_{\parallel }=v-(v\cdot u)u$$
  （对应下方"图1"：$v$ 在左上方，沿 $u$ 轴方向的投影是 $v_{\parallel }$，垂直分量是左下方虚线箭头 $v_{\perp }$）

2.1 垂直分量转起来之后是什么样

$v_{\perp }$ 与 $u$ 垂直，所以它在垂直于 $u$ 的那个平面内。要让它绕 $u$ 转过角度 $\theta$，可以在这个平面里再引入一个辅助向量：
$$w=u\times v_{\perp }$$
因为 $u\perp v_{\perp }$，所以 $w$ 和 $v_{\perp }$ 长度相等、且互相垂直，二者恰好构成这个旋转平面内的一组"直角坐标轴"。于是 $v_{\perp }$ 转过角度 $\theta$ 后的新向量，就是这组坐标轴下的圆周运动，跟平面几何里"单位圆上一点转 $\theta$ 角"完全一样：
$$v_{\perp \text{rot}}=\cos \theta v_{\perp }+\sin \theta u\times v_{\perp }$$
（对应"图1"：$v_{\perp }$ 绕 $u$ 轴转过角度 $\theta$ 后落到右下方的 $w$ 方向；准确地说 $w$ 是辅助基向量，$v_{\perp \text{rot}}$ 是转动后的垂直分量）
由于轴向分量 $v_{\parallel }$全程不变，旋转后的总向量就是两部分相加：
$$v_{\text{rot}}=v_{\parallel }+v_{\perp \text{rot}}=(v\cdot u)u+\cos \theta v_{\perp }+\sin \theta u\times v_{\perp }$$
（对应"图1"：右上方的浅蓝色箭头 $v_{\text{rot}}$，正是 $v_{\parallel }$ 与旋转后垂直分量的合成）

图1（旋转向量几何分解示意图）见文末所附 PDF 第 1 页。

2.2 化简成只含 $u,v,\theta$ 的形式

上面的式子里还留着 $v_{\parallel }$、$v_{\perp }$ 这些中间变量，我们希望把它们全部替换成 $u$ 和 $v$ 本身，方便后续写成矩阵形式。这里要用到向量的"双重叉乘公式"（这是一个纯粹的向量恒等式，对任意向量都成立）：
$$u\times (u\times v)=(u\cdot v)u-v$$
这个公式的意义可以这样理解：叉乘两次相当于把 $v$"投影到轴上再翻转垂直分量"。把 $v_{\perp }=v-(v\cdot u)u$ 代入 $u\times v_{\perp }$，因为 $u\times u=0$，所以：
$$u\times v_{\perp }=u\times v$$
也就是说，$u$ 与 $v$ 的叉乘，其实就等于 $u$ 与 $v_{\perp }$ 的叉乘（$v_{\parallel }$ 那部分被叉乘"消掉"了）。再利用双重叉乘公式：
$$u\times (u\times v_{\perp })=u\times (u\times v)=(u\cdot v)u-v$$
而左边也等于 $u\times v_{\perp }$ 关于 $u$ 再叉乘一次，展开后可以得到：
$$v_{\perp }=-u\times (u\times v)=v-(u\cdot v)u$$
于是把 $v_{\parallel }=(v\cdot u)u$、$v_{\perp }=-u\times (u\times v)$ 都代回旋转公式：
$$v_{\text{rot}}=(v\cdot u)u+\cos \theta (v-(v\cdot u)u)+\sin \theta u\times v$$
整理一下，把含 $\cos \theta$ 的项合并：
$$\boxed{v_{\text{rot}}=v+(1-\cos \theta )u\times (u\times v)+\sin \theta u\times v}$$
或者写成另一种等价形式（把 $u\times (u\times v)=(u\cdot v)u-v$ 代进去）：
$$\boxed{v_{\text{rot}}=\cos \theta v+(1-\cos \theta )(v\cdot u)u+\sin \theta u\times v}$$
这两个公式就是**罗德里格斯旋转公式（Rodrigues’ rotation formula）**的"向量形式"——直接告诉你一个向量绕给定轴转一定角度之后的新坐标，不需要先构造旋转矩阵。

3. 把叉乘写成矩阵乘法：反对称矩阵 $[u]$

上面的公式里反复出现 $u\times (\cdot )$ 这种运算，而"叉乘"本质上是一个线性变换——对任意固定的 $u$，"$u\times v$"是关于 $v$ 的线性函数。既然是线性的，就一定能写成矩阵乘法的形式：
$$u\times v=[u]v,[u]=\left(\begin{array}{ccc}0& -u_z& u_y\\ u_z& 0& -u_x\\ -u_y& u_x& 0\end{array}\right)$$
这里 $[u]$ 叫做 $u$ 的反对称矩阵（也叫斜对称矩阵，满足 $[u{]}^{\mathrm{T}}=-[u]$）。把叉乘换成矩阵乘法之后，罗德里格斯公式里的 $u\times v$ 就变成 $[u]v$，$u\times (u\times v)$ 就变成 $[u]([u]v)=[u{]}^{2}v$。于是：
$$v_{\text{rot}}=v+(1-\cos \theta )[u{]}^{2}v+\sin \theta [u]v=(I+(1-\cos \theta )[u{]}^2+\sin \theta [u])v$$
括号里这一整坨东西不再依赖 $v$，它就是把"绕 $u$ 轴转 $\theta$ 角"这件事完全封装起来的旋转矩阵：
$$\boxed{R(u,\theta )=I+(1-\cos \theta )[u{]}^2+\sin \theta [u]}$$
这就是罗德里格斯旋转公式的矩阵形式：给定轴角 $(u,\theta )$，直接套公式就能得到对应的旋转矩阵 $R(u,\theta )$，不需要一步步做投影、分解、旋转再合成。

图1、图2 中间的推导过程，对应的正是"罗德里格斯旋转公式"从向量形式到矩阵形式的转化。

4. 反过来：已知旋转矩阵 $R$，怎么求出轴角 $(u,\theta )$

正向公式解决了"已知 $(u,\theta )$ 求 $R$"，实际应用中经常还需要反过来：“已知一个旋转矩阵 $R$，它对应的转轴和转角是多少？”（比如做动画插值、求相对旋转的时候都要用到）。

4.1 先求角度 $\theta$：利用矩阵的迹

矩阵的迹（trace，对角线元素之和）在旋转、镜像这类操作下有很好的性质。对罗德里格斯公式两边同时取迹：
$$\mathrm{tr}(R)=\mathrm{tr}(I)+(1-\cos \theta )\mathrm{tr}([u{]}^2)+\sin \theta \mathrm{tr}([u])$$
$[u]$ 是反对称矩阵，对角线全是 0，所以 $\mathrm{tr}([u])=0$；而 $[u{]}^2$ 可以证明满足：
$$[u{]}^{2}v=u\times (u\times v)=(u\cdot v)u-(u\cdot u)v=(u{u}^{\mathrm{T}}-I)v⟹[u{]}^2=u{u}^{\mathrm{T}}-I$$
（这里用到 $\Vert u\Vert =1$，即 $u\cdot u=1$）。因此 $\mathrm{tr}([u{]}^2)=\mathrm{tr}(u{u}^{\mathrm{T}})-\mathrm{tr}(I)=\Vert u{\Vert }^2-3=1-3=-2$。代入：
$$\mathrm{tr}(R)=3+(1-\cos \theta )(-2)=3-2(1-\cos \theta )=1+2\cos \theta$$
反解出 $\theta$：
$$\boxed{\theta =\arccos \left(\frac{\mathrm{tr}(R)-1}{2}\right)}$$

4.2 再求轴 $u$：利用矩阵的反对称部分

再看 $R$ 减去它自己的转置 $R^{\mathrm{T}}$。因为 $I$ 和 $[u{]}^2=u{u}^{\mathrm{T}}-I$ 都是对称矩阵（转置等于自身），所以它们在做"减转置"的时候会自动抵消掉；只有 $\sin \theta [u]$ 这一项是反对称的（转置变号），会被放大成两倍：
$$R-R^{\mathrm{T}}=2\sin \theta [u]$$
把这个等式按分量展开（利用 $[u]$ 的矩阵形式）,可以直接读出 $u$ 的三个分量：
$$\boxed{u=\frac{1}{2\sin \theta }\left(\begin{array}{c}{R}_{21}-R_{12}\\ R_{02}-R_{20}\\ R_{10}-R_{01}\end{array}\right)}$$
至此，"旋转矩阵 $\leftrightarrow$ 轴角"的正、反两个方向公式就都推完了。

图3（旋转矩阵与轴角互相转化的示意图）见文末所附 PDF，本质上就是上面这一套公式的图形化总结。

5. 指数映射：为什么罗德里格斯公式长这样并不是巧合

前面是从几何直觉一步步"拼"出罗德里格斯公式的，这一节从另一个角度——矩阵指数——重新推一遍，会发现殊途同归，而且能看出这背后更深的数学结构（李群与李代数）。
矩阵指数的定义和数字的指数函数泰勒展开完全类似：
$$\exp (A)=I+A+\frac{1}{2!}{A}^2+\frac{1}{3!}{A}^3+\cdots$$
我们断言：绕轴 $u$ 转角度 $\theta$ 的旋转矩阵，其实就等于反对称矩阵 $\theta [u]$ 的矩阵指数：
$$\boxed{R(u,\theta )=\exp (\theta [u])=\exp ([\mathbf{\theta }])}$$
（这里 $[\mathbf{\theta }]=\theta [u]$，就是把旋转向量 $\mathbf{\theta }=\theta u$ 对应的反对称矩阵）。为什么？关键在于反对称矩阵 $[u]$ 的幂具有周期性（因为 $\Vert u\Vert =1$）：
$$[u{]}^2=u{u}^{\mathrm{T}}-I,[u{]}^3=-[u],[u{]}^4=-[u{]}^2,[u{]}^5=[u],\dots$$
也就是说 $[u]$ 的幂每 4 次一循环，奇数次幂都是 $[u]$ 或 $-[u]$，偶数次幂都是 $[u{]}^2$ 或 $-[u{]}^2$。把这个规律代入指数的泰勒展开，按奇偶次幂分组：
$$\exp (\theta [u])=I+\theta [u]+\frac{{\theta }^2}{2!}[u{]}^2+\frac{{\theta }^3}{3!}[u{]}^3+\cdots$$
$$=I+\left(\theta -\frac{{\theta }^3}{3!}+\frac{{\theta }^5}{5!}-\cdots \right)[u]+\left(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots \right)[u{]}^2$$
括号里两串级数，正好分别是 $\sin \theta$ 和 $1-\cos \theta$ 的泰勒展开！所以：
$$\exp (\theta [u])=I+\sin \theta [u]+(1-\cos \theta )[u{]}^2$$
这正是第 3 节推出的罗德里格斯旋转公式。这不是巧合，而是因为反对称矩阵的集合构成了旋转矩阵群 $SO(3)$ 的李代数（记作 $\mathfrak{so}(3)$），而矩阵指数正是从李代数映射到对应的李群（也就是所有旋转矩阵组成的群）的指数映射（Exponential Map）。直观理解就是：反对称矩阵 $[u]$ 描述了"绕 $u$ 轴做无穷小旋转的速度"，把这个"瞬时旋转速度"沿着时间（角度）积分足够久（即取指数），就得到了真正转过 $\theta$ 角之后的旋转矩阵。

6. 用轴角做旋转插值：一个容易踩的坑

在做动画、机器人轨迹规划的时候，经常需要在两个已知姿态 $R_0$、$R_1$ 之间生成平滑过渡的中间姿态 $R_t$（$t\in [0,1]$，$t=0$ 时是 $R_0$，$t=1$ 时是 $R_1$）。轴角表示天然适合干这件事，但一定要按正确的方式来插值。

6.1 正确做法：相对旋转插值

思路是：先求出从 $R_0$"转到" $R_1$ 需要经过的相对旋转 $\Delta R$：
$$\Delta R=R_1{R}_0^{\mathrm{T}}$$
（$R_0^{\mathrm{T}}=R_0^{-1}$，所以 $\Delta R\cdot R_0=R_1$，即 $\Delta R$ 就是"接着在 $R_0$ 基础上再转一下就能得到 $R_1$"的那个旋转。）
再用第 4 节的方法，把 $\Delta R$ 转成轴角形式 $(\Delta \theta ,\Delta u)$。既然相对旋转本身就是"绕固定轴 $\Delta u$ 转 $\Delta \theta$ 角"，那插值就非常自然：只转一部分角度即可，转轴不变：
$${\theta }_t=t\Delta \theta ,\Delta R_t\leftarrow ({\theta }_t,\Delta u)\text{（用罗德里格斯公式算出对应旋转矩阵）}$$
最后把这一小段旋转叠加回初始姿态上：
$$R_t=\Delta R_t{R}_0$$
这样得到的 $R_t$ 就是沿着"从 $R_0$ 到 $R_1$ 的最短旋转路径"匀速转动的中间姿态——就像奶牛绕着一根固定的轴，匀速翻滚过去一样，是最自然的运动。

图4（$R_0\to R_1$ 沿最短路径依次翻滚的连续帧）见文末所附 PDF。

6.2 错误做法：直接对旋转向量做线性插值

有一种看起来"更省事"的想法：既然旋转向量 $\mathbf{\theta }=\theta u$ 就是一个普通的三维向量，那能不能直接对两个旋转向量线性插值？
$${\mathbf{\theta }}_t=(1-t){\mathbf{\theta }}_0+t{\mathbf{\theta }}_1$$
这是错的。举一个具体的反例（对应图2）：设
$${\mathbf{\theta }}_0=\frac{3\pi }{2}(1,0,0),{\mathbf{\theta }}_1=\frac{3\pi }{2}(0,1,0)$$
也就是说，姿态 0 是绕 $x$ 轴转了 $270°$，姿态 1 是绕 $y$ 轴转了 $270°$。这两个旋转向量的长度都是 $\frac{3\pi }{2}$，方向不同。如果直接线性插值 ${\mathbf{\theta }}_t$，它的模长会先从 $\frac{3\pi }{2}$ 减小（因为两个不同方向的向量线性组合，模长一般会先变短）再回到 $\frac{3\pi }{2}$，转轴也会不断跑偏，插值路径完全不是一条自然的旋转轨迹，中间会出现奇怪的、扭曲的姿态。
根本原因在于：旋转向量所在的空间只是局部近似欧几里得空间，旋转的合成是矩阵乘法（对应李群 $SO(3)$ 上的运算），而不是向量加法；只有转轴相同的旋转，角度才能直接相加减；转轴不同时，简单的线性插值完全不对应真实的旋转合成规律。而 6.1 节的"相对旋转插值"之所以正确，是因为它先用 $\Delta R=R_1{R}_0^{\mathrm{T}}$ 把两个姿态的差异统一表示成单一固定轴上的一次旋转，插值只发生在这个固定轴的角度上，天然满足旋转的合成规律，因此才能得到沿着 $SO(3)$ 上最短测地线的平滑插值。
（对应"图2"：左起第二格是错误的旋转向量线性插值，标了 ✗；第三格是正确的相对旋转插值，标了 ✓）
下面这张图总结了两种插值路径在流程上的区别：

7. 各种表示之间的关系全景图

把前面推出的所有公式串起来，可以画一张"旋转的不同表示法互相转换"的全景图：

8. C++ 完整实现

下面用 C++ 把上面推导的所有公式都实现一遍，包含：

1. 三维向量 Vec3 及基本运算（点乘、叉乘、模长、归一化）
2. 反对称矩阵 $[u]$ 的构造
3. 罗德里格斯公式正向计算：(u, theta) -> R
4. 反向计算（矩阵对数）：R -> (u, theta)
5. 相对旋转插值：给定 R0、R1，计算任意 t 处的中间姿态 R_t
   代码不依赖任何第三方库，只用标准库 <iostream>、<cmath>、<array>，可以直接用 g++ -std=c++17 main.cpp -o main 编译运行。

#include <iostream>   // 用于打印输出
#include <cmath>      // 用于 sin, cos, acos, sqrt 等数学函数
#include <array>       // 用于固定长度数组，存放矩阵/向量数据
#include <iomanip>    // 用于控制输出的小数位数
// ------------------------------------------------------------------
// 三维向量类：封装点乘、叉乘、模长、归一化这些基础运算，
// 后面所有的轴角公式都会反复用到这些运算。
// ------------------------------------------------------------------
struct Vec3 {
    double x, y, z;
    // 向量加法：逐分量相加
    Vec3 operator+(const Vec3& o) const { return {x + o.x, y + o.y, z + o.z}; }
    // 向量减法：逐分量相减
    Vec3 operator-(const Vec3& o) const { return {x - o.x, y - o.y, z - o.z}; }
    // 向量数乘：每个分量都乘以标量 s
    Vec3 operator*(double s) const { return {x * s, y * s, z * s}; }
    // 点乘（dot product）：对应公式里的 v·u，结果是一个标量，
    // 几何意义是 “v 在 u 方向上的投影长度” 乘以 “u 的模长”
    double dot(const Vec3& o) const { return x * o.x + y * o.y + z * o.z; }
    // 叉乘（cross product）：对应公式里的 u×v，结果仍是一个向量，
    // 方向垂直于 u 和 v 所在的平面，这是罗德里格斯公式的核心运算
    Vec3 cross(const Vec3& o) const {
        return {
            y * o.z - z * o.y,
            z * o.x - x * o.z,
            x * o.y - y * o.x
        };
    }
    // 模长：向量自身点乘再开方，即 ||v|| = sqrt(v·v)
    double norm() const { return std::sqrt(dot(*this)); }
    // 归一化：除以模长，得到方向相同、长度为1的单位向量
    // （轴角表示里的旋转轴 u 必须是单位向量，所以要用到这个函数）
    Vec3 normalized() const {
        double n = norm();
        return {x / n, y / n, z / n};
    }
};
// ------------------------------------------------------------------
// 用一个 3x3 的二维数组来表示矩阵，行优先存储：M[row][col]
// ------------------------------------------------------------------
using Mat3 = std::array<std::array<double, 3>, 3>;
// 返回 3x3 单位矩阵 I
Mat3 identity() {
    return {{ {1,0,0}, {0,1,0}, {0,0,1} }};
}
// 矩阵乘法：C = A * B，按照标准的矩阵乘法定义逐元素计算
Mat3 matMul(const Mat3& A, const Mat3& B) {
    Mat3 C{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 3; ++k)
                sum += A[i][k] * B[k][j];
            C[i][j] = sum;
        }
    return C;
}
// 矩阵加法：C = A + B，逐元素相加
Mat3 matAdd(const Mat3& A, const Mat3& B) {
    Mat3 C{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            C[i][j] = A[i][j] + B[i][j];
    return C;
}
// 矩阵数乘：每个元素都乘以标量 s，对应公式里的 (1-cos theta)*[u]^2 这种项
Mat3 matScale(const Mat3& A, double s) {
    Mat3 C{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            C[i][j] = A[i][j] * s;
    return C;
}
// 矩阵转置：R^T，用于第 6 节计算相对旋转 Delta R = R1 * R0^T
Mat3 transpose(const Mat3& A) {
    Mat3 C{};
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            C[i][j] = A[j][i];
    return C;
}
// 矩阵的迹（trace）：对角线元素之和，用于第 4.1 节反解角度 theta
double trace(const Mat3& A) {
    return A[0][0] + A[1][1] + A[2][2];
}
// ------------------------------------------------------------------
// 构造反对称矩阵 [u]，对应第 3 节公式：
//     [u] = | 0    -uz   uy |
//           | uz    0   -ux |
//           |-uy   ux    0  |
// 满足性质：对任意向量 v，都有 [u]*v == u×v（叉乘等价于矩阵乘法）
// ------------------------------------------------------------------
Mat3 skew(const Vec3& u) {
    return {{
        { 0.0,  -u.z,   u.y },
        { u.z,   0.0,  -u.x },
        {-u.y,   u.x,   0.0 }
    }};
}
// ------------------------------------------------------------------
// 罗德里格斯旋转公式（正向）：已知旋转轴 u（会自动归一化）和角度 theta，
// 计算旋转矩阵 R(u, theta) = I + (1-cos theta)*[u]^2 + sin(theta)*[u]
// 这一步直接对应第 3 节推出的矩阵形式公式。
// ------------------------------------------------------------------
Mat3 axisAngleToMatrix(Vec3 u, double theta) {
    u = u.normalized();              // 保证是单位向量，公式才成立
    Mat3 U = skew(u);                // 反对称矩阵 [u]
    Mat3 U2 = matMul(U, U);          // [u]^2 = [u] * [u]
    Mat3 term_sin = matScale(U, std::sin(theta));           // sin(theta) * [u]
    Mat3 term_cos = matScale(U2, 1.0 - std::cos(theta));    // (1-cos theta) * [u]^2
    // R = I + (1-cos theta)*[u]^2 + sin(theta)*[u]
    return matAdd(matAdd(identity(), term_cos), term_sin);
}
// ------------------------------------------------------------------
// 矩阵对数（反向）：已知旋转矩阵 R，反解出旋转轴 u 和角度 theta。
// 对应第 4 节推出的两个公式：
//   theta = arccos( (tr(R)-1) / 2 )
//   u = 1/(2 sin theta) * (R21-R12, R02-R20, R10-R01)
// 返回值通过引用参数 outU, outTheta 传出。
// ------------------------------------------------------------------
void matrixToAxisAngle(const Mat3& R, Vec3& outU, double& outTheta) {
    // 第一步：用迹求角度
    double cosTheta = (trace(R) - 1.0) / 2.0;
    // 数值上可能因为浮点误差略微超出 [-1,1]，这里做一下截断保护
    cosTheta = std::max(-1.0, std::min(1.0, cosTheta));
    double theta = std::acos(cosTheta);
    if (std::sin(theta) < 1e-9) {
        // theta 接近 0（几乎没转）或接近 pi 时，公式里的分母 sin(theta) 接近 0，
        // 会导致数值不稳定。theta≈0 时旋转轴无意义，这里给一个默认轴即可。
        outU = {1.0, 0.0, 0.0};
        outTheta = theta;
        return;
    }
    // 第二步：用 R - R^T = 2 sin(theta) [u] 反解轴
    double k = 1.0 / (2.0 * std::sin(theta));
    Vec3 u{
        k * (R[2][1] - R[1][2]),
        k * (R[0][2] - R[2][0]),
        k * (R[1][0] - R[0][1])
    };
    outU = u.normalized(); // 理论上已经是单位向量，归一化一下消除浮点误差
    outTheta = theta;
}
// ------------------------------------------------------------------
// 相对旋转插值：对应第 6.1 节的正确插值方法。
// 输入：起始姿态 R0、目标姿态 R1、插值参数 t（0到1之间）
// 输出：插值后的中间姿态 Rt
// 步骤：
//   1) 计算相对旋转 DeltaR = R1 * R0^T
//   2) 把 DeltaR 转成轴角形式 (Delta_u, Delta_theta)
//   3) 按比例 t 只转一部分角度：theta_t = t * Delta_theta
//   4) 用 (Delta_u, theta_t) 重新算出小旋转矩阵 DeltaR_t
//   5) 叠加回初始姿态：Rt = DeltaR_t * R0
// ------------------------------------------------------------------
Mat3 interpolateRotation(const Mat3& R0, const Mat3& R1, double t) {
    Mat3 R0T = transpose(R0);
    Mat3 DeltaR = matMul(R1, R0T);           // Delta R = R1 * R0^T
    Vec3 deltaU;
    double deltaTheta;
    matrixToAxisAngle(DeltaR, deltaU, deltaTheta); // 提取相对旋转的轴角
    double thetaT = t * deltaTheta;                 // 按比例截取角度
    Mat3 DeltaRt = axisAngleToMatrix(deltaU, thetaT); // 重新构造该角度下的旋转矩阵
    return matMul(DeltaRt, R0);                     // Rt = DeltaR_t * R0
}
// 打印矩阵，方便在控制台查看结果，保留4位小数
void printMat(const Mat3& M, const std::string& name) {
    std::cout << name << " =\n";
    std::cout << std::fixed << std::setprecision(4);
    for (int i = 0; i < 3; ++i) {
        std::cout << "  ";
        for (int j = 0; j < 3; ++j)
            std::cout << std::setw(9) << M[i][j] << " ";
        std::cout << "\n";
    }
}
int main() {
    // ---------------------------------------------------------
    // 示例：验证正向公式 (u, theta) -> R 与反向公式 R -> (u, theta)
    // 是否互为逆运算
    // ---------------------------------------------------------
    Vec3 axis = {0.0, 0.0, 1.0};     // 绕 z 轴
    double angle = M_PI / 2.0;       // 转 90 度
    Mat3 R = axisAngleToMatrix(axis, angle);
    printMat(R, "R (绕z轴转90度)");
    Vec3 recoveredU;
    double recoveredTheta;
    matrixToAxisAngle(R, recoveredU, recoveredTheta);
    std::cout << "反解得到的轴 u = (" << recoveredU.x << ", "
              << recoveredU.y << ", " << recoveredU.z << ")\n";
    std::cout << "反解得到的角 theta(弧度) = " << recoveredTheta << "\n\n";
    // ---------------------------------------------------------
    // 示例：复现"图2"里的插值对比场景
    //   R0 是绕 x 轴转 3*pi/2，R1 是绕 y 轴转 3*pi/2
    //   用相对旋转插值的方法，输出 t = 0, 0.2, 0.5, 0.8, 1 处的姿态
    // ---------------------------------------------------------
    Vec3 xAxis = {1.0, 0.0, 0.0};
    Vec3 yAxis = {0.0, 1.0, 0.0};
    double bigAngle = 3.0 * M_PI / 2.0;
    Mat3 R0 = axisAngleToMatrix(xAxis, bigAngle);
    Mat3 R1 = axisAngleToMatrix(yAxis, bigAngle);
    double ts[] = {0.0, 0.2, 0.5, 0.8, 1.0};
    for (double t : ts) {
        Mat3 Rt = interpolateRotation(R0, R1, t);
        printMat(Rt, "R(t=" + std::to_string(t) + ")");
    }
    return 0;
}

代码关键点说明

• Vec3::cross 实现的就是数学上的叉乘 $u\times v$，是罗德里格斯公式里出现次数最多的运算。
• skew(u) 构造的反对称矩阵 $[u]$ 满足 $[u]v=u\times v$，这是把"叉乘"这种向量运算改写成"矩阵乘法"的关键桥梁，后面才能把旋转写成一个矩阵 $R(u,\theta )$。
• axisAngleToMatrix 就是第 3 节推出的公式 $R=I+(1-\cos \theta )[u{]}^2+\sin \theta [u]$ 的直接翻译，三行代码对应公式里的三项，逐项相加。
• matrixToAxisAngle 是第 4 节的反向公式：先用迹求角度（trace(R)），再用 $R-R^{\mathrm{T}}$ 的三个独立分量求轴。这里额外加了一个 sin(theta) 太小时的保护，因为公式里有除以 $\sin \theta$ 的操作，角度太接近 0 或 $\pi$ 时数值上会不稳定，这是实际写代码时经常会漏掉的细节。
• interpolateRotation 完整实现了第 6.1 节的"相对旋转插值"：先求 $\Delta R=R_1{R}_0^{\mathrm{T}}$，转成轴角后按比例 $t$ 截取角度，再变回矩阵并叠加回 $R_0$，输出的就是沿最短路径旋转的中间姿态，而不会出现第 6.2 节里"直接线性插值旋转向量"那种扭曲、抄近路失败的问题。

附：图片与文档对应关系

• 图1（旋转向量的几何分解：$v$、$v_{\parallel }$、$v_{\perp }$、$u$、$w$、$v_{\text{rot}}$ 之间的空间关系）——对应本文第 2 节，具体图形见附带 PDF 第 1 页。
• 图2（两种旋转插值方式的流程与结果对比：错误的旋转向量线性插值 vs 正确的相对旋转插值）——对应本文第 6 节，具体图形见附带 PDF 第 2 页。
  两张图均已绘制为矢量图 PDF：轴角表示_图解.pdf，可放大查看不失真。

四元数（Quaternion）从零理解

1. 引子：从二维复数旋转，想到三维怎么办

先回顾一个很熟悉的技巧：在二维平面上，如果把点 $(x,y)$ 看成一个复数 $z=x+iy$，那么把 $z$ 乘以一个模长为 1 的复数 $\cos \theta +i\sin \theta$，得到的新复数
$$z^{\prime }=(\cos \theta +i\sin \theta )z$$
正好就是把点 $(x,y)$ 绕原点逆时针转过角度 $\theta$。这件事也可以完全写成矩阵形式：把复数 $c=a+ib$ 看成向量 $(a,b)$，复数乘法 $cz$ 展开后正好等于
$$cz=(ax-by)+i(bx+ay)⟺[c]z=\left(\begin{array}{cc}a& -b\\ b& a\end{array}\right)\left(\begin{array}{c}x\\ y\end{array}\right)=\left(\begin{array}{c}ax-by\\ bx+ay\end{array}\right)$$
也就是说，复数乘法本质上就是一种特殊的矩阵乘法。更进一步，虚数单位 $i$ 自己也有周期性：
$$i,i^2=-1,i^3=-i,i^4=1,\dots$$
对应的矩阵 $[i]=\left(\begin{array}{cc}0& -1\\ 1& 0\end{array}\right)$ 同样满足 $[i{]}^2=-I,[i{]}^3=-[i],[i{]}^4=I$。利用这个周期性对指数函数 $\exp (\theta [i])$ 按泰勒展开分组求和，正好能得到欧拉公式：
$$\exp (\theta [i])=\sum _k\frac{1}{k!}{\theta }^k[i{]}^k=\cos \theta I+\sin \theta [i]=\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)⟺e^{i\theta }=\cos \theta +i\sin \theta$$
这套"用一个数/矩阵的乘法来表示旋转"的思路，在二维里靠复数就完全够用了。那三维旋转能不能也找一种类似复数的"数"，让旋转变成乘法呢？ 答案是肯定的，这种"数"就是本文要讲的四元数。
复数是在实数的基础上，多加了一个满足 $i^2=-1$ 的虚数单位。而四元数则是更进一步，一口气加入三个互相关联的虚数单位 $i,j,k$，用来同时编码三维空间里的旋转轴方向和转动角度。

2. 四元数的定义

一个四元数由 1 个实数部分和 3 个虚数部分组成，写成：
$$q=w+xi+yj+zk,w,x,y,z\in \mathbb{R}$$
也可以写成更紧凑的"标量+向量"形式，把 $w$ 叫做标量部分，把 $(x,y,z)$ 打包成一个三维向量 $\mathbf{v}$，叫做向量部分：
$$q=(w,\mathbf{v}),\mathbf{v}=(x,y,z)$$
三个虚数单位 $i,j,k$ 满足下面这套乘法规则（这是四元数的"灵魂"，是 1843 年哈密顿（Hamilton）刻在都柏林一座桥上的著名公式）：
$$i^2=j^2=k^2=ijk=-1$$
从这一条核心规则出发，可以推出 $i,j,k$ 两两相乘的完整规则：

乘法	结果	乘法	结果
$i\cdot i$	$-1$	$j\cdot i$	$-k$
$j\cdot j$	$-1$	$k\cdot j$	$-i$
$k\cdot k$	$-1$	$i\cdot k$	$-j$
$i\cdot j$	$k$	$j\cdot k$	$i$
$k\cdot i$	$j$

---

关键点：$i\cdot j=k$ 但 $j\cdot i=-k$，两者不相等！这说明四元数乘法不满足交换律（$q_1{q}_2\ne q_2{q}_1$，一般情况下）。这一点和矩阵乘法很像（矩阵乘法通常也不满足交换律），但和复数乘法、实数乘法不同（复数乘法是满足交换律的）。后面写代码的时候一定要记住乘法顺序不能随便换。

3. 四元数乘法：Hamilton 积

有了 $i,j,k$ 的乘法表，两个四元数 $q_1=w_1+x_{1}i+y_{1}j+z_{1}k$ 与 $q_2=w_2+x_{2}i+y_{2}j+z_{2}k$ 相乘，只要像多项式乘法一样逐项展开，再用乘法表化简，就能得到结果（这个乘积叫做 Hamilton 积）。逐项展开会得到 16 项，整理后按 $1,i,j,k$ 分类，结果是：
$$q_1{q}_2=(w_1{w}_2-x_1{x}_2-y_1{y}_2-z_1{z}_2)+(w_1{x}_2+x_1{w}_2+y_1{z}_2-z_1{y}_2)i$$
$$+(w_1{y}_2-x_1{z}_2+y_1{w}_2+z_1{x}_2)j+(w_1{z}_2+x_1{y}_2-y_1{x}_2+z_1{w}_2)k$$
这个式子直接背下来会很痛苦，但如果换成"标量+向量"的写法，会发现它其实和向量的点乘、叉乘长得一模一样。设 $q_1=(w_1,{\mathbf{v}}_1)$，$q_2=(w_2,{\mathbf{v}}_2)$，则：
$$\boxed{q_1{q}_2=(w_1{w}_2-{\mathbf{v}}_1\cdot {\mathbf{v}}_2,w_1{\mathbf{v}}_2+w_2{\mathbf{v}}_1+{\mathbf{v}}_1\times {\mathbf{v}}_2)}$$
这是一个非常重要的公式，后面推导旋转公式全靠它。可以这样理解每一项的来源：

• 标量部分的 $-{\mathbf{v}}_1\cdot {\mathbf{v}}_2$：来自 $i\cdot i=j\cdot j=k\cdot k=-1$，两个向量部分对应分量相乘时都会带出一个负号，累加起来正好是负的点积。
• 向量部分的 ${\mathbf{v}}_1\times {\mathbf{v}}_2$：来自 $i\cdot j=k,j\cdot k=i,k\cdot i=j$ 这种"错位相乘"，正好是叉乘的定义。
• $w_1{\mathbf{v}}_2+w_2{\mathbf{v}}_1$：标量和向量部分交叉相乘，各自保持原方向不变。

4. 共轭、模长、逆

和复数的共轭 $\overline{a+bi}=a-bi$ 类似，四元数的共轭是把向量部分取负号，标量部分不变：
$$q^{\ast }=(w,-\mathbf{v})=w-xi-yj-zk$$
用共轭乘以自己，可以验证结果一定是一个纯实数（这一点和复数 $z\bar{z}=|z{|}^2$ 也很像）：
$$q{q}^{\ast }=(w,\mathbf{v})(w,-\mathbf{v})=(w^2-\mathbf{v}\cdot (-\mathbf{v}),w(-\mathbf{v})+w\mathbf{v}+\mathbf{v}\times (-\mathbf{v}))=(w^2+\Vert \mathbf{v}{\Vert }^2,0)$$
这个实数的平方根就定义为四元数的模长：
$$\Vert q\Vert =\sqrt{w^2+x^2+y^2+z^2}$$
模长为 1 的四元数叫做单位四元数，这正是接下来用来表示旋转的对象。四元数的逆满足 $q{q}^{-1}=1$，一般公式是：
$$q^{-1}=\frac{q^{\ast }}{\Vert q{\Vert }^2}$$
特别地，如果 $q$ 本身就是单位四元数（$\Vert q\Vert =1$），逆就直接等于共轭：
$$q^{-1}=q^{\ast }(\text{仅当 }\Vert q\Vert =1\text{ 时成立})$$
这个简化在写旋转代码的时候非常重要——意味着做旋转的"逆运算"根本不需要真的去求逆矩阵那种复杂运算，只要把向量部分取个负号就行。

5. 用单位四元数表示旋转

核心思路（呼应第 1 节复数旋转的思路）：在二维里，“乘以 $\cos \theta +i\sin \theta$” 就是转 $\theta$ 角；在三维里，类比的做法是先构造这样一个单位四元数：
$$\boxed{q=\left(\cos \frac{\theta }{2},\sin \frac{\theta }{2}u\right)}$$
其中 $u$ 是单位旋转轴，$\theta$ 是旋转角度——注意这里角度用的是 $\theta /2$，先记住这个"减半"的细节，第 5.2 节会专门解释为什么必须是一半。
但和复数不同的是，三维旋转不能简单地"乘一下"就完事，必须用四元数两边夹逼向量才行。把待旋转的向量 $v=(v_x,v_y,v_z)$ 包装成一个标量部分为 0 的"纯四元数" $(0,v)$，旋转公式是：
$$\boxed{v_{\text{rot}}=qv{q}^{\ast }}$$
（对应"图1"：左侧是二维复数直接相乘实现旋转；右侧是三维里用四元数"两边夹逼"的方式实现旋转，两者是同一种思想在不同维度下的体现）

5.1 推导：$qv{q}^{\ast }$ 算出来到底是什么

这一步来验证一下：套用第 3 节的 Hamilton 积公式，把 $q=(c,su)$（记 $c=\cos \frac{\theta }{2},s=\sin \frac{\theta }{2}$）代入，算一算 $qv{q}^{\ast }$ 到底等于什么。
第一步，先算 $qv$（这里 $v=(0,\mathbf{v})$）：
$$qv=(c,su)(0,\mathbf{v})=(c\cdot 0-su\cdot \mathbf{v},c\mathbf{v}+0\cdot su+su\times \mathbf{v})=(-s(u\cdot \mathbf{v}),c\mathbf{v}+s(u\times \mathbf{v}))$$
第二步，把这个结果再乘以 $q^{\ast }=(c,-su)$：
$$(qv)q^{\ast }=(-s(u\cdot \mathbf{v}),c\mathbf{v}+s(u\times \mathbf{v}))(c,-su)$$
标量部分展开（利用 $(u\times \mathbf{v})\cdot u=0$，因为叉乘结果必然垂直于 $u$）：
$$\underbrace{-s(u\cdot \mathbf{v})\cdot c}+\underbrace{s[c\mathbf{v}+s(u\times \mathbf{v})]\cdot u}=-sc(u\cdot \mathbf{v})+sc(u\cdot \mathbf{v})+0=0$$
标量部分算出来正好是 0，这说明旋转后的结果依然是一个纯四元数（也就是依然对应一个普通的三维向量），这正是我们期望的——旋转不应该把"向量"变成别的什么怪东西。
向量部分展开后（利用向量三重积恒等式 $(A\times B)\times C=B(A\cdot C)-A(B\cdot C)$ 化简），合并同类项，最终结果是：
$${\mathbf{v}}_{\text{rot}}=(c^2-s^2)\mathbf{v}+2s^2(u\cdot \mathbf{v})u+2sc(u\times \mathbf{v})$$
再利用半角公式 $c^2-s^2=\cos \theta$、$2s^2=1-\cos \theta$、$2sc=\sin \theta$（这三个都是标准的二倍角公式，把 $\theta /2$ 代入 $\cos 2\alpha ={\cos }^2\alpha -{\sin }^2\alpha$ 等公式就能得到），代入后得到：
$$\boxed{{\mathbf{v}}_{\text{rot}}=\cos \theta \mathbf{v}+(1-\cos \theta )(u\cdot \mathbf{v})u+\sin \theta (u\times \mathbf{v})}$$
这正是本系列上一篇文档里推出的罗德里格斯旋转公式（Rodrigues’ rotation formula）！ 也就是说，"四元数夹逼运算"和"轴角表示的罗德里格斯公式"本质上是同一个旋转，只是用了两套不同的数学工具去描述同一件事情。这个巧合并不意外——它们都是描述三维旋转群 $SO(3)$ 的两种等价方式。

5.2 为什么角度要取一半

上面的推导直接解释了为什么四元数里用的是 $\theta /2$ 而不是 $\theta$：因为 $qv{q}^{\ast }$ 这个"两边夹"的运算里，$q$ 和 $q^{\ast }$ 各自贡献了一次半角的三角函数，两者相乘、相加组合之后，通过二倍角公式，才刚好凑出了完整角度 $\theta$ 的 $\cos \theta ,\sin \theta$。如果直接用 $q=(\cos \theta ,\sin \theta u)$（不减半），算出来的旋转角度就会变成 $2\theta$，是期望角度的两倍，这是初学时最容易犯的错误之一。

5.3 四元数与旋转矩阵的相互转换

如果需要把四元数换成大家更熟悉的 $3\times 3$ 旋转矩阵（比如要交给某个只接受矩阵输入的图形接口），可以把 $qv{q}^{\ast }$ 这个运算，分别代入三个基向量 $(1,0,0),(0,1,0),(0,0,1)$ 算一遍，拼成矩阵的三列，结果是：
$$R(q)=\left(\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right)$$
这里 $q=(w,x,y,z)$ 必须是单位四元数（$w^2+x^2+y^2+z^2=1$）这个公式才成立。

6. 双重覆盖：$q$ 与 $-q$ 表示同一个旋转

有一个四元数特有、容易让人困惑的性质：把 $q$ 换成 $-q=(-w,-x,-y,-z)$，代入旋转公式 $qv{q}^{\ast }$，结果完全不变：
$$(-q)v(-q{)}^{\ast }=(-1)qv{q}^{\ast }(-1)=qv{q}^{\ast }$$
（两个负号相乘抵消了）。也就是说，每一个三维旋转，都对应着两个相差一个符号的单位四元数，这叫做四元数的**双重覆盖（double cover）**性质。
（对应"图1"附图：球面上关于球心对称的两点 $q$ 与 $-q$，实际对应的是同一个旋转矩阵 $R$）
这个性质平时不会造成什么麻烦，但在做插值（下一节）的时候必须格外注意：如果不做处理，直接对 $q_0$ 和 $q_1$ 插值，有可能因为选错了"符号版本"而绕了远路。

7. 四元数的指数映射

回顾第 1 节，复数的旋转本质上是矩阵 $[i]$ 的指数映射 $\exp (\theta [i])$。四元数也有完全类似的结构。把纯四元数（标量部分为0）$\theta u=(0,\theta u)$ 代入四元数版本的指数函数定义：
$$\exp (0,\theta u)=\sum _{k=0}^{\infty }\frac{1}{k!}(0,\theta u{)}^k$$
由于 $(0,u)$ 自己乘自己：$(0,u)(0,u)=(-u\cdot u,u\times u)=(-1,0)=-1$（因为 $u$ 是单位向量），也就是说纯四元数 $(0,u)$ 的平方正好是 $-1$，和虚数单位 $i$ 的性质一模一样！于是同样可以按奇偶次幂分组，用 $\sin ,\cos$ 的泰勒级数化简，得到：
$$\boxed{\exp \left(\frac{\theta }{2}(0,u)\right)=\left(\cos \frac{\theta }{2},\sin \frac{\theta }{2}u\right)=q}$$
这和第 5 节里直接"拼凑"出来的旋转四元数 $q$ 完全一致。这说明单位四元数所在的集合，正是三维旋转群 $SO(3)$ 的一个"双重覆盖群"（称为 $SU(2)$ 或者自旋群 $\mathrm{Spin}(3)$）所对应的指数映射结果，和上一篇文档里旋转矩阵的指数映射 $R=\exp (\theta [u])$ 是同一套李群-李代数理论在不同表示下的体现。

8. 四元数插值：SLERP（球面线性插值）

8.1 为什么不能直接对四元数分量做线性插值

四元数的模长必须恒等于 1，才能代表一个合法的旋转。而四个分量 $(w,x,y,z)$ 全体，可以看成是嵌入在四维空间里的一个"单位超球面"上的点。如果像插值普通向量那样，直接对两个四元数的分量做线性插值：
$$q_t=(1-t)q_0+t{q}_1$$
得到的中间结果一般不再落在单位球面上（模长会小于 1），这就不是一个合法的旋转，还需要额外做一次归一化。即便归一化之后凑合能用，插值出来的旋转角速度也是不均匀的（中间快两头慢），运动效果不自然——这和上一篇文档里"直接线性插值旋转向量"踩的坑，本质上是同一个问题：把一个应该沿着弯曲空间（球面/旋转群）走的路径，错误地当成了平直空间里的直线来插值。

8.2 正确做法：沿球面测地线插值

正确的思路是：把 $q_0$ 与 $q_1$ 都看成四维单位球面上的点，插值应该沿着这两点之间的最短大圆弧（测地线）走，而不是直接连一条穿过球体内部的直线。这就是 SLERP（Spherical Linear intERPolation，球面线性插值）：
$$\boxed{\mathrm{slerp}(q_0,q_1,t)=\frac{\sin ((1-t)\Omega )}{\sin \Omega }{q}_0+\frac{\sin (t\Omega )}{\sin \Omega }{q}_1}$$
其中 $\Omega$ 是 $q_0$ 与 $q_1$ 之间的夹角，满足 $\cos \Omega =q_0\cdot q_1$（四个分量当成四维向量做点积）。
（对应"图2"：红色虚线是直接对四元数分量线性插值，会切入球体内部，模长小于1；蓝色弧线是SLERP，始终贴着球面走最短弧线）
这个公式可以这样理解：如果把 $q_0,q_1$ 想象成两个方向不同、长度都是 1 的"钟表指针"，$\mathrm{slerp}$ 就是让指针以均匀的角速度从指向 $q_0$ 转到指向 $q_1$，公式里的两个系数 $\sin ((1-t)\Omega )/\sin \Omega$ 和 $\sin (t\Omega )/\sin \Omega$ 保证了任意时刻 $t$ 处的结果都精确落在单位球面上（可以直接代入 $t=0$ 和 $t=1$ 验证，分别正好得到 $q_0$ 和 $q_1$）。

8.3 别忘了处理双重覆盖

结合第 6 节的双重覆盖性质：如果算出来 $\cos \Omega =q_0\cdot q_1$ 是负数，说明 $q_0$ 和 $q_1$ 这两个"版本"之间的夹角大于 $90°$，走的不是最短路线。这时候应该把 $q_1$ 换成它的等价版本 $-q_1$（代表同一个旋转，但夹角变成了 $180°-\Omega$，是更短的路），再做 SLERP，才能保证真的是"沿最短路径"插值。

8.4 交叉验证：SLERP 和"相对旋转插值"算出来是同一个结果

在上一篇轴角表示的文档里，我们用"求相对旋转 $\Delta R=R_1{R}_0^{\mathrm{T}}$，再对相对旋转的轴角按比例插值"的方法，算出了绕 $x$ 轴转 $\frac{3\pi }{2}$、绕 $y$ 轴转 $\frac{3\pi }{2}$ 这两个姿态之间、$t=0.5$ 处的旋转矩阵：
$$R(t=0.5)=\left(\begin{array}{ccc}0.6667& 0.3333& -0.6667\\ 0.3333& 0.6667& 0.6667\\ 0.6667& -0.6667& 0.3333\end{array}\right)$$
用本文的四元数 SLERP 方法对同样的两个姿态、同样的 $t=0.5$ 重新算一遍（构造 $q_0,q_1$，做 SLERP，再转回旋转矩阵），结果分毫不差，完全是同一个矩阵。这说明"轴角的相对旋转插值"和"四元数的SLERP"这两条看起来完全不同的路子，本质上算的是同一件事——都是沿着旋转群 $SO(3)$ 上的最短测地线在插值，只是用了不同的数学语言。

9. 各种旋转表示法的关系全景图（补充四元数）

在上一篇文档"旋转矩阵 $\leftrightarrow$ 轴角"关系图的基础上，把四元数也加进来：

10. C++ 完整实现

下面用 C++ 实现四元数的完整功能：构造、Hamilton 积、共轭、旋转向量、转旋转矩阵、以及 SLERP 插值。代码不依赖任何第三方库，只用标准库，可以直接用 g++ -std=c++17 quaternion_demo.cpp -o quaternion_demo 编译运行。

#include <iostream>   // 用于打印输出
#include <cmath>      // 用于 sin, cos, acos, sqrt 等数学函数
#include <array>       // 用于固定长度数组，存放旋转矩阵
#include <iomanip>    // 用于控制输出的小数位数
#include <algorithm>  // 用于 std::max, std::min 做数值截断保护
#include <string>
// ------------------------------------------------------------------
// 三维向量：只保留四元数运算里会用到的最基本功能
// ------------------------------------------------------------------
struct Vec3 {
    double x, y, z;
    double norm() const { return std::sqrt(x*x + y*y + z*z); }
    Vec3 normalized() const { double n = norm(); return {x/n, y/n, z/n}; }
};
using Mat3 = std::array<std::array<double, 3>, 3>;
// ------------------------------------------------------------------
// 四元数类：q = w + x*i + y*j + z*k，也可以写成标量+向量的形式
// q = (w, v)，其中 v=(x,y,z)。这是本文件的核心数据结构。
// ------------------------------------------------------------------
struct Quaternion {
    double w, x, y, z; // w 是标量部分，(x,y,z) 是向量部分
    // ----------------------------------------------------------
    // 四元数乘法（Hamilton 积）：这是四元数最核心、也最容易出错的运算，
    // 一定要按照 i*i=j*j=k*k=-1, i*j=k, j*k=i, k*i=j 这套规则展开。
    // 公式来源：把 q1=(w1,v1), q2=(w2,v2) 看成"标量+向量"，
    //   q1*q2 = (w1*w2 - v1·v2,  w1*v2 + w2*v1 + v1×v2)
    // 这里直接展开成分量形式，避免再调用向量类，方便阅读。
    // ----------------------------------------------------------
    Quaternion operator*(const Quaternion& o) const {
        return {
            w*o.w - x*o.x - y*o.y - z*o.z,   // 新的标量部分：w1w2 - v1·v2
            w*o.x + x*o.w + y*o.z - z*o.y,   // 新的 i 分量：来自 w1*v2 + w2*v1 + v1×v2 的 x 分量
            w*o.y - x*o.z + y*o.w + z*o.x,   // 新的 j 分量
            w*o.z + x*o.y - y*o.x + z*o.w    // 新的 k 分量
        };
    }
    // 四元数加法：用于插值时对分量做加权求和（SLERP 内部会用到）
    Quaternion operator+(const Quaternion& o) const {
        return {w+o.w, x+o.x, y+o.y, z+o.z};
    }
    // 四元数数乘：每个分量都乘以标量 s
    Quaternion operator*(double s) const {
        return {w*s, x*s, y*s, z*s};
    }
    // 共轭：标量部分不变，向量部分取负号。
    // 对单位四元数来说，共轭正好等于它的逆（q* = q^{-1}），
    // 这也是为什么旋转公式里可以直接用共轭代替求逆。
    Quaternion conjugate() const { return {w, -x, -y, -z}; }
    // 模长：四个分量看成一个四维向量，取欧几里得范数
    double norm() const { return std::sqrt(w*w + x*x + y*y + z*z); }
    // 归一化：除以模长，得到单位四元数（旋转必须用单位四元数表示）
    Quaternion normalized() const {
        double n = norm();
        return {w/n, x/n, y/n, z/n};
    }
    // 四个分量之间的点积，SLERP 里用它计算两个四元数的夹角余弦
    double dot(const Quaternion& o) const {
        return w*o.w + x*o.x + y*o.y + z*o.z;
    }
};
// ------------------------------------------------------------------
// 从轴角 (u, theta) 构造单位四元数：
//     q = (cos(theta/2),  sin(theta/2) * u)
// 注意角度要除以 2，这是四元数表示旋转的一个关键细节
// （因为 q 和 -q 表示同一个旋转，转一整圈 2*pi 对应四元数只转半圈）。
// ------------------------------------------------------------------
Quaternion fromAxisAngle(Vec3 u, double theta) {
    u = u.normalized();               // 旋转轴必须是单位向量
    double half = theta / 2.0;
    double s = std::sin(half);
    return { std::cos(half), s*u.x, s*u.y, s*u.z };
}
// ------------------------------------------------------------------
// 用四元数旋转一个三维向量 v：
//     v' = q * v * q^{-1} = q * v * q*   （单位四元数时 q^{-1}=q*）
// 这里把 v 看成一个"纯四元数"（标量部分为0）：v_quat = (0, v)
// ------------------------------------------------------------------
Vec3 rotateVector(const Quaternion& q, const Vec3& v) {
    Quaternion vQuat{0.0, v.x, v.y, v.z};      // 把普通向量包装成纯四元数
    Quaternion qConj = q.conjugate();          // 单位四元数的共轭 = 逆
    Quaternion result = q * vQuat * qConj;     // 三个四元数连乘，"夹逼"运算
    // 理论上 result 的标量部分应该严格等于 0（旋转后仍然是纯向量），
    // 这里只取向量部分返回即可。
    return { result.x, result.y, result.z };
}
// ------------------------------------------------------------------
// 把单位四元数转换成对应的 3x3 旋转矩阵，公式为：
//   R = | 1-2(y^2+z^2)   2(xy-wz)      2(xz+wy)    |
//       | 2(xy+wz)       1-2(x^2+z^2)  2(yz-wx)    |
//       | 2(xz-wy)       2(yz+wx)      1-2(x^2+y^2)|
// 这是把 q*v*q* 这个运算，对每个基向量 (1,0,0)(0,1,0)(0,0,1) 分别算一遍，
// 拼出来的矩阵形式，效果和直接调用 rotateVector 完全一样，
// 只是转成矩阵后可以配合其它需要矩阵形式的代码（比如渲染管线）使用。
// ------------------------------------------------------------------
Mat3 toRotationMatrix(const Quaternion& q) {
    double w=q.w, x=q.x, y=q.y, z=q.z;
    Mat3 R;
    R[0][0] = 1 - 2*(y*y + z*z);  R[0][1] = 2*(x*y - w*z);      R[0][2] = 2*(x*z + w*y);
    R[1][0] = 2*(x*y + w*z);      R[1][1] = 1 - 2*(x*x + z*z);  R[1][2] = 2*(y*z - w*x);
    R[2][0] = 2*(x*z - w*y);      R[2][1] = 2*(y*z + w*x);      R[2][2] = 1 - 2*(x*x + y*y);
    return R;
}
// ------------------------------------------------------------------
// SLERP（球面线性插值，Spherical Linear intERPolation）：
// 在两个单位四元数 q0、q1 之间，沿着四维单位球面上的最短测地线
// 插值出参数 t 处的中间旋转。公式：
//   slerp(q0,q1,t) = [sin((1-t)*Omega)/sin(Omega)] * q0
//                   + [sin(t*Omega)/sin(Omega)]     * q1
// 其中 cos(Omega) = q0 · q1（四个分量的点积）。
// 由于 q 和 -q 表示同一个旋转（双重覆盖），如果点积是负的，
// 说明两个四元数"分别指向了球面上相反的方向"，要先把 q1 取反，
// 才能保证插值走的是"最短"路径，否则会绕远路。
// ------------------------------------------------------------------
Quaternion slerp(const Quaternion& q0, Quaternion q1, double t) {
    double cosOmega = q0.dot(q1);
    // 双重覆盖处理：点积为负说明夹角大于90度，走了"远路"，
    // 取 -q1 可以得到一个方向相同、但夹角更小的等价四元数
    if (cosOmega < 0.0) {
        q1 = q1 * (-1.0);
        cosOmega = -cosOmega;
    }
    // 数值截断保护，避免 acos 输入超出 [-1,1] 导致 NaN
    cosOmega = std::max(-1.0, std::min(1.0, cosOmega));
    const double EPS = 1e-6;
    if (cosOmega > 1.0 - EPS) {
        // 两个四元数几乎重合（夹角接近0），sin(Omega)接近0会导致除0，
        // 这种情况下直接退化成线性插值再归一化即可，误差可忽略
        Quaternion result = q0 * (1.0 - t) + q1 * t;
        return result.normalized();
    }
    double omega = std::acos(cosOmega);         // 两个四元数之间的夹角
    double sinOmega = std::sin(omega);
    double coeff0 = std::sin((1.0 - t) * omega) / sinOmega;
    double coeff1 = std::sin(t * omega) / sinOmega;
    return q0 * coeff0 + q1 * coeff1;
}
// 打印矩阵，保留4位小数，方便核对结果
void printMat(const Mat3& M, const std::string& name) {
    std::cout << name << " =\n" << std::fixed << std::setprecision(4);
    for (int i = 0; i < 3; ++i) {
        std::cout << "  ";
        for (int j = 0; j < 3; ++j) std::cout << std::setw(9) << M[i][j] << " ";
        std::cout << "\n";
    }
}
void printQuat(const Quaternion& q, const std::string& name) {
    std::cout << std::fixed << std::setprecision(4)
              << name << " = (w=" << q.w << ", x=" << q.x
              << ", y=" << q.y << ", z=" << q.z << ")\n";
}
int main() {
    // ---------------------------------------------------------
    // 示例1：验证"四元数旋转向量"与"罗德里格斯公式算出的旋转矩阵旋转向量"
    // 结果完全一致 —— 这正是第 5.1 节推导的核心结论
    // ---------------------------------------------------------
    Vec3 axis = {0.0, 0.0, 1.0};       // 绕 z 轴
    double angle = M_PI / 2.0;         // 转 90 度
    Quaternion q = fromAxisAngle(axis, angle);
    printQuat(q, "q (绕z轴转90度)");
    Vec3 v = {1.0, 0.0, 0.0};          // 待旋转的向量
    Vec3 vRot = rotateVector(q, v);
    std::cout << "四元数旋转结果 v' = (" << vRot.x << ", " << vRot.y << ", " << vRot.z << ")\n";
    Mat3 R = toRotationMatrix(q);
    printMat(R, "由四元数转换得到的旋转矩阵 R");
    std::cout << "\n";
    // ---------------------------------------------------------
    // 示例2：SLERP 插值演示，复现"绕x轴转270度" -> "绕y轴转270度"
    // 的插值过程，输出 t=0,0.2,0.5,0.8,1 处的四元数
    // ---------------------------------------------------------
    Vec3 xAxis = {1.0, 0.0, 0.0};
    Vec3 yAxis = {0.0, 1.0, 0.0};
    double bigAngle = 3.0 * M_PI / 2.0;
    Quaternion q0 = fromAxisAngle(xAxis, bigAngle);
    Quaternion q1 = fromAxisAngle(yAxis, bigAngle);
    double ts[] = {0.0, 0.2, 0.5, 0.8, 1.0};
    for (double t : ts) {
        Quaternion qt = slerp(q0, q1, t);
        std::cout << "t=" << t << "  ";
        printQuat(qt, "q_t");
        std::cout << "  模长 = " << qt.norm() << " (应始终为1)\n";
    }
    return 0;
}

代码关键点说明

• Quaternion::operator* 是整份代码里最核心的函数，实现的就是第 3 节推出的 Hamilton 积公式。四行返回值分别对应新四元数的 $w,x,y,z$ 四个分量，每一项都能在 $q_1{q}_2=(w_1{w}_2-{\mathbf{v}}_1\cdot {\mathbf{v}}_2,w_1{\mathbf{v}}_2+w_2{\mathbf{v}}_1+{\mathbf{v}}_1\times {\mathbf{v}}_2)$ 里找到对应的来源，写代码时最容易出错的就是符号和顺序，建议对照这个公式逐项核对。
• conjugate() 只是把 $x,y,z$ 取负号，非常简单，但它承担了"单位四元数求逆"的全部工作——这是四元数比矩阵求逆方便得多的地方（矩阵求逆通常复杂得多，四元数求逆对单位四元数来说只是变个符号）。
• rotateVector 直接翻译了 $v_{\text{rot}}=qv{q}^{\ast }$ 这个"夹逼"公式：先把向量包装成纯四元数（标量部分补 0），做两次 Hamilton 积，再把结果的向量部分取出来。
• toRotationMatrix 把四元数转成矩阵，是第 5.3 节公式的直接翻译，如果后续代码只认矩阵（比如某些图形库的接口），可以用这个函数做桥接。
• slerp 完整实现了第 8 节的球面线性插值：先算点积判断是否要处理双重覆盖（if (cosOmega < 0.0) 这一段），再处理夹角接近 0 时除以 sinOmega 可能出现的数值不稳定（if (cosOmega > 1.0 - EPS) 这一段退化成线性插值），最后套用标准 SLERP 公式算出结果。这几个边界情况的处理，在实际项目里都是容易被漏掉、导致偶尔"抽风"的地方，务必都要考虑到。
  代码里的两个示例分别验证了：第一，四元数旋转和上一篇文档里罗德里格斯公式算出的旋转矩阵完全等价；第二，用四元数 SLERP 在同样的两个姿态之间插值，$t=0.5$ 时刻算出的矩阵，和上一篇文档里"轴角相对旋转插值"算出的矩阵分毫不差，这两种方法本质上是同一条最短旋转路径的两种不同实现方式。

附：图片与文档对应关系

• 图1（复数旋转与四元数"夹逼"旋转的类比图，以及双重覆盖 $q$、$-q$ 对应同一旋转的示意）——对应本文第 5、6 节，具体图形见附带 PDF 第 1、2 页。
• 图2（SLERP 球面插值 vs 直接线性插值对比图）——对应本文第 8 节，具体图形见附带 PDF 第 3 页。
  图形均已绘制为矢量图 PDF：四元数_图解.pdf，可放大查看不失真。