绕任意轴旋转, 绕任意直线旋转矩阵推导

问题描述：

已知空间中的任意轴axis(u,v,w)，点v绕该轴旋转$\theta$后得到点v’，求满足条件的矩阵$R_\theta$，使得有
$v’ = R_\theta v$

思路1:
先将旋转到z轴，然后引用绕z轴旋转公式，再将该轴再旋转回原来的地方。
思路2：
矩阵的本质是线性变换，我们通过几何方法对三个轴分别写出三个线性变换的表达式，再将其拼接成矩阵。

这里我们讲一下思路1，思路2见3D Math Primer for Graphics and Game Development, 2nd Edition，不管采取哪种方法，得到的结果肯定是一样的。

众所周知，把一头大象关进冰箱需要三步
1.打开冰箱门
2.把大象放进去
3.关闭冰箱门

于是按思路一来说的话，把一个点绕任意轴旋转也可以简单的分成三步
1.任意轴旋转至z轴
2.使用绕z轴旋转矩阵旋转$\theta$
3.任意轴旋转回原来位置

使用公式表达的话，便有$v’ = R^{-1}{to-x}R_zR{to-x} v$

即我们把$R_\theta$拆分成了三部分

$R_\theta = R^{-1}{to-x}R_zR{to-x}$

其中绕z轴旋转的矩阵我们是知道的

$$
R_\theta=
\begin{bmatrix}
cos\theta & -sin\theta & 0 & 0\\
sin\theta & cos\theta & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

emmm，如果你不知道这个式子怎么来的话，推导大致是从二维旋转升到三维。二维的推导可以参考其他的blog或任意一本图形学教材。搜索旋转矩阵就行，大概是由三角和差公式推出来的。

现在问题就转化成了如果将任意轴旋转至z轴了。

这个问题我们也可以分两步走
1.任意轴先 绕z轴旋转到xz平面(标记该矩阵为$R_1$)
2.再 绕y轴旋转到与z轴重合(标记该矩阵为$R_2$)

于是我们可以很方便的写出$R_1$,$R_2$

$$
R_1=
\begin{bmatrix}
cos\alpha & -sin\alpha & 0 & 0\\
sin\alpha & cos\alpha & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

$$
R_2=
\begin{bmatrix}
cos\beta & 0 & -sin\beta & 0\\
0 & 0 & 1 & 0 \\
sin\beta & 0 & cos\beta & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

其中角$\alpha$不太好理解，这个角是一开始轴上的点投影到XOY平面后与x轴所成的夹角。摆弄一下圆珠笔有助于理解。如果几何上想不太通，可以算一下$R1v$看看是不是等于$(\sqrt{u^2+v^2},0,w)$

$cos\alpha=\frac{u}{\sqrt{u^2+v^2}}$（特别的，这里的uv不全为零，但是在之后的式子被化简掉）

角$\beta$就比较好理解了

$cos\beta=\frac{w}{\sqrt{u^2+v^2+w^2}}$

于是我们就能写出$R_\theta$了

$R_\theta = R^{-1}_1R^{-1}_2R_zR_2R_1$

别忘了旋转矩阵是正交矩阵，其转置等于它的逆，这样会省去求逆的麻烦。

$R_\theta = R^{T}_1R^{T}_2R_zR_2R_1$

所有的五个矩阵我们都有了，我们将其连乘，得到$R_\theta$

$$
R_\theta=
\begin{bmatrix}
\frac{u^2(1-cos\theta) + cos\theta}{\sqrt{u^2+v^2+w^2}} & \frac{uv(1-cos\theta) - wsin\theta}{\sqrt{u^2+v^2+w^2}} & \frac{uw(1-cos\theta) + vsin\theta}{\sqrt{u^2+v^2+w^2}} & 0\\
\frac{vu(1-cos\theta) + w\theta}{\sqrt{u^2+v^2+w^2}} & \frac{v^2(1-cos\theta) + cos\theta}{\sqrt{u^2+v^2+w^2}} & \frac{vw(1-cos\theta) - usin\theta}{\sqrt{u^2+v^2+w^2}} & 0 \\
\frac{wu(1-cos\theta) - vsin\theta}{\sqrt{u^2+v^2+w^2}} & \frac{wv(1-cos\theta) + usin\theta}{\sqrt{u^2+v^2+w^2}} & \frac{w^2(1-cos\theta) + cos\theta}{\sqrt{u^2+v^2+w^2}} & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

