旋轉(zhuǎn)變換_四元數(shù)

2017年03月29日 11:59:38 csxiaoshui 閱讀數(shù)：5686

1.簡(jiǎn)介

四元數(shù)是另一種描述三維旋轉(zhuǎn)的方式，四元數(shù)使用4個(gè)分量來(lái)描述旋轉(zhuǎn)，四元數(shù)的描述方式如下：

?

q=s+xi+yj+zk,(s,x,y,z∈?）i2=j2=k2=ijk=?1

?

四元數(shù)的由來(lái)和復(fù)數(shù)很很大的關(guān)系，因此首先討論一下關(guān)于復(fù)數(shù)的內(nèi)容。

1.1 復(fù)數(shù)

復(fù)數(shù)可以定義如下：

z=a+bia,b∈?i2=?1

復(fù)數(shù)常用的基本運(yùn)算如下：

?

復(fù)數(shù)中一個(gè)比較重要的概念是共軛復(fù)數(shù)，將復(fù)數(shù)的虛部取相反數(shù)，得到它的共軛復(fù)數(shù)：

z=a+biz?=a?bi

復(fù)數(shù)的模，定義為：

?

復(fù)數(shù)還可以使用復(fù)平面來(lái)表示，復(fù)平面分為實(shí)軸和虛軸（類似于二維直角坐標(biāo)系中的x軸和y軸），如下圖所示：

當(dāng)我們使用i去乘以一個(gè)復(fù)數(shù)時(shí)，當(dāng)我們把得到的結(jié)果繪制在復(fù)平面上時(shí)，發(fā)現(xiàn)得到的位置正好是繞原點(diǎn)旋轉(zhuǎn)90度的效果。

于是可以猜測(cè)，復(fù)數(shù)的乘法和旋轉(zhuǎn)之間應(yīng)該有某些關(guān)系。
我們可以通過(guò)定義一個(gè)復(fù)數(shù)

q=cosθ+isinθ

使用它作為一個(gè)旋轉(zhuǎn)的因子，當(dāng)與復(fù)數(shù)相乘時(shí)，得到：

寫成矩陣的形式是：

[a′b′]=[cosθsinθ?sinθcosθ]?[ab]

這個(gè)公式正好是二維的旋轉(zhuǎn)公式，當(dāng)把新的到的(a′+b′i)繪制在復(fù)平面上時(shí)，得到的正好是原來(lái)的點(diǎn)(a+bi)旋轉(zhuǎn)θ角之后的位置。

?

2. 四元數(shù)

既然使用復(fù)數(shù)的乘法可以描述二維的旋轉(zhuǎn)，那么拓展一個(gè)維度是否能表示三維旋轉(zhuǎn)呢，這個(gè)也正是四元數(shù)發(fā)明者William Hamilton最初的想法，也就是說(shuō)使用

z=a+ib+jci2=j2=?1

但是很遺憾 “三維的復(fù)數(shù)”（這僅僅是我按概念杜撰的一個(gè)詞，并不存在）的乘法并不是閉合的。也就是說(shuō)有可能兩個(gè)值相乘得到的結(jié)果并不是三維的復(fù)數(shù)。
William Hamilton經(jīng)歷了無(wú)數(shù)個(gè)日日夜夜，他絞盡腦汁也沒(méi)想明白這個(gè)問(wèn)題。終于有一天（1843年的一天），他意識(shí)到自己所需要的運(yùn)算在三維空間中是不可能實(shí)現(xiàn)的，但在四維空間中是可以的，他是如此的興奮，以至于把四元數(shù)的公式刻在了愛(ài)爾蘭的一座橋上。

?

四元數(shù)可以寫成下面的方式：

q=[s,v]s∈Rv∈?3

或者寫成

q=[s,xi+yj+zk]s,x,y,z∈?

?

2.1 四元數(shù)的運(yùn)算

• 兩四元數(shù)相加：
  A（a+bi+cj+dk） + B(e + fi + gj + hk) = C【 (a+e) + (b+f)i + (c+g)j + (d+h)k 】，實(shí)現(xiàn)代碼：

// Quat的成員_v[4]代表四元數(shù)的（x,y,z,w)inline Quat operator + (const Quat& rhs) const{return Quat(_v[0] + rhs._v[0],_v[1] + rhs._v[1],_v[2] + rhs._v[2],_v[3] + rhs._v[3]);}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

• 兩個(gè)四元數(shù)相減
  (sa,va) - (sb,vb) = (sa-sb,va-vb)

inline Quat operator - (const Quat& rhs) const{return Quat(_v[0] - rhs._v[0],_v[1] - rhs._v[1],_v[2] - rhs._v[2],_v[3] - rhs._v[3]);}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10

• 兩個(gè)四元數(shù)相乘

兩個(gè)四元數(shù)相乘的規(guī)則和多項(xiàng)式乘法一樣， (a + i b + j c + k d)*(e + i f + j g + k h) 當(dāng)有i，j，k參與時(shí)，規(guī)則如下： i*i = j*j = k*k = -1 i*j = k, j*i = -k j*k = i, k*j = -i k*i = j, i*k = -j 使用多項(xiàng)式乘法展開，可以得到： a*e - b*f - c*g - d*h + i (b*e + a*f + c*h- d*g) + j (a*g - b*h+ c*e + d*f) + k (a*h + b*g - c*f + d*e)

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15

實(shí)現(xiàn)代碼：

inline const Quat operator*(const Quat& rhs) const{return Quat( rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2] );}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8

