四元數(Quaternion)是由愛爾蘭數學家 哈密頓(Hamilton)在1843年發明的概念。四元數的乘法不符合交換律(commutative law)。
四元數 可以描述為 (x * i, y * j, z * k, w) ,其中(x,y,z)表示虛部,w表示實部。
四元數 最大的價值在于 描述旋轉,解決三維中的 萬向節鎖 的問題。
萬向節鎖:
三維中的旋轉通常用繞 XYZ 三個軸的旋轉來表示,這個旋轉角度即 歐拉角,也就是經常說的 Pitch、Yaw、Roll。
采用歐拉角 描述旋轉的 優點是直覺簡單(矩陣乘),容易了解,旋轉過程分解為:按照一定的順序(比如X、Y、Z) 依次獨立地繞軸旋轉。
這種獨立的旋轉帶來了一個問題,也就是 未考慮到旋轉之間的關聯性。
假設 物體先繞轉X軸旋轉,然後再繞Y軸,最後繞Z軸選擇,當繞Y軸旋轉90度之後 萬向節鎖 的問題就出現了,因為X軸已經被求值了,它不再随同其他兩個軸旋轉,這樣X軸與Z軸就指向同一個方向(它們相當于同一個軸了)。
歐拉角 與 四元數
歐拉角可以和四元數進行互相轉換,一個四元數可以 由歐拉角(對應 φ,θ,ψ)變換得到:
同樣,歐拉角 也可以通過四元數 變換得到:
代碼可以參考作者定義的 四元數 類(Quaternion.h)
/* 四元數定義 - 用于旋轉
linolzhang, 2010.2.10
*/
#ifndef QUATERNION_H
#define QUATERNION_H
#include "BaseOperation.h"
#include "Matrix4.h"
template <typename T>
class Quaternion
{
public:
T x, y, z, w; // (x,y,z)(i,j,k) + w - 虛部(x,y,z),實部(w)
public:
// --------------------------------------------------------
// 構造函數
Quaternion()
{
x = y = z = 0; // 初始化成員變量
w = 1;
}
Quaternion(T _x,T _y,T _z,T _w)
{
x = _x; y = _y; z = _z;
w = _w;
}
Quaternion(const Quaternion<T> &q) // 拷貝構造函數
{
x = q.x; y = q.y; z = q.z;
w = q.w;
}
const Quaternion &operator=(const Quaternion<T> &q) // 指派操作符
{
x = q.x; y = q.y; z = q.z;
w = q.w;
return *this;
}
// --------------------------------------------------------
// 構造函數 - 由其它轉換
Quaternion(const Tuple3<T> &rAxisVector, float rAngle) // 由 旋轉軸向量,旋轉角 得到
{
w = cos(0.5*rAngle);
float t = sin(0.5*rAngle);
x = rAxisVector.x * t;
y = rAxisVector.y * t;
z = rAxisVector.z * t;
}
Quaternion(const Matrix4<T> &mat) // 由 4*4 矩陣轉換得到
{
T trace = mat.m[0] + mat.m[5] + mat.m[10]; // 用來儲存矩陣的 迹(trace)
float s = 0;
if (trace > 0) // 如果矩陣的trace>0
{
float temp = sqrt(trace + 1);
s = 0.5 / temp;
w = 0.5 * temp;
x = (mat.m[9] - mat.m[6]) * s;
y = (mat.m[2] - mat.m[8]) * s;
z = (mat.m[4] - mat.m[1]) * s;
}
else // 矩陣的trace<0
{
T tq[3];
tq[0] = 1 + mat.m[0] - mat.m[5] - mat.m[10];
tq[1] = 1 - mat.m[0] + mat.m[5] - mat.m[10];
tq[2] = 1 - mat.m[0] - mat.m[5] + mat.m[10];
// 查找最大的
int maxPos = 0;
for(int i=1; i<3; i++)
maxPos = tq[i]>tq[maxPos] ? i : maxPos;
float temp = sqrt(tq[maxPos]);
if( temp!= 0)
s = 0.5 / temp;
// 檢查對角線
if (maxPos == 0)
{
w = (mat.m[9] - mat.m[6]) * s;
x = 0.5 * temp;
y = (mat.m[4] + mat.m[1]) * s;
z = (mat.m[2] + mat.m[8]) * s;
}
else if (maxPos == 1)
{
w = (mat.m[2] - mat.m[8]) * s;
x = (mat.m[4] + mat.m[1]) * s;
y = 0.5 * temp;
z = (mat.m[9] + mat.m[6]) * s;
}
else /* if (maxPos == 2) */
{
w = (mat.m[4] - mat.m[1]) * s;
x = (mat.m[2] + mat.m[8]) * s;
y = (mat.m[9] + mat.m[6]) * s;
z = 0.5 * temp;
}
}
}
// --------------------------------------------------------
// 重載 +=, -=, *=
void operator +=(const Quaternion<T> &q)
{
w += q.w;
x += q.x;
y += q.y;
z += q.z;
}
void operator -=(const Quaternion<T> &q)
{
w -= q.w;
x -= q.x;
y -= q.y;
z -= q.z;
}
void operator *=(const Quaternion<T> &q)
{
T w1 = w*q.w - x*q.x - y*q.y - z*q.z;
T x1 = w*q.x + x*q.w + y*q.z - z*q.y;
T y1 = w*q.y + y*q.w + z*q.x - x*q.z;
T z1 = w*q.z + z*q.w + x*q.y - y*q.x;
w = w1;
w = x1; y = y1; z = z1;
}
// --------------------------------------------------------
// 轉換為矩陣表示
const Matrix4<T> convertToMatrix4()const
{
// 注:
// [ 1-2y2-2z2 , 2xy-2wz , 2xz+2wy ]
// [ 2xy+2wz , 1-2x2-2z2 , 2yz-2wx ]
// [ 2xz-2wy , 2yz+2wx , 1-2x2-2y2 ]
T xx = x*x; T xy = x*y; T xz = x*z; T xw = x*w;
T yy = y*y; T yz = y*z; T yw = y*w;
T zz = z*z; T zw = z*w;
return Matrix4<T>( 1-2*(yy+zz), 2*(xy-zw), 2*(xz+yw), 0,
2*(xy+zw), 1-2*(xx+zz), 2*(yz-xw), 0,
2*(xz-yw), 2*(yz+xw), 1-2*(xx+yy), 0,
0, 0, 0, 1 );
// 如果使用規範後的四元數 - 機關四元數
// 代入 w2 = 1-x2-y2-z2 得到
// [ w2+x2-y2-z2 , 2xy-2wz , 2xz+2wy ]
// [ 2xy+2wz , w2-x2+y2-z2 , 2yz-2wx ]
// [ 2xz-2wy , 2yz+2wx , w2-x2-y2+z2 ]
}
const Quaternion getConjugate()const // 得到共轭四元數
{
return Quaternion<T> (-x, -y, -z, w);
}
// --------------------------------------------------------
// 運算操作
void setIdentity() // 設定初始化值
{
x = y = z = 0;
w = 1;
}
void normalize() // 機關化
{
float magnitude = sqrtf(x*x + y*y + z*z + w*w);
if (magnitude != 0)
{
x /= magnitude;
y /= magnitude;
z /= magnitude;
w /= magnitude;
}
}
};
// --------------------------------------------------------
// 外部運算,重載*
template <typename T>
inline const Quaternion<T> operator*(const Quaternion<T> &left,const Quaternion<T> &right)
{
return Quaternion<T>( left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y,
left.w * right.y - left.x * right.z + left.y * right.w + left.z * right.x,
left.w * right.z + left.x * right.y - left.y * right.x + left.z * right.w,
left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z );
}
// --------------------------------------------------------
// 外部運算 - 插值(q1->q2) t[0,1] 注:接受機關四元數
template<typename T>
const Quaternion<T> Slerp(const Quaternion<T> &q1, const Quaternion<T> &q2, float t)
{
// 四元數插值公式
// 線性插值 q(t) = q1 * (1-t) + q2 * t
// 球面插值 q(t) = q1 * sin( (1-t)*θ) ) /sinθ + q2 * sin( t*θ))/sinθ
float theta; // 表示四元數之間的夾角(實際上應該是 theta)
float scale0, scale1; // 縮放因子
float qi[4]; // 結果暫存
float cosTheta = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w; // 點乘 = cos(theta)
if (cosTheta < 0)
{
cosTheta = -cosTheta;
qi[0] = -q2.x;
qi[1] = -q2.y;
qi[2] = -q2.z;
qi[3] = -q2.w;
}
else
{
qi[0] = q2.x;
qi[1] = q2.y;
qi[2] = q2.z;
qi[3] = q2.w;
}
// If the quaternions are really close, do a simple linear interpolation.
if ( 1-cosTheta <= 0.0001 ) // 線性插值公式
{
scale0 = 1 - t;
scale1 = t;
}
else // 球面插值公式
{
theta = (float) acos( cosTheta );
float temp = 1.0f/sinf(theta);
scale0 = sinf((1 - t) * theta) * temp;
scale1 = sinf(t * theta) * temp;
}
// 根據公式得到四元數結果
return Quaternion( scale0 * q1.x + scale1 * qi[0],
scale0 * q1.y + scale1 * qi[1],
scale0 * q1.z + scale1 * qi[2],
scale0 * q1.w + scale1 * qi[3] );
}
typedef Quaternion<float> Quaternionf;
typedef Quaternion<double> Quaterniond;
#endif
四元數 關鍵應用在于其表示旋轉的插值,通過 四元數插值 能夠有效進行平滑,在 OSG、OGRE、Unity、CryEngine、UnReal等引擎中早已成熟應用,其中的開源代碼部分大家可以借鑒。