旋轉變換_四元數

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

1.簡介

四元數是另一種描述三維旋轉的方式，四元數使用4個分量來描述旋轉，四元數的描述方式如下：

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

四元數的由來和復數很很大的關系，因此首先討論一下關于復數的內容。

1.1 復數

復數可以定義如下：

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

復數常用的基本運算如下：

Complex
復數中一個比較重要的概念是共軛復數，將復數的虛部取相反數，得到它的共軛復數：

z=a+biz?=a?bi

復數的模，定義為：
complex2

復數還可以使用復平面來表示，復平面分為實軸和虛軸（類似于二維直角坐標系中的x軸和y軸），如下圖所示：

Complex3
當我們使用i去乘以一個復數時，當我們把得到的結果繪制在復平面上時，發現得到的位置正好是繞原點旋轉90度的效果。
complex4
于是可以猜測，復數的乘法和旋轉之間應該有某些關系。
我們可以通過定義一個復數

q=cosθ+isinθ

使用它作為一個旋轉的因子，當與復數相乘時，得到：
complex5
寫成矩陣的形式是：

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

這個公式正好是二維的旋轉公式，當把新的到的(a′+b′i)繪制在復平面上時，得到的正好是原來的點(a+bi)旋轉θ角之后的位置。

2. 四元數

既然使用復數的乘法可以描述二維的旋轉，那么拓展一個維度是否能表示三維旋轉呢，這個也正是四元數發明者William Hamilton最初的想法，也就是說使用

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

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

quaternions

四元數可以寫成下面的方式：

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

或者寫成

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

2.1 四元數的運算

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

      // Quat的成員_v[4]代表四元數的（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]);}

兩個四元數相減
(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]);}

兩個四元數相乘

兩個四元數相乘的規則和多項式乘法一樣，
(a + i b + j c + k d)*(e + i f + j g + k h)
當有i，j，k參與時，規則如下：
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
使用多項式乘法展開，可以得到：
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)

實現代碼：

        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] );}

兩四元數相除
四元數一般來說不定義除法，因為四元數的乘法運算并不滿足交換律。一般有四元數的類定義除法是，其實定義的是q1?q2?1

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

，這個結論很容易推導出來。conj(q)稱為q的共軛表達式，
con(q) = w - xi - yj -zk，只需要四元數向量部分取負即可
實現如下：

        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];}

2.2 四元數的旋轉

四元數在三維圖形學領域的一個重要應用是用它來描述三維旋轉，四元數從某種意義上來說是四維空間的旋轉，難以想象，了解它的結論和使用場景更加重要。歐拉定理告訴我們任意三維旋轉都可以使用一個旋轉向量和旋轉角度來描述。因此四元數往往是使用旋轉軸和旋轉角來構造的，構造它的方法如下：

2.2.1 繞向量u旋轉角度θ

構造四元數

可以用下面的四元數來表示：

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

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

實現代碼如下：

   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;}

2.2.2 從一個向量旋轉到另一個向量構造四元數

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

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());}

有一種特殊情況需要考慮：兩向量共線（包括方向相同和方向相反，也就是夾角是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());}}

上述的代碼改進了之前代碼，但是在計算過程中使用了反三角函數（相對比較耗時），可以通過三角函數公式，簡化，不需要調用反三角函數：

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;}

這樣修改之后，去掉了算法中復雜的三角函數運算，事實上還可以進一步改進計算過程，考慮到代碼中多次的歸一化（normalize）的操作，需要進行多次開方運算，為了簡化，可以考慮：

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

sinθ=2sinθ2cosθ2

同時有：

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;}

注意到_v[0]到_v[3]中乘以half_sin，之前axis計算的分母中就有half_sin，也就是說這一項可以被化簡掉，于是代碼簡化成：

        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;}

2.2.3 從四元數獲取旋轉矩陣和旋轉角

這個過程是上面的反過程，根據之前描述的公式反算就可以，得到的公式是：

 //設四元數是 xi+yj+zk+w，那么旋轉角度和旋轉軸(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)

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

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

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

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

a=x/sin(acos(w))

，參考下圖：

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

0度的情況：
當時0度的時候，得到w=1，會導致計算公式中分母是0，除以0出現無窮大，因此需要單獨討論
180度的情況
當180度是 w=0，可以通過計算得到
a = x， b=y，c=z
計算過程是正確的，因此這種情況不需要特殊的去分析。
綜合上面整體的描述，代碼如下：

        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;}}

2.3 向量使用四元數進行旋轉

這么辛苦寫了四元數的類，為的就是使用它對頂點和向量進行旋轉的操作，也就是說我們需要完成下面的函數實現：

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

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

        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;}

2.4 四元數的插值

使用四元數來表示旋轉，在插值時非常的方便和平滑，如果使用歐拉角來進行插值運算，除了會出現萬向節死鎖外，插值的效果顯得十分的生硬。四元數的球面插值使用下面的公式：

這里寫圖片描述

其中：
- qm：插值的四元數
- qa：插值四元數的第一個值（起點）
- qb：插值四元數的第二個值（終點）
- t：（0.0,1.0）之間的一個數
- θ

: qa和qb夾角的一半

實現如下：

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);
}

參考文獻：

Understanding Quaternions
如何形象地理解四元數？知乎Yang Eninala的解答
Quaternion
Quaternion
Maths - Quaternions
Beautiful maths simplification: quaternion from two vectors
Rotating vector3 by a quaternion
quaternion vector product四元數變換向量算法的原理