• 兩四元數(shù)相除
  四元數(shù)一般來(lái)說(shuō)不定義除法，因?yàn)樗脑獢?shù)的乘法運(yùn)算并不滿足交換律。一般有四元數(shù)的類定義除法是，其實(shí)定義的是q1?q2?1

，其中
q?1=conj(q)/|q2|，為什么定義這么奇怪的表達(dá)式呢，其實(shí)是為了讓q?q?1=1

• ，這個(gè)結(jié)論很容易推導(dǎo)出來(lái)。conj(q)稱為q的共軛表達(dá)式，
  con(q) = w - xi - yj -zk，只需要四元數(shù)向量部分取負(fù)即可
  實(shí)現(xiàn)如下：

inline const Quat operator/(const Quat& denom) const{return ( (*this) * denom.inverse() );}/// Conjugateinline Quat conj () const{return Quat( -_v[0], -_v[1], -_v[2], _v[3] );}/// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)inline const Quat inverse () const{return conj() / length2();}value_type length2() const{return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20

2.2 四元數(shù)的旋轉(zhuǎn)

四元數(shù)在三維圖形學(xué)領(lǐng)域的一個(gè)重要應(yīng)用是用它來(lái)描述三維旋轉(zhuǎn)，四元數(shù)從某種意義上來(lái)說(shuō)是四維空間的旋轉(zhuǎn)，難以想象，了解它的結(jié)論和使用場(chǎng)景更加重要。歐拉定理告訴我們?nèi)我馊S旋轉(zhuǎn)都可以使用一個(gè)旋轉(zhuǎn)向量和旋轉(zhuǎn)角度來(lái)描述。因此四元數(shù)往往是使用旋轉(zhuǎn)軸和旋轉(zhuǎn)角來(lái)構(gòu)造的，構(gòu)造它的方法如下：

---

2.2.1 繞向量u旋轉(zhuǎn)角度θ

構(gòu)造四元數(shù)

可以用下面的四元數(shù)來(lái)表示：

u??=(ux,uy,uz)=uxi+uyj+uzk

?

q=eθ2(uxi+uyj+uzk)=cosθ2+(uxi+uyj+uzk)sinθ2

實(shí)現(xiàn)代碼如下：

?

