基础数学回顾：向量与矩阵的“肌肉记忆”
说实话，每次带新人做机器人项目，我都要先问一句：“向量点积和叉积，你分得清吗？”
不是故意刁难。而是坐标变换这玩意儿，说白了就是向量和矩阵的舞蹈。你如果连舞步都踩不准，后面学旋转、学四元数，那真是寸步难行。
这一节，咱们就把这些“基本功”彻底捋一遍。我尽量用我踩过的坑，帮你把路铺平。
1. 向量基础：点积与叉积
向量是什么？有方向、有大小。在三维空间里，我们通常用 (x, y, z) 表示。但真正干活的时候，你得理解它的“灵魂”——点积和叉积。
1.1 点积：投影的度量
点积的公式很简单：a · b = |a| |b| cosθ。结果是一个标量。
我个人的习惯是，把点积理解为“一个向量在另一个向量方向上的投影长度，再乘以另一个向量的长度”。
在实际项目中，点积最常用的场景就是判断两个向量的方向关系：
- 同向：点积 > 0
- 垂直：点积 = 0
- 反向：点积 < 0
避坑指南： 我曾经在写机械臂逆解算法时，忘了归一化向量就直接算点积，结果角度算出来全是错的。记住，点积公式里的 cosθ 是基于单位向量的。如果你要算夹角，先把向量归一化！
代码实现也很直接：
// 三维向量点积
float dotProduct(Vector3 a, Vector3 b) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
// 计算夹角（弧度）
float angleBetween(Vector3 a, Vector3 b) {
float dot = dotProduct(a, b);
float magProduct = magnitude(a) * magnitude(b);
return acos(dot / magProduct);
}1.2 叉积：法向量的生成器
叉积 a × b 的结果是一个向量。这个向量垂直于 a 和 b 所在的平面。方向由右手定则决定。
你想想看，在计算机图形学里，我们要算一个平面的法向量，靠的就是叉积。在机器人学里，我们要算工具坐标系的姿态，也离不开它。
叉积的几何意义是：结果向量的模长等于 a 和 b 构成的平行四边形的面积。
我的小技巧： 如果你记不住叉积的公式，可以用行列式来记。把 i, j, k 放在第一行，a 的分量放第二行，b 的分量放第三行，然后算行列式。这个方法我用了十年，从来没出过错。
// 三维向量叉积
Vector3 crossProduct(Vector3 a, Vector3 b) {
return Vector3(
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x
);
}2. 矩阵基础：乘法与转置
矩阵在坐标变换里，就是“变换”本身。一个旋转矩阵、一个平移矩阵，本质上都是在描述“如何把点从一个坐标系搬到另一个坐标系”。
2.1 矩阵乘法：变换的串联
矩阵乘法的规则是：C = A × B，其中 C 的第 i 行第 j 列元素，等于 A 的第 i 行与 B 的第 j 列的点积。
这里有个关键点：矩阵乘法不满足交换律。也就是说，A × B ≠ B × A。
我记得有一次调试一个视觉伺服系统，发现机械臂总是往反方向跑。查了半天，原来是旋转矩阵和平移矩阵的乘法顺序写反了。先旋转再平移，和先平移再旋转，结果天差地别。
重要提醒： 在计算机图形学和机器人学中，我们通常使用“左乘”还是“右乘”？这取决于你的坐标系约定。我个人习惯用列向量左乘矩阵（即 v' = M * v），这样变换顺序是从右向左读的。如果你用行向量右乘，顺序就反了。一定要统一！
// 4x4 矩阵乘法（齐次坐标常用）
Matrix4x4 multiply(Matrix4x4 a, Matrix4x4 b) {
Matrix4x4 result;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
result.data[i][j] = 0;
for (int k = 0; k < 4; k++) {
result.data[i][j] += a.data[i][k] * b.data[k][j];
}
}
}
return result;
}2.2 矩阵转置：行与列的互换
转置就是把矩阵的行变成列，列变成行。记作 A^T。
转置在坐标变换里有什么用？
- 正交矩阵的逆等于转置：旋转矩阵是正交矩阵，所以
R^{-1} = R^T。这意味着求逆运算变成了简单的转置，性能提升巨大。 - 向量点积的矩阵形式：
a · b = a^T * b（当 a 和 b 是列向量时）。
实战经验： 我在做点云配准（ICP算法）时，经常需要计算旋转矩阵的逆。如果直接用高斯消元法求逆，效率很低。但我知道旋转矩阵是正交的，所以直接转置就搞定了。这个优化，能让你的算法快一个数量级。
3. 三角函数回顾
三角函数在坐标变换里无处不在。旋转矩阵里的 sin 和 cos，欧拉角里的各种组合，都离不开它。
我建议你重点掌握这几个：
| 函数 | 含义 | 常见陷阱 |
|---|---|---|
| sin(θ), cos(θ) | 角度 θ 的正弦和余弦 | 注意角度制 vs 弧度制！C/C++ 的数学库默认弧度 |
| atan2(y, x) | 计算点 (x, y) 的方位角，范围 (-π, π] | 比 atan(y/x) 更安全，能处理 x=0 的情况 |
| acos(x) | 反余弦，返回 [0, π] 的弧度 | 输入必须在 [-1, 1] 内，否则返回 NaN |
我的习惯： 在代码里，我永远用 atan2(y, x) 而不是 atan(y/x)。为什么？因为 atan 无法区分 (1, 1) 和 (-1, -1) 这两个点，而 atan2 可以。这个坑，我刚开始做机器人时踩过好几次。
另外，三角恒等式也很重要。比如 sin²θ + cos²θ = 1，这个在推导旋转矩阵的逆时经常用到。
知识体系总览
下面这张图，是我自己梳理的本章知识结构。你可以把它当作一个“思维导图”来用：
嗯，这一章的内容就到这里。数学这东西，光看是没用的。我建议你打开编辑器，把上面的代码敲一遍，然后自己改几个参数试试。只有亲手写过，这些公式才会变成你的“肌肉记忆”。