四元数的意义及代码应用

四元数

四元数的定义是$[w(xyz)]=[cos(\theta /2)(sin(\theta /2)n_{x}sin(\theta /2)n_{z}sin(\theta /2)n_z)]$

四元数满足结合律但是不满足交换律:
$$\begin{gathered} (ab)c=a(bc) \\ ab\ne ba \end{gathered}$$
两个四元数模的积满足$\Vert q_1{q}_2\Vert =\Vert q_1\Vert \Vert q_2\Vert$

四元数的逆和矩阵的逆类似
$$(ab{)}^{-1}=b^{-1}{a}^{-1}=(q_1{q}_2...q_{n-1}{q}_n{)}^{-1}=q_n^{-1}{q}_{n-1}^{-1}.,.q_2^{-1}{q}_1^{-1}$$
利用这些性质,我们可以对四元数$p$向任意表示旋转方向的单位向量$\hat{n}$旋转表示成四元数之间的乘积
$$q^{\prime }=qp{q}^{-1}$$
其中$q=[cos\theta /2,\hat{n}sin\theta /2]$。特别地,$p$先向四元数$a$映射在三维空间的方向旋转再向四元数$b$映射在三维空间的方向旋转等于向四元数$ba$映射在三维空间的方向上旋转,这个方向就是它们各自$\hat{n}$在三维空间的方向:
$$\begin{gathered} p^{{}^{\prime }}=b(ap{a}^{-1})b^{-1} \\ =(ba)p(a^{-1}{b}^{-1}) \\ =(ba)p(ba{)}^{-1} \end{gathered}$$
四元数对log的定义为
$$lo{g}^q=log([cos\alpha \hat{n}sin\alpha ])=[0\alpha sin\alpha ]$$
四元数对exp的定义为
$$exp(p)=exp([0\alpha \hat{n}])=[cos\alpha \hat{n}sin\alpha ]$$
从而有
$$exp(lo{g}^q)=q$$
以四元数$q$为底的指数$q^t$相当于随t从0到1对单位四元数到四元数q旋转的插值而单位四元数定义为$[10]$或$[-10]$

代码调式如下:

//Quaternion (input and output)
float w,x,y,z;
//Input exponent
float exponent;
//Check for the case of an Identity quaternion,this will protect against divide by zero
if(fabs(w)<.9999f)
{
    float alpha = acos(w);
    float newAlpha = alpha * exponent;
    w=cos(newAlpha);
    float mult=sin(newAlpha)/sin(alpha);
    x*=mult;
    y*=mult;
    z*=mult;
}

如果以四元数$q_0$到四元数$q_1$平滑过渡的旋转,则要使用以下代码

float w0,x0,y0,z0;
float w1,x1,y1,z1;

//The interpolation parameter
float t;
float w,x,y,z;
//output quaternion
float cosOmega=w0*w1+x0*x1+y0*y1+z0*z1;

if(cosOmega<0.0f){
	w1=-w1;
	x1=-x1;
	y1=-y1;
	z1=-z1;
	cosOmega=-cosOmega;
}

float k0,k1;
if(cosOmega>0.9999f)
{
	k0=1.0f-t;
	k1=t;
}else{
	float sinOmega=sqrt(1.0f-cosOmega*cosOmega);
	
	float omega=atan2(sinOmega,cosOmega);
	
	flat oneOverSinOmega=1.0f/sinOmega;
	
	k0=sin((1.0f-t)*omega)*oneOverSinOmega;
	k1=sin(t*omega)*sinOverSinOmega;
}

w=w0*k0+w1*k1;
x=x0*k0+x1*k1;
y=y0*k0+wy1*k1;
z=z0*k0+z1*k1;

上述的代码仅仅以单位四元数为基础坐标系描述旋转的插值,根据书本前部分的讨论,从$q_0$到$q_1$四元数之差$\Delta q=q_1{q}_0^{-1}$,取这段差的一小部分t可表示为$(\Delta q{)}^t$,以$q_0$点为坐标系变换到原坐标系则表示为$(\Delta q{)}^t{q}_0$,这样可以描述任意四元数到其它四元数旋转插值的过程。

四元数中有一个专门的函数表示这种变换:
$$slerp(q_0,q_1,t)=(q_1{q}_0^{-1}{)}^t{q}_0$$
这种代数的表示往往需要消耗更多的计算资源.实际上采用的是一种数学上相等的计算形式。我们将采用二维平面扩展到四维超平面的过程来描述这种变换,如图所示

从平面上的点$v_0$到$v_1$需要旋转$\omega$度,我们要找到一种普适的计算来表达$slerp(v_0,v_1,t)$,继而扩展到$slerp(q_0,q_1,t)$

对于以$k_1{v}_1$为斜边的三角形可以得出
$$\begin{gathered} sin(\omega )=\frac{sin(t\omega )}{k_1} \\ {k}_1=\frac{sin(t\omega )}{sin\omega } \end{gathered}$$
以红色箭头作为由$k_0{v}_0$和$k_1{v}_1$组成的三角形的中垂线,由$v_1$和$k_1{v}_1$平行可知
$$k_0\Vert v_0\Vert sin(t\omega )=k_1\Vert v_1\Vert sin(1-t)\omega$$
四元数旋转后模不变可得$\Vert v_0\Vert =\Vert v_1\Vert$,那么:
$$\begin{gathered} k_{0}sin(t\omega )=k_{1}sin(1-t)\omega \\ {k}_0=\frac{k_{1}sin(1-w)}{sin(w)} \end{gathered}$$
由于$k_1=\frac{sin(t\omega )}{sin\omega }$,化简可得$k_0=\frac{sin(1-w)}{sin(w)}$

既然如此,$v_t$可以被表示为
$$v_t=k_0{v}_0+k_1{v}_1=\frac{sin(1-t)w}{sinw}{v}_0+\frac{sin(tw)}{sin(w)}{v}_1$$
相同的基本思想可以扩展到四元数空间，我们可以将slerp改写为
$$slerp(q_0,q_1,t)=\frac{sin(1-t)w}{sinw}{q}_0+\frac{sin(tw)}{sin(w)}{q}_1$$
C++代码如下

float w0,x0,y0,z0;
float w1,x1,y1,z1;

float t;

float w,x,y,z;

float cosOmega=w0*w1+x0*x1+y0*y1+z0*z1;

if(cosOmega<0.0f){
	w1=-w1;
	x1=-x1;
	y1=-y1;
	z1=-z1;
}

float k0,k1;
if(cosOmega>0.9999f)
{
	k0=1.0f-t;
	k1=t;
}else{
	float sinOmega=sqrt(1.0f-cosOmega*cosOmega); 
	float omega=atan2(sinOmega,cosOmega);
	float oneOverSinOmega=1.0f/sinOmega;
	
	k0=sin((1.0f-t)*omega)*oneOverSinOmega;
    k1=sin(t*omega)*oneOverSinOmega;
}

//interpolation
w=w0*k0+w1*k1;
x=x0*k0+w1*k1;
y=y0*k0+y1*k1;
z=z0*k0+z1*k1;

Reference:<<3D Math Primer for Graphics and Game Development>> P246~P263