void makeRotate(value_type angle, value_type x, value_type y, value_type z){const value_type epsilon = 1e-7;value_type length = sqrt(x*x + y*y + z*z);if (length < epsilon){*this = Quat();return;}value_type inversenorm = 1.0 / length;value_type coshalfangle = cos(0.5*angle);value_type sinhalfangle = sin(0.5*angle);_v[0] = x * sinhalfangle * inversenorm;_v[1] = y * sinhalfangle * inversenorm;_v[2] = z * sinhalfangle * inversenorm;_v[3] = coshalfangle;}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19

---

2.2.2 從一個(gè)向量旋轉(zhuǎn)到另一個(gè)向量構(gòu)造四元數(shù)

按照最原始的想法，從一個(gè)向量旋轉(zhuǎn)到另一個(gè)向量，那么旋轉(zhuǎn)軸可以通過(guò)兩個(gè)向量的叉乘得到，旋轉(zhuǎn)角度可以通過(guò)兩個(gè)向量間的夾角得到。（向量間的夾角的余弦可以通過(guò)兩向量點(diǎn)乘去除以它們的模，再通過(guò)反余弦函數(shù)計(jì)算），得到旋轉(zhuǎn)軸和旋轉(zhuǎn)角度之后就轉(zhuǎn)換成2.2.1中的情形了。
也就是說(shuō)最初的代碼如下：

void makeRotate(Vec3<value_type>& u, Vec3<value_type>& v){u.normalize();v.normalize();double costheta = u*v;double angle = acos(costheta);Vec3<value_type> w = u^v;w.normalize();makeRotate(angle, w.x(), w.y(), w.z());}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13

有一種特殊情況需要考慮：兩向量共線（包括方向相同和方向相反，也就是夾角是0度和180度的情形）

void makeRotate(const Vec3<value_type>& from, const Vec3<value_type>& to){const value_type epsilon = 1e-7;value_type length1 = from.length();value_type length2 = to.length();value_type cosangle = from*to / (length1*length2);if (fabs(cosangle - 1) < epsilon){makeRotate(0.0, 0.0, 0.0, 1.0);}else if (fabs(cosangle + 1.0) < epsilon){Vec3<value_type> tmp;if ((fabs(from.x())) < fabs(from.y())){if (fabs(from.x()) < fabs(from.z())){tmp.set(1.0, 0.0, 0.0);}else{tmp.set(0.0, 0.0, 1.0);}}else if (fabs(from.y()) < fabs(from.z())){tmp.set(0.0, 1.0, 0.0);}else{tmp.set(0.0, 0.0, 1.0);}Vec3<value_type> fromd(from.x(), from.y(), from.z());Vec3<value_type> axis(fromd^tmp);axis.normalize();_v[0] = axis[0];_v[1] = axis[1];_v[2] = axis[2];_v[3] = 0.0;}else{Vec3<value_type> axis(from^to);value_type angle = acos(cosangle);makeRotate(angle, axis.x(), axis.y(), axis.z());}}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37
• 38
• 39
• 40
• 41
• 42
• 43
• 44
• 45
• 46
• 47
• 48
• 49
• 50
• 51
• 52
• 53

上述的代碼改進(jìn)了之前代碼，但是在計(jì)算過(guò)程中使用了反三角函數(shù)（相對(duì)比較耗時(shí)），可以通過(guò)三角函數(shù)公式，簡(jiǎn)化，不需要調(diào)用反三角函數(shù)：

sinθ2cosθ2=1?cosθ2￣￣￣￣￣￣￣￣￣√=1+cosθ2￣￣￣￣￣￣￣￣￣√

代碼可以修改為：

?

//省略部分相同的代碼else{Vec3<value_type> axis(from^to);//替換成下面幾行//value_type angle = acos(cosangle);//makeRotate(angle, axis.x(), axis.y(), axis.z());axis.normalize();value_type half_cos = sqrt(0.5*(1+cosangle));value_type half_sin = sqrt(0.5*(1-cosangle));_v[0] = axis[0] * half_sin;_v[1] = axis[1] * half_sin;_v[2] = axis[2] * half_sin;_v[3] = half_cos;}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19