我们这里希望$u^2+v^2+w^2 = 1$就好了，式子会简化很多。那么什么样的情况下对于axis(u,v,w)来说，有$u^2+v^2+w^2 = 1$呢？

其实这就是单位向量的模为1，所以我们设$normal(n_x,n_y,n_z) = axis(u,v,w).Normalized()$并将上面的矩阵改写成以下形式:

$$
R_\theta=
\begin{bmatrix}
n_xn_x(1-cos\theta) + cos\theta & n_xn_y(1-cos\theta) - n_zsin\theta & n_xn_z(1-cos\theta) + n_ysin\theta & 0\\
n_yn_x(1-cos\theta) + n_zsin\theta & n_yn_y(1-cos\theta) + cos\theta & n_yn_z(1-cos\theta) - n_xsin\theta & 0 \\
n_zn_x(1-cos\theta) - n_ysin\theta & n_zn_y(1-cos\theta) + n_xsin\theta & n_zn_z(1-cos\theta) + cos\theta & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

我们就得到绕任意轴旋转的旋转矩阵

//rotate by the axis cross the world origin(0,0,0)
Matrix4f get_rotation(Vector3f axis, float angle) {
	Matrix4f rotate = Matrix4f::Identity();
	float cos_theta = cos(ToRadius(angle));
	float sin_theta = sin(ToRadius(angle));
	float one_minus_cos_theta = 1- cos_theta;
	axis = axis.normalized();
	rotate <<
		axis[0]*axis[0]*one_minus_cos_theta + cos_theta,
		axis[0]*axis[1]*one_minus_cos_theta - axis[2]*sin_theta,
		axis[0]*axis[2]*one_minus_cos_theta + axis[1]*sin_theta,
		0,
		axis[1] * axis[0] *one_minus_cos_theta + axis[2]*sin_theta,
		axis[1] * axis[1] *one_minus_cos_theta + cos_theta,
		axis[1] * axis[2] *one_minus_cos_theta - axis[0]*sin_theta,
		0,
		axis[2] * axis[0] *one_minus_cos_theta - axis[1]*sin_theta,
		axis[2] * axis[1] *one_minus_cos_theta + axis[0]*sin_theta,
		axis[2] * axis[2] *one_minus_cos_theta + cos_theta,
		0,
		0,0,0,1;
	return rotate;
}

但这还不够，这里所谓的绕任意轴旋转，其实是过原点的任意轴旋转。即我们传入的axis经过点(0,0,0)。如果我们想让物体不是沿着过原点的轴，而是沿着任意一条直线，举个例子，让一个三角形沿着自己的一条边旋转，上面的函数显然就不够用了。

从另一个角度来说，我们可以观察到上面的矩阵的格式，其实完全可以不使用齐次矩阵，写成3x3的格式，这也正是印证了这一点。

那么如果我想让点绕着空间中任意的一条直线旋转呢？首先，如何用参数来表示这条直线？我们只需要一个方向向量与直线上的任意一点就能确定空间中唯一的一条直线。所以这里我们就需要至少传入两个vector3作为参数（传入两个点或者传入一个点一个方向向量都行）。

Matrix4f get_rotation(Vector3f from, Vector3f to, float angle){
    ...
}
//or 
Matrix4f get_rotation(Vector3f any_point_on_line, Vector3f dir, float angle){
    ...
}

我们之前的函数Matrix4f get_rotation(Vector3f axis, float angle)就是默认了any_point_on_line = Vector3f(0,0,0);。反正直线上的两个点相减就能得出直线的方向，为了方便，下面我们采用第一种传参方法来写。

思考一下在2D空间中我们是怎么将一个点绕任意点旋转的？首先我们将任意点连着坐标系一起平移至原点，然后采用绕任意点旋转矩阵旋转，最后再将任意点从原点移回它本来所在的位置。这又是一个把大象关进冰箱的例子。

$v’ = T^{-1}R_\theta T*v$

现在拓展到三维，这个方法依然使用。我们需要将这条直线平移，使其经过原点，然后使用绕任意轴旋转公式旋转，最后再把直线移回去。

$v’ = T^{-1}R_\theta T*v$

看，式子完全一样。唯一的不同就是从二维拓展到了三维。

假设点P(a,b,c)为该直线上的一点，那么其中

$$
T=
\begin{bmatrix}
1 & 0 & 0 & -a \\
0 & 1 & 0 & -b \\
0 & 0 & 1 & -c \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

于是我们可以写出下面的代码:

Matrix4f get_rotation(Vector3f from, Vector3f to, float angle) {
	Matrix4f transform = Matrix4f::Identity();
	Vector3f axis = (to - from).normalized();
    float cos_theta = cos(ToRadius(angle));
	float sin_theta = sin(ToRadius(angle));
	transform <<
		1, 0, 0, -from[0],
		0, 1, 0, -from[1],
		0, 0, 1, -from[2],
		0, 0, 0, 1;
	return transform.inverse()*get_rotation(axis, angle)*transform;
}

这里有几个需要注意的地方:

1. axis单不单位化都行，反正get_rotation(axis, angle)中会将其单位化。

2. transform矩阵中不使用from，使用to来平移也行，甚至你可以写成-(to + k*axis)(k为任意浮点数，即直线上的任意点)这种形式，只要平移之后的直线经过原点就行。

但这样就多出了两个矩阵乘法的运算，我们自然很不乐意。所以可以将transform.inverse()*get_rotation(axis, angle)*transform， 即$T^{-1}R_\theta T$算一算，写成一个矩阵，这也正是齐次坐标系的好处。

这个推导计算的过程不难，但异常的繁杂，我十分不建议大家手推，除非你想锻炼一下计算能力。算完应该是这样的:

$$
T^{-1}R_\theta T=
\begin{bmatrix}
n_xn_x(1-cos\theta) + cos\theta & n_xn_y(1-cos\theta) - n_zsin\theta & n_xn_z(1-cos\theta) + n_ysin\theta & (a(1-n_xn_x)-n_x(bn_y+cn_z))(1-cos\theta)+(bn_z-cn_y)sin\theta\\
n_yn_x(1-cos\theta) + n_zsin\theta & n_yn_y(1-cos\theta) + cos\theta & n_yn_z(1-cos\theta) - n_xsin\theta & (b(1-n_yn_y)-n_y(an_x+cn_z))(1-cos\theta)+(cn_x-an_z)sin\theta\\
n_zn_x(1-cos\theta) - n_ysin\theta & n_zn_y(1-cos\theta) + n_xsin\theta & n_zn_z(1-cos\theta) + cos\theta & (c(1-n_zn_z)-n_z(an_x+bn_y))(1-cos\theta)+(an_y-bn_x)sin\theta\\
0 & 0 & 0 & 1
\end{bmatrix}
$$

可以看到其实$R_{3x3}$的元素其实没变，只是表示平移的最后一列发生了变化。

于是我们得到了绕任意直线旋转的函数的最终版

Matrix4f get_rotation(Vector3f from, Vector3f to, float angle) {
	Matrix4f rotate = Matrix4f::Identity();
	Vector3f axis = (to - from).normalized();
	float cos_theta = cos(ToRadius(angle));
	float sin_theta = sin(ToRadius(angle));
	float one_minus_cos_theta = 1 - cos_theta;
	float a = from.x();
	float b = from.y();
	float c = from.z();
	rotate <<
		axis[0] * axis[0] * one_minus_cos_theta + cos_theta,
		axis[0] * axis[1] * one_minus_cos_theta - axis[2] * sin_theta,
		axis[0] * axis[2] * one_minus_cos_theta + axis[1] * sin_theta,
		(a*(1 - axis[0]*axis[0]) - axis[0]*(b*axis[1] + c*axis[2]))*one_minus_cos_theta + (b*axis[2] - c*axis[1])*sin_theta,
		axis[1] * axis[0] * one_minus_cos_theta + axis[2] * sin_theta,
		axis[1] * axis[1] * one_minus_cos_theta + cos_theta,
		axis[1] * axis[2] * one_minus_cos_theta - axis[0] * sin_theta,
		(b*(1 - axis[1]*axis[1]) - axis[1]*(a*axis[0] + c*axis[2]))*one_minus_cos_theta + (c*axis[0] - a*axis[2])*sin_theta,
		axis[2] * axis[0] * one_minus_cos_theta - axis[1] * sin_theta,
		axis[2] * axis[1] * one_minus_cos_theta + axis[0] * sin_theta,
		axis[2] * axis[2] * one_minus_cos_theta + cos_theta,
		(c*(1 - axis[2]*axis[2]) - axis[2]*(a*axis[0] + b*axis[1]))*one_minus_cos_theta + (a*axis[1] - b*axis[0])*sin_theta,
		0, 0, 0, 1;
	return rotate;
}

这里axis必须normalized，因为上述推导过程中的nx,ny,nz严格满足$n^2_x+n^2_y+n^2_z=1$

哦，这是透视投影，这种现象是正常的。

reference:
Rotation About an Arbitrary Axis in 3 Dimensions,Glenn Murray