四元数旋转

前言

在https://blog.csdn.net/b1049112625/article/details/140848588?spm=1001.2014.3001.5501
这篇文章中介绍了三维旋转的概念，本篇文章介绍一种更高效的方法，四元数旋转

复数

复数的表示方法

复数$z$的一种表示方法是使用有序实数对来表示
$$z=(x,y)$$
其中，$x$为$z$的实部，$y$为$z$的虚部，$x$和$y$都是实数

$(0,y)$表示纯虚数
$(x,0)$表示实数

另一种表示方法是使用
$$z=x+yi$$
表示，$i$是虚数单位
注意：
$$i\ast i=-1$$

复数的运算

复数的加法、减法和标量乘法是使用与二维向量 一样的规则进行的
$$\begin{gathered} z_1+z_2=(x_1,y_z)+(x_2,y_2)=(x_1+x_2,y_1+y_2) \\ {z}_1-z_2=(x_1,y_z)-(x_2,y_2)=(x_1-x_2,y_1-y_2) \end{gathered}$$
或者
$$\begin{gathered} z_1+z_2=x_1+y_{1}i+x_2+y_{2}i=(x_1+x_2)+(y_1+y_2)i \\ {z}_1-z_2=x_1+y_{1}i-x_2-y_{2}i=(x_1-x_2)+(y_1-y_2)i \end{gathered}$$

复数的乘法公式如下：
$$\begin{array}{rl}{z}_1{z}_2& =(x_1+y_{1}i)(x_2+y_{2}i)\\ & =x_1{x}_2+x_1{y}_{2}i+y_{1}i{x}_2+y_{1}i{y}_{2}i\\ & =x_1{x}_2-y_1{y}_2+(x_1{y}_2+x_2{y}_1)i\end{array}$$

共轭复数

对于复数$z=x+yi$，称$\overline{z}$为$z$的共轭复数，其中
$$\overline{z}=x-yi$$

复数的模

复数的模或者绝对值定义为：
$$|z|=\sqrt{z\overline{z}}=\sqrt{x^2+y^2}$$

四元数(quaternion)

四元数的定义

复数概念由四元数扩充到高维，四元数使用一个实部和三个虚部来表示
$$q=s+ai+bj+ck$$
其中，$s$、$a$、$b$、$c$都是实数，$i$、$j$、$k$是虚数单位

$i$、$j$、$k$满足下面的关系：

• $i^2=j^2=k^2=-1$
• $ij=-ji=k$
• $jk=-kj=i$
• $ki=-ik=j$

四元数的运算

四元数的加减如下：
$$q_1+q_2=(s_1+s_2)+(a_1+a_2)i+(b_1+b_2)j+(c_1+c_2)k{q}_1-q_2=(s_1-s_2)+(a_1-a_2)i+(b_1-b_2)j+(c_1-c_2)k$$

四元数的乘法如下：
$$\begin{array}{rl}{q}_1{q}_2& =(s_1+a_{1}i+b_{1}j+c_{1}k)(s_2+a_{2}i+b_{2}j+c_{2}k)\\ & =s_1{s}_2+s_1{a}_{2}i+s_1{b}_{2}j+s_1{c}_{2}k+a_{1}i{s}_2+a_{1}i{a}_{2}i+a_{1}i{b}_{2}j+a_{1}i+c_{2}k+b_{1}j{s}_2+b_{1}j{a}_{2}i+b_{1}j{b}_{2}j+b_{1}j+c_{2}k+c_{1}k{s}_2+c_{1}k{a}_{2}i+c_{1}k{b}_{2}j+c_{1}k+c_{2}k\\ & =s_1{s}_2+s_1{a}_{2}i+s_1{b}_{2}j+s_1{c}_{2}k+a_1{s}_{2}i-a_1{a}_2+a_1{b}_{2}k-a_1{c}_{2}j+b_1{s}_{2}j-b_1{a}_{2}k-b_1{b}_2+b_1{c}_{2}i+c_1{s}_{2}k+c_1{a}_{2}j-c_1{b}_{2}i-c_1{c}_2\\ & =(s_1{s}_2-a_1{a}_2-b_1{b}_2-c_1{c}_2)+(s_1{a}_2+a_1{s}_2+b_1{c}_2-c_1{b}_2)i+(s_1{b}_2+b_1{s}_2+c_1{a}_2-a_1{c}_2)j+(s_1{c}_2+c_1{s}_2+a_1{b}_2-b_1{a}_2)k\end{array}$$
我们分别把四个部分写出来：

• 实部： $(s_1{s}_2-a_1{a}_2-b_1{b}_2-c_1{c}_2)$
• $i$虚部：$(s_1{a}_2+a_1{s}_2+b_1{c}_2-c_1{b}_2)$
• $j$虚部：$(s_1{b}_2+b_1{s}_2+c_1{a}_2-a_1{c}_2)$
• $k$虚部：$(s_1{c}_2+c_1{s}_2+a_1{b}_2-b_1{a}_2)$

我们如果定义向量
$v_1=(a_1,b_1,c_1),v_2=(a_2,b_2,c_2)$
那么
$v_1\cdot v_2=(a_1{a}_2+b_1{b}_2+c_1{c}_2)$
$v_1\times v_2=(b_1{c}_2-c_1{b}_2,c_1{a}_2-a_1{c}_2,a_1{b}_2-b_1{a}_2)$