這樣修改之后，去掉了算法中復(fù)雜的三角函數(shù)運(yùn)算，事實(shí)上還可以進(jìn)一步改進(jìn)計(jì)算過(guò)程，考慮到代碼中多次的歸一化（normalize）的操作，需要進(jìn)行多次開方運(yùn)算，為了簡(jiǎn)化，可以考慮：

||u×v||=|u|.|v|.|sinθ|

?

?

sinθ=2sinθ2cosθ2

?

同時(shí)有：

sqrt(a)sqrt(b)=sqrt(ab)

?

于是代碼可以修改為：

else{value_type normFromAndTo = sqrt(from.length2()*to.length2());value_type cos_theta = from * to / normFromAndTo;value_type half_cos = sqrt(0.5 * (1+cos_theta));value_type half_sin = sqrt(0.5 * (1-cos_theta));Vec3<value_type> axis = from^to / (normFromAndTo*2*half_sin * half_cos);_v[0] = axis[0]*half_sin;_v[1] = axis[1]*half_sin;_v[2] = axis[2]*half_sin;_v[3] = half_cos;}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15

注意到_v[0]到_v[3]中乘以half_sin，之前axis計(jì)算的分母中就有half_sin，也就是說(shuō)這一項(xiàng)可以被化簡(jiǎn)掉，于是代碼簡(jiǎn)化成：

else{value_type normFromAndTo = sqrt(from.length2()*to.length2());value_type cos_theta = from * to / normFromAndTo;value_type half_cos = sqrt(0.5 * (1+cos_theta));Vec3<value_type> axis = from^to / (normFromAndTo*2 * half_cos);_v[0] = axis[0];_v[1] = axis[1];_v[2] = axis[2];_v[3] = half_cos;}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13

2.2.3 從四元數(shù)獲取旋轉(zhuǎn)矩陣和旋轉(zhuǎn)角

這個(gè)過(guò)程是上面的反過(guò)程，根據(jù)之前描述的公式反算就可以，得到的公式是：

//設(shè)四元數(shù)是 xi+yj+zk+w，那么旋轉(zhuǎn)角度和旋轉(zhuǎn)軸(a,b,c)是： angle = 2 * acos(w) a = x / sqrt(1-w*w) b = y / sqrt(1-w*w) c = z / sqrt(1-w*w)

• 1
• 2
• 3
• 4
• 5

推導(dǎo)過(guò)程如下：
有之前的公式可以知道： w=cos(θ/2)

可以得到 角度 θ=2?acos(w)

?

x=a?sin(θ/2)y=b?sin(θ/2)z=c?sin(θ/2)

先分析x這個(gè)等式，帶入求出的θ角，得到：

a=x/sin(acos(w))

，參考下圖：

得到 sin(acosw) = sqrt(1-w*w)，同理可以推出其他的結(jié)論。
但是還需要考慮其他兩個(gè)特殊情況：也就是共線的情形（角度θ是0度或者180度）

?

• 0度的情況：
  當(dāng)時(shí)0度的時(shí)候，得到w=1，會(huì)導(dǎo)致計(jì)算公式中分母是0，除以0出現(xiàn)無(wú)窮大，因此需要單獨(dú)討論

• 180度的情況
  當(dāng)180度是 w=0，可以通過(guò)計(jì)算得到
  a = x， b=y，c=z
  計(jì)算過(guò)程是正確的，因此這種情況不需要特殊的去分析。
  綜合上面整體的描述，代碼如下：

void getRotate(value_type& angle, value_type& x, value_type& y, value_type& z) const {Quat q1 = *this;if (_v[3] > 1)q1.normalize();angle = 2 * acos(q1.w());value_type s = sqrt(1 - q1.w()*q1.w());if (s < 1e-6){x = q1.x();y = q1.y();z = q1.z();}else{x = q1.x() / s;y = q1.y() / s;z = q1.z() / s;}}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22

