四元数旋转与矩阵旋转

四元数
$$p = a + \vec{b}, \quad q = c + \vec{d}.$$
$$p + q = a + c + \vec{b} + \vec{d}.$$
$$pq = ac + a \vec{d} + c\vec{b} - \vec{b} \cdot \vec{d} + \vec{b} \times \vec{d}.$$
$$p^* = a - \vec{b}.$$
$$pp^* = p^*p = a^2 + \vec{b} \cdot \vec{b}.$$
$$(pq)^* = q^* p^*.$$

向量$\vec{u}$，绕单位轴$\vec{r}$旋转$\theta$，得到$\vec{v}$.显然
$$\vec{u} = (\vec{r} \cdot \vec{u}) \vec{r} + \vec{r} \times (\vec{u} \times \vec{r}).$$
旋转平面上的$x$轴为$\vec{r} \times (\vec{u} \times \vec{r})$，$y$轴为$\vec{r} \times \vec{u}$.所以
$$\vec{v} = (\vec{r} \cdot \vec{u}) \vec{r} + \vec{r} \times (\vec{u} \times \vec{r}) \cos\theta + \vec{r} \times \vec{u} \sin\theta.$$
令
$$p = a + \vec{b}.$$
则
$$p \vec{u} p^* = a^2 \vec{u} + 2a \vec{b} \times \vec{u} + (\vec{u} \cdot \vec{b}) \vec{b} - \vec{b} \times (\vec{u} \times \vec{b}).$$
所以
$$p = \cos \frac{\theta}{2} + \vec{r} \sin \frac{\theta}{2}.$$
有
$$\vec{v} = p \vec{u} p^*.$$

矩阵
\begin{equation}
\nonumber
\begin{split}
Ru = & (\vec{r} \cdot \vec{u}) \vec{r} + \vec{r} \times (\vec{u} \times \vec{r}) \cos\theta + \vec{r} \times \vec{u} \sin\theta \\
= & \vec{r} (\vec{r} \cdot \vec{u}) (1 - \cos\theta) + (\vec{r} \cdot \vec{r}) \vec{u} \cos\theta + \vec{r} \times \vec{u} \sin\theta.
\end{split}
\end{equation}
展开得
$$Ru = rr^T u (1 - \cos\theta) + r^Tr u \cos\theta + \begin{bmatrix}
0 & -r_3 & r_2 \\
r_3 & 0 & -r_1 \\
-r_2 & r_1 & 0
\end{bmatrix} u \sin \theta.$$
所以
$$R = rr^T (1 - \cos\theta) + I \cos\theta + \begin{bmatrix}
0 & -r_3 & r_2 \\
r_3 & 0 & -r_1 \\
-r_2 & r_1 & 0
\end{bmatrix} \sin \theta.$$
有
$$R = \begin{bmatrix}
r_1^2(1-\cos\theta) +\cos\theta & r_1r_2(1-\cos\theta) -r_3\sin \theta & r_1r_3(1-\cos\theta) +r_2\sin \theta \\
r_2r_1(1-\cos\theta) +r_3\sin \theta & r_2^2(1-\cos\theta) +\cos\theta & r_2r_3(1-\cos\theta) -r_1\sin \theta \\
r_3r_1(1-\cos\theta) -r_2\sin \theta & r_3r_2(1-\cos\theta) +r_1\sin \theta & r_3^2(1-\cos\theta) +\cos\theta
\end{bmatrix}.$$

绝对角速度，四元数
$$\vec{\omega} \times (p \vec{u} p^*) = \frac{d}{dt} p \vec{u} p^*.$$
所以
$$\frac{\vec{\omega}p\vec{u}p^* - p\vec{u}p^*\vec{\omega}}{2} = \dot{p}\vec{u}p^* + p\vec{u}\dot{p}^*.$$
$$(\vec{\omega}p - 2\dot{p})\vec{u}p^* - p\vec{u}(p^*\vec{\omega} + 2\dot{p}^*) = 0.$$
由于
$$p = \cos \frac{\theta}{2} + \vec{r} \sin \frac{\theta}{2}.$$
有
$$\left(\frac{dp}{dt}\right)^* = \frac{dp^*}{dt}.$$
所以
$$\vec{\omega} = - 2 p\dot{p}^* = 2\dot{p}p^*.$$
则
$$\vec{\omega} = \vec{r}\dot{\theta} + \frac{d\vec{r}}{dt} \sin \theta + \vec{r} \times \frac{d\vec{r}}{dt} (1 - \cos\theta).$$

矩阵
$$\dot{R}u = \omega \times (Ru).$$
$$\dot{R} = \omega \times R = \begin{bmatrix}
0 & -w_3 & w_2 \\
w_3 & 0 & -w_1 \\
-w_2 & w_1 & 0
\end{bmatrix} R.$$
$$\begin{bmatrix}
0 & -w_3 & w_2 \\
w_3 & 0 & -w_1 \\
-w_2 & w_1 & 0
\end{bmatrix} = \dot{R} R^T.$$

相对角速度，四元数
$$p(\vec{\omega} \times \vec{u} )p^* = \frac{d}{dt} p \vec{u} p^*.$$
则
$$\vec{\omega} = 2p^*\dot{p}.$$
$$\vec{\omega} = \vec{r}\dot{\theta} + \frac{d\vec{r}}{dt} \sin \theta + \frac{d\vec{r}}{dt} \times \vec{r} (1 - \cos\theta).$$

矩阵
$$\dot{R}u = R(\omega \times u).$$
$$\dot{R} = R \begin{bmatrix}
0 & -w_3 & w_2 \\
w_3 & 0 & -w_1 \\
-w_2 & w_1 & 0
\end{bmatrix}.$$
$$\begin{bmatrix}
0 & -w_3 & w_2 \\
w_3 & 0 & -w_1 \\
-w_2 & w_1 & 0
\end{bmatrix} = R^T \dot{R}.$$