观察一下，上面的四部分正好可以写成：

• 实部： $(s_1{s}_2-v_1\cdot v_2)$
• $i$虚部：$(s_1{v}_2+s_2{v}_2+v_1\times v_2)$的$x$部分
• $j$虚部：$(s_1{v}_2+s_2{v}_2+v_1\times v_2)$的$y$部分
• $k$虚部：$(s_1{v}_2+s_2{v}_2+v_1\times v_2)$的$z$部分

所以，四元数的乘法公式如下：
$$\begin{gathered} q_1=(s_1,v_1) \\ {q}_2=(s_2,v_2) \\ {q}_1{q}_2=(s_1{s}_2-v_1\cdot v_2,s_1{v}_2+s_2{v}_2+v_1\times v_2) \end{gathered}$$

四元数的共轭

类似复数共轭的定义，四元数的共轭定义如下：
$\overline{q}=(s,-v)$

四元数的模表示为：
$|q|=\sqrt{q\overline{q}}=\sqrt{s^2+v\cdot v}$
$|q{|}^2=s^2+v\cdot v$

四元数的逆表示为：
$q^{-1}=\frac{(s,-v)}{|q{|}^2}$
所以：
$q{q}^{-1}=\frac{(s,v)(s,-v)}{|q{|}^2}=\frac{(s^2+v\cdot v,0)}{|q{|}^2}=(1,0)=q^{-1}q$

使用四元数进行旋转

使用四元数对任意轴进行旋转计算有两个好处：

• 四元数占用内存小
• 方便对旋转进行插值

第一步，定义四元数

假设我们需要计算围绕经过原点的一个轴进行旋转，旋转弧度值为$\theta$，轴的单位向量为$u=(x_0,y_0,z_0)$
那么，我们应该这样定义四元数
$$q=(s,v),s=cos(\frac{\theta }{2}),v=usin(\frac{\theta }{2})$$

第二步，旋转点四元数

比如现在进行旋转的顶点坐标为$p=(x,y,z)$，将顶点表示成四元数形式
$$P=(0,p)$$

第三步，进行旋转

旋转后的四元数定义为
$$P^{{}^{\prime }}=qP{q}^{-1}$$
$\because |q{|}^2=s^2+v\cdot v$
$\because u\cdot u=1$
$\therefore |q{|}^2==cos(\frac{\theta }{2}{)}^2+sin(\frac{\theta }{2}{)}^2=1$
$\therefore q^{-1}=(s,-v)$

第四步，获取旋转后的坐标

再次写上前面的公式
$q_1{q}_2=(s_1{s}_2-v_1\cdot v_2,s_1{v}_2+s_2{v}_2+v_1\times v_2)$

开始计算旋转后的坐标
$$\begin{array}{rl}{P}^{{}^{\prime }}& =qP{q}^{-1}\\ & =(s,v)(0,p)(s,-v)\\ & =(-v\cdot p,sp+v\times p)(s,-v)\\ & =(-(v\cdot p)s+sp\cdot v+v\times p\cdot v,s^{2}p+v(v\cdot p)+2s(v\times p)+v\times (v\times p))\\ & =(0,s^{2}p+v(v\cdot p)+2s(v\times p)+v\times (v\times p))\end{array}$$

所以，旋转后的顶点坐标为
$s^{2}p+v(v\cdot p)+2s(v\times p)+v\times (v\times p)$

带入之前定义的值
假设$a=sin(\frac{\theta }{2})x_0,b=sin(\frac{\theta }{2})y_0,c=sin(\frac{\theta }{2})z_0$
$$\begin{array}{rl}& s^{2}p+v(v\cdot p)+2s(v\times p)+v\times (v\times p)\\ =& \left(\begin{array}{ccc}1-2b^2-2c^2& 2ab-2sc& 2ac+2sb\\ 2ab+2sc& 1-2a^2-2c^2& 2bc-2sa\\ 2ac-2sb& 2bc+2sa& 1-2a^2-2b^2\end{array}\right)p\\ =& M_{r}p\end{array}$$
$M_r$矩阵即我们要使用的旋转矩阵

下面给出$M_r$矩阵计算的代码

void GetMatrix(matrix4d<float>& matrix,const vector3d<float>& rotateLine,const float angle)
{
	float temp = angle * 0.5;
	const float sr = sin(temp);
	const float cr = cos(temp);
	rotateLine = rotateLine.normalize();
	X =rotateLine.X*sr;
	Y =rotateLine.Y*sr;
	Z =rotateLine.Z*sr;
	W =cr;
    matrix[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
    matrix[1] = 2.0f*X*Y + 2.0f*Z*W;
    matrix[2] = 2.0f*X*Z - 2.0f*Y*W;
    matrix[3] = 0.0f;
    matrix[4] = 2.0f*X*Y - 2.0f*Z*W;
    matrix[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
    matrix[6] = 2.0f*Z*Y + 2.0f*X*W;
    matrix[7] = 0.0f;
    matrix[8] = 2.0f*X*Z + 2.0f*Y*W;
    matrix[9] = 2.0f*Z*Y - 2.0f*X*W;
    matrix[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
    matrix[11] = 0.0f;
    matrix[12] = 0.0f;
    matrix[13] = 0.0f;
    matrix[14] = 0.0f;
    matrix[15] = 1.f;
}