2.3 向量使用四元數(shù)進(jìn)行旋轉(zhuǎn)

這么辛苦寫了四元數(shù)的類，為的就是使用它對(duì)頂點(diǎn)和向量進(jìn)行旋轉(zhuǎn)的操作，也就是說(shuō)我們需要完成下面的函數(shù)實(shí)現(xiàn)：

//Rotate a vector by this quaternionVec3<value_type> operator* (const Vec3<value_type>& v);

• 1
• 2

四元數(shù)變換向量的算法如下：
1. 創(chuàng)建一個(gè)以v為虛部的純虛的向量，（v.x + v.y + v.z + 0)
2. 左乘四元數(shù) q 接著右乘四元數(shù)q的共軛四元數(shù) q*
3. 計(jì)算得到的結(jié)果也是一個(gè)純的四元數(shù)，它的虛部就是變換之后的向量v’
盡管這樣做可以得到變換后的向量，如果計(jì)算過(guò)程完全按照四元數(shù)乘法法則去展開計(jì)算，計(jì)算量略大 ，可以使用下面的方式優(yōu)化一下：

Vec3<value_type> operator* (const Vec3<value_type>& v){// nVidia SDK implementationVec3<value_type> uv, uuv;Vec3<value_type> qvec(_v[0], _v[1], _v[2]);uv = qvec ^ v;uuv = qvec ^ uv;uv *= ( 2.0f * _v[3] );uuv *= 2.0f;return v + uv + uuv;}

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11

2.4 四元數(shù)的插值

使用四元數(shù)來(lái)表示旋轉(zhuǎn)，在插值時(shí)非常的方便和平滑，如果使用歐拉角來(lái)進(jìn)行插值運(yùn)算，除了會(huì)出現(xiàn)萬(wàn)向節(jié)死鎖外，插值的效果顯得十分的生硬。四元數(shù)的球面插值使用下面的公式：

其中：
- qm：插值的四元數(shù)
- qa： 插值四元數(shù)的第一個(gè)值（起點(diǎn)）
- qb：插值四元數(shù)的第二個(gè)值（終點(diǎn)）
- t： （0.0,1.0）之間的一個(gè)數(shù)
- θ

: qa和qb夾角的一半

實(shí)現(xiàn)如下：

void dslerp( value_type t, const Quat& from, const Quat& to ) {const double epsilon = 0.00001;double omega, cosomega, sinomega, scale_from, scale_to ;osg::Quat quatTo(to);// this is a dot productcosomega = from.asVec4() * to.asVec4();if ( cosomega <0.0 ){cosomega = -cosomega;quatTo = -to;}if( (1.0 - cosomega) > epsilon ){omega= acos(cosomega) ; // 0 <= omega <= Pi (see man acos)sinomega = sin(omega) ; // this sinomega should always be +ve so// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?scale_from = sin((1.0-t)*omega)/sinomega ;scale_to = sin(t*omega)/sinomega ;}else{/* --------------------------------------------------The ends of the vectors are very closewe can use simple linear interpolation - no needto worry about the "spherical" interpolation-------------------------------------------------- */scale_from = 1.0 - t ;scale_to = t ;}*this = (from*scale_from) + (quatTo*scale_to); }

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11
• 12
• 13
• 14
• 15
• 16
• 17
• 18
• 19
• 20
• 21
• 22
• 23
• 24
• 25
• 26
• 27
• 28
• 29
• 30
• 31
• 32
• 33
• 34
• 35
• 36
• 37

參考文獻(xiàn)：

Understanding Quaternions

如何形象地理解四元數(shù)？知乎Yang Eninala的解答

Quaternion

Quaternion

Maths - Quaternions

Beautiful maths simplification: quaternion from two vectors

Rotating vector3 by a quaternion

quaternion vector product四元數(shù)變換向量算法的原理

總結(jié)

以上是生活随笔為你收集整理的四元素理解的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。