四元数

在討論「四元數」之前，我們來想想對三維直角座標而言，在物體旋轉會有何影響，可以擴充三維直角座標系統的旋轉為三角度系統（Three-angle system），在Game Programming Gems中有提供這麼一段：
Quaternions do not suffer from gimbal lock. With a three-angle(roll, pitch, yaw) system, there are always certain orientations in which there is no simple change to the trhee values to represent a simple local roation. You often see this rotation having "pitched up" 90 degree when you are trying to specify a local yaw for right.

簡單的說，三角度系統無法表現任意軸的旋轉，只要一開始旋轉，物體本身即失去對任意軸的自主性。
四元數（Quaternions）為數學家Hamilton於1843年所創造的，您可能學過的是複數，例如：a + b i 這樣的數，其中i * i = -1，Hamilton創造了三維的複數，其形式為 w + x i + y j + z k，其中i、j、k的關係如下：
i² = j² = k² = -1
i * j = k = -j * i
j * k = i = -k * j
k * i = j = -i * k

假設有兩個四元數：
q₁ = w₁ + x₁ i + y₁ j + z₁ k
q₂ = w₂ + x₂ i + y₂ j + z₂ k

四元數的加法定義如下：
q₁ + q₂ = (w₁+w₂) + (x₁+x₂) i + (y₁+y₂) j + (z₁+z₂) k

四元數的乘法定義如下，利用簡單的分配律就是了：
q₁ * q₂ =

(w₁*w₂ - x₁*x₂ - y₁*y₂ - z₁*z₂) +
(w₁*x₂ + x₁*w₂ + y₁*z₂ - z₁*y₂) i +
(w₁*y₂ - x₁*z₂ + y₁*w₂ + z₁*x₂) j +
(w₁*z₂ + x₁*y₂ - y₁*x₂ + z₁*w₂) k

由於q = w + x i + y j + z k中可以分為純量w與向量x i + y j + z k，所以為了方便表示，將q表示為(S, V)，其中S表示純量w，V表示向量x i + y j + z k，所以四元數乘法又可以表示為：
q₁ * q₂ = (S₁ + V₁)*(S₂ + V₂) = S₁*S₂ - V₁.V₂ + V₁XV₂ + S₁*V₂ + S₂*V₁

其中V₁.V₂表示向量內積，V₁XV₂表示向量外積。
定義四元數q = w + x i + y j +ｚ k 的norm為：
N(q) = |q| = x² + y² + z² + w²

滿足N(q) = 1的四元數集合，稱之為單位四元數（Unit quaternions）。
定義四元數定義四元數q = w + x i + y j +ｚk的共軛（Conjugate）為：

q* = 定義四元數q = w - x i - y j -ｚ k = [S - V]

定義四元數的倒數為：
1/ q = q* / N(q)

說明了一些數學，您所關心的或許是，四元數與旋轉究竟有何關係，假設有一任意旋轉軸的向量A(Xa, Ya, Za)與一旋轉角度θ，如下圖所示：

可以將之轉換為四元數：

x = s * Xa
y = s * Xb
z = s * Xc
w = cos(θ/2)
s = sin(θ/2)

所以使用四元數來表示的好處是：我們可以簡單的取出旋轉軸與旋轉角度。那麼四元數如何表示三維空間的任意軸旋轉？假設有一向量P(X, Y, Z)對著一單位四元數q作旋轉，則將P視為無純量的四元數X i + Y j + Z k，則向量的旋轉經導證如下：

Rot(P) = q p q*

四元數具有純量與向量，為了計算方便，將之以矩陣的方式來表現四元數的乘法，假設將四元數表示如下：

q = [w, x, y, z] = [S, V]

兩個四元數相乘q" = q * q'的矩陣表示法如下所示：

若令q = [S, V] = [cosθ, u*sinθ]，其中u為單位向量，而令q'= [S', V']為一四元數，則經過導證，可以得出q * q' * q^(-1)會使得q'繞著u軸旋轉2θ。
由四元數的矩陣乘法與四元數的旋轉，可以導證出上面的旋轉公式可以使用以下的矩陣乘法來達成：
講了這麼多，其實就是要引出上面這個矩陣乘法，也就是說如果您要讓向量(x', y', z')（w'為0）對某個單位向量軸u(x, y, z)旋轉角度2θ，則w = cosθ，代入以上的矩陣乘法，即可得旋轉後的(x", y", z")，如果為了方便，轉換矩陣的最下列與最右行會省略不寫出來，而如下所示：

關於四元數的其它說明，您可以參考 向量外積與四元數 這篇文章。
關於旋轉的轉換矩陣導證，在Game Programming Gems第二章有詳細的說明。
關於 Gimbal lock。

Quaternion(四元素)和旋转

Quaternion 的定义

四元素一般定义如下：

             q=w+xi+yj+zk

其中 w,x,y,z是实数。同时，有:

    i*i=-1

    j*j=-1

    k*k=-1

四元素也可以表示为：

   q=[w,v]

其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。

通俗的讲，一个四元数（Quaternion）描述了一个旋转轴和一个旋转角度。这个旋转轴和这个角度可以通过 Quaternion::ToAngleAxis转换得到。当然也可以随意指定一个角度一个旋转轴来构造一个Quaternion。这个角度是相对于单位四元数而言的，也可以说是相对于物体的初始方向而言的。

当用一个四元数乘以一个向量时，实际上就是让该向量围绕着这个四元数所描述的旋转轴，转动这个四元数所描述的角度而得到的向量。

四元组的优点¹

有多种方式可表示旋转，如 axis/angle、欧拉角(Euler angles)、矩阵(matrix)、四元组等。 相对于其它方法，四元组有其本身的优点：

• 四元数不会有欧拉角存在的 gimbal lock 问题
• 四元数由4个数组成，旋转矩阵需要9个数
• 两个四元数之间更容易插值
• 四元数、矩阵在多次运算后会积攒误差，需要分别对其做规范化(normalize)和正交化(orthogonalize)，对四元数规范化更容易
• 与旋转矩阵类似，两个四元组相乘可表示两次旋转

Quaternion 的基本运算¹

Normalizing a quaternion

// normalising a quaternion works similar to a vector. This method will not do anything

// if the quaternion is close enough to being unit-length. define TOLERANCE as something

// small like 0.00001f to get accurate results

void Quaternion::normalise()

{

// Don't normalize if we don't have to

float mag2 = w * w + x * x + y * y + z * z;

if ( mag2!=0.f && (fabs(mag2 - 1.0f) > TOLERANCE)) {

float mag = sqrt(mag2);

w /= mag;

x /= mag;

y /= mag;

z /= mag;

}

}

定義四元數q = w + x i + y j +ｚ k 的norm為：

N(q) = |q| = x² + y² + z² + w²

定義四元數的倒數為：

1/ q = q* / N(q)

The complex conjugate of a quaternion

// We need to get the inverse of a quaternion to properly apply a quaternion-rotation to a vector

// The conjugate of a quaternion is the same as the inverse, as long as the quaternion is unit-length

Quaternion Quaternion::getConjugate()

{

return Quaternion(-x, -y, -z, w);

}

q* = 定義四元數q = w - x i - y j -ｚ

Multiplying quaternions

// Multiplying q1 with q2 applies the rotation q2 to q1

Quaternion Quaternion::operator* (const Quaternion &rq) const

{

// the constructor takes its arguments as (x, y, z, w)

return Quaternion(w * rq.x + x * rq.w + y * rq.z - z * rq.y,

w * rq.y + y * rq.w + z * rq.x - x * rq.z,

w * rq.z + z * rq.w + x * rq.y - y * rq.x,

w * rq.w - x * rq.x - y * rq.y - z * rq.z);

}

四元數的乘法定義如下，利用簡單的分配律就是了：

q₁ * q₂ =

(w₁*w₂ - x₁*x₂ - y₁*y₂ - z₁*z₂) +
(w₁*x₂ + x₁*w₂ + y₁*z₂ - z₁*y₂) i +
(w₁*y₂ - x₁*z₂ + y₁*w₂ + z₁*x₂) j +
(w₁*z₂ + x₁*y₂ - y₁*x₂ + z₁*w₂) k

Rotating vectors

// Multiplying a quaternion q with a vector v applies the q-rotation to v

Vector3 Quaternion::operator* (const Vector3 &vec) const

{

Vector3 vn(vec);

vn.normalise();

Quaternion vecQuat, resQuat;

vecQuat.x = vn.x;

vecQuat.y = vn.y;

vecQuat.z = vn.z;

vecQuat.w = 0.0f;

resQuat = vecQuat * getConjugate();

resQuat = *this * resQuat;

return (Vector3(resQuat.x, resQuat.y, resQuat.z));

}

說明了一些數學，您所關心的或許是，四元數與旋轉究竟有何關係，假設有一任意旋轉軸的向量A(Xa, Ya, Za)與一旋轉角度θ，如下圖所示：
可以將之轉換為四元數：

x = s * Xa
y = s * Xb
z = s * Xc
w = cos(θ/2)
s = sin(θ/2)

所以使用四元數來表示的好處是：我們可以簡單的取出旋轉軸與旋轉角度。
那麼四元數如何表示三維空間的任意軸旋轉？假設有一向量P(X, Y, Z)對著一單位四元數q作旋轉，則將P視為無純量的四元數X i + Y j + Z k，則向量的旋轉經導證如下：

Rot(P) = q p q*

How to convert to/from quaternions¹

Quaternion from axis-angle

// Convert from Axis Angle

void Quaternion::FromAxis(const Vector3 &v, float angle)

{

float sinAngle;

angle *= 0.5f;

Vector3 vn(v);

vn.normalise();

sinAngle = sin(angle);

x = (vn.x * sinAngle);

y = (vn.y * sinAngle);

z = (vn.z * sinAngle);

w = cos(angle);

}

Quaternion from Euler angles

// Convert from Euler Angles

void Quaternion::FromEuler(float pitch, float yaw, float roll)

{

// Basically we create 3 Quaternions, one for pitch, one for yaw, one for roll

// and multiply those together.

// the calculation below does the same, just shorter

float p = pitch * PIOVER180 / 2.0;

float y = yaw * PIOVER180 / 2.0;

float r = roll * PIOVER180 / 2.0;

float sinp = sin(p);

float siny = sin(y);

float sinr = sin(r);

float cosp = cos(p);

float cosy = cos(y);

float cosr = cos(r);

this->x = sinr * cosp * cosy - cosr * sinp * siny;

this->y = cosr * sinp * cosy + sinr * cosp * siny;

this->z = cosr * cosp * siny - sinr * sinp * cosy;

this->w = cosr * cosp * cosy + sinr * sinp * siny;

normalise();

}

Quaternion to Matrix

// Convert to Matrix

Matrix4 Quaternion::getMatrix() const

{

float x2 = x * x;

float y2 = y * y;

float z2 = z * z;

float xy = x * y;

float xz = x * z;

float yz = y * z;

float wx = w * x;

float wy = w * y;

float wz = w * z;

}

// This calculation would be a lot more complicated for non-unit length quaternions

// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by

// OpenGL

return Matrix4( 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy), 0.0f,

2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx), 0.0f,

2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2), 0.0f,

0.0f, 0.0f, 0.0f, 1.0f)

}

Quaternion to axis-angle

// Convert to Axis/Angles

void Quaternion::getAxisAngle(Vector3 *axis, float *angle)

{

float scale = sqrt(x * x + y * y + z * z);

axis->x = x / scale;

axis->y = y / scale;

axis->z = z / scale;

*angle = acos(w) * 2.0f;

}

Quaternion 插值²

线性插值

最简单的插值算法就是线性插值，公式如：

q(t)=(1-t)q1 + t q2

但这个结果是需要规格化的，否则q(t)的单位长度会发生变化，所以

q(t)=(1-t)q1+t q2 / || (1-t)q1+t q2 ||

球形线性插值

尽管线性插值很有效，但不能以恒定的速率描述q1到q2之间的曲线，这也是其弊端，我们需要找到一种插值方法使得q1->q(t)之间的夹角θ是线性的，即θ(t)=(1-t)θ1+t*θ2，这样我们得到了球形线性插值函数q(t)，如下：

q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ

如果使用D3D，可以直接使用 D3DXQuaternionSlerp 函数就可以完成这个插值过程。

用 Quaternion 实现 Camera 旋转

总体来讲，Camera 的操作可分为如下几类：

· 沿直线移动

· 围绕某轴自转

· 围绕某轴公转

下面是一个使用了 Quaternion 的 Camera 类：

class Camera {

private:

Quaternion m_orientation;

public:

void rotate (const Quaternion& q);

void rotate(const Vector3& axis, const Radian& angle);

void roll (const GLfloat angle);

void yaw (const GLfloat angle);

void pitch (const GLfloat angle);

};

void Camera::rotate(const Quaternion& q)

{

// Note the order of the mult, i.e. q comes after

m_Orientation = q * m_Orientation;

}

void Camera::rotate(const Vector3& axis, const Radian& angle)

{

Quaternion q;

q.FromAngleAxis(angle,axis);

rotate(q);

}

void Camera::roll (const GLfloat angle) //in radian

{

Vector3 zAxis = m_Orientation * Vector3::UNIT_Z;

rotate(zAxis, angleInRadian);

}

void Camera::yaw (const GLfloat angle) //in degree

{

Vector3 yAxis;

{

// Rotate around local Y axis

yAxis = m_Orientation * Vector3::UNIT_Y;

}

rotate(yAxis, angleInRadian);

}

void Camera::pitch (const GLfloat angle) //in radian

{

Vector3 xAxis = m_Orientation * Vector3::UNIT_X;

rotate(xAxis, angleInRadian);

}

void Camera::gluLookAt() {

GLfloat m[4][4];

identf(&m[0][0]);

m_Orientation.createMatrix (&m[0][0]);

glMultMatrixf(&m[0][0]);

glTranslatef(-m_eyex, -m_eyey, -m_eyez);

}

用 Quaternion 实现 trackball

用鼠标拖动物体在三维空间里旋转，一般设计一个 trackball，其内部实现也常用四元数。

class TrackBall

{

public:

TrackBall();

void push(const QPointF& p);

void move(const QPointF& p);

void release(const QPointF& p);

QQuaternion rotation() const;

private:

QQuaternion m_rotation;

QVector3D m_axis;

float m_angularVelocity;

QPointF m_lastPos;

};

void TrackBall::move(const QPointF& p)

{

if (!m_pressed)

return;

QVector3D lastPos3D = QVector3D(m_lastPos.x(), m_lastPos.y(), 0.0f);

float sqrZ = 1 - QVector3D::dotProduct(lastPos3D, lastPos3D);

if (sqrZ > 0)

lastPos3D.setZ(sqrt(sqrZ));

else

lastPos3D.normalize();

QVector3D currentPos3D = QVector3D(p.x(), p.y(), 0.0f);

sqrZ = 1 - QVector3D::dotProduct(currentPos3D, currentPos3D);

if (sqrZ > 0)

currentPos3D.setZ(sqrt(sqrZ));

else

currentPos3D.normalize();

m_axis = QVector3D::crossProduct(lastPos3D, currentPos3D);

float angle = 180 / PI * asin(sqrt(QVector3D::dotProduct(m_axis, m_axis)));

m_axis.normalize();

m_rotation = QQuaternion::fromAxisAndAngle(m_axis, angle) * m_rotation;

m_lastPos = p;

}

Yaw, pitch, roll 的含义³

Yaw – Vertical axis：

yaw

Pitch – Lateral axis

pitch

Roll – Longitudinal axis

roll

The Position of All three axes

Yaw Pitch Roll

Reference

1. Using Quaternions to represent rotation
2. 四元数
3. Yaw,pitch,roll的含义

· Main Page

· Related Pages

· Modules

· Namespaces

· Classes

· Files

Tutorial page 8 - Geometry

• Previous: Tutorial page 7 - Reductions, visitors and broadcasting
• Next: Tutorial page 9 - Sparse Matrix

In this tutorial, we will briefly introduce the many possibilities offered by the geometry module, namely 2D and 3D rotations and projective or affine transformations.

Table of contents

• Transformation types
• Common API across transformation types
• Affine transformations
• Euler angles

Eigen's Geometry module provides two different kinds of geometric transformations:

• Abstract transformations, such as rotations (represented by angle and axis or by a quaternion),translations, scalings. These transformations are NOT represented as matrices, but you can nevertheless mix them with matrices and vectors in expressions, and convert them to matrices if you wish.
• Projective or affine transformation matrices: see the Transform class. These are really matrices.

Note

If you are working with OpenGL 4x4 matrices then Affine3f and Affine3d are what you want. Since Eigen defaults to column-major storage, you can directly use the Transform::data() method to pass your transformation matrix to OpenGL.

You can construct a Transform from an abstract transformation, like this:

Transform t(AngleAxis(angle,axis));

or like this:

Transform t;

t = AngleAxis(angle,axis);

But note that unfortunately, because of how C++ works, you can not do this:

Transform t = AngleAxis(angle,axis);

Explanation: In the C++ language, this would require Transform to have a non-explicit conversion constructor from AngleAxis, but we really don't want to allow implicit casting here.

Transformation types

Transformation type	Typical initialization code
2D rotation from an angle	Rotation2D<float> rot2(angle_in_radian);
3D rotation as an angle + axis	AngleAxis<float> aa(angle_in_radian, Vector3f(ax,ay,az)); The axis vector must be normalized.
3D rotation as a quaternion	Quaternion<float> q; q = AngleAxis<float>(angle_in_radian, axis);
N-D Scaling	Scaling(sx, sy) Scaling(sx, sy, sz) Scaling(s) Scaling(vecN)
N-D Translation	Translation<float,2>(tx, ty) Translation<float,3>(tx, ty, tz) Translation<float,N>(s) Translation<float,N>(vecN)
N-D Affine transformation	Transform<float,N,Affine> t = concatenation_of_any_transformations; Transform<float,3,Affine> t = Translation3f(p) * AngleAxisf(a,axis) *Scaling(s);
N-D Linear transformations (pure rotations, scaling, etc.)	Matrix<float,N> t = concatenation_of_rotations_and_scalings; Matrix<float,2> t = Rotation2Df(a) * Scaling(s); Matrix<float,3> t = AngleAxisf(a,axis) * Scaling(s);

Notes on rotations
To transform more than a single vector the preferred representations are rotation matrices, while for other usages Quaternion is the representation of choice as they are compact, fast and stable. Finally Rotation2Dand AngleAxis are mainly convenient types to create other rotation objects.

Notes on Translation and Scaling
Like AngleAxis, these classes were designed to simplify the creation/initialization of linear (Matrix) and affine (Transform) transformations. Nevertheless, unlike AngleAxis which is inefficient to use, these classes might still be interesting to write generic and efficient algorithms taking as input any kind of transformations.

Any of the above transformation types can be converted to any other types of the same nature, or to a more generic type. Here are some additional examples:

Rotation2Df r; r = Matrix2f(..); // assumes a pure rotation matrix AngleAxisf aa; aa = Quaternionf(..); AngleAxisf aa; aa = Matrix3f(..); // assumes a pure rotation matrix Matrix2f m; m = Rotation2Df(..); Matrix3f m; m = Quaternionf(..); Matrix3f m; m = Scaling(..); Affine3f m; m = AngleAxis3f(..); Affine3f m; m = Scaling(..); Affine3f m; m = Translation3f(..); Affine3f m; m = Matrix3f(..);

top

Common API across transformation types

To some extent, Eigen's geometry module allows you to write generic algorithms working on any kind of transformation representations:

Concatenation of two transformations	gen1 * gen2;
Apply the transformation to a vector	vec2 = gen1 * vec1;
Get the inverse of the transformation	gen2 = gen1.inverse();
Spherical interpolation (Rotation2D and Quaternion only)	rot3 = rot1.slerp(alpha,rot2);

top

Affine transformations

Generic affine transformations are represented by the Transform class which internaly is a (Dim+1)^2 matrix. In Eigen we have chosen to not distinghish between points and vectors such that all points are actually represented by displacement vectors from the origin ( ). With that in mind, real points and vector distinguish when the transformation is applied.

Apply the transformation to a point	VectorNf p1, p2; p2 = t * p1;
Apply the transformation to a vector	VectorNf vec1, vec2; vec2 = t.linear() * vec1;
Apply a general transformation to a normal vector (explanations)	VectorNf n1, n2; MatrixNf normalMatrix = t.linear().inverse().transpose(); n2 = (normalMatrix * n1).normalized();
Apply a transformation with pure rotation to a normal vector (no scaling, no shear)	n2 = t.linear() * n1;
OpenGL compatibility 3D	glLoadMatrixf(t.data());
OpenGL compatibility 2D	Affine3f aux(Affine3f::Identity()); aux.linear().topLeftCorner<2,2>() = t.linear(); aux.translation().start<2>() = t.translation(); glLoadMatrixf(aux.data());

Component accessors

full read-write access to the internal matrix	t.matrix() = matN1xN1; // N1 means N+1 matN1xN1 = t.matrix();
coefficient accessors	t(i,j) = scalar; <=> t.matrix()(i,j) = scalar; scalar = t(i,j); <=> scalar = t.matrix()(i,j);
translation part	t.translation() = vecN; vecN = t.translation();
linear part	t.linear() = matNxN; matNxN = t.linear();
extract the rotation matrix	matNxN = t.rotation();

Transformation creation
While transformation objects can be created and updated concatenating elementary transformations, theTransform class also features a procedural API:

	procedural API	equivalent natural API
Translation	t.translate(Vector_(tx,ty,..)); t.pretranslate(Vector_(tx,ty,..));	t *= Translation_(tx,ty,..); t = Translation_(tx,ty,..) * t;
Rotation In 2D and for the procedural API, any_rotation can also be an angle in radian	t.rotate(any_rotation); t.prerotate(any_rotation);	t *= any_rotation; t = any_rotation * t;
Scaling	t.scale(Vector_(sx,sy,..)); t.scale(s); t.prescale(Vector_(sx,sy,..)); t.prescale(s);	t *= Scaling(sx,sy,..); t *= Scaling(s); t = Scaling(sx,sy,..) * t; t = Scaling(s) * t;
Shear transformation ( 2D only ! )	t.shear(sx,sy); t.preshear(sx,sy);

Note that in both API, any many transformations can be concatenated in a single expression as shown in the two following equivalent examples:

t.pretranslate(..).rotate(..).translate(..).scale(..);

t = Translation_(..) * t * RotationType(..) * Translation_(..) * Scaling(..);

top

Euler angles

Euler angles might be convenient to create rotation objects. On the other hand, since there exist 24 different conventions, they are pretty confusing to use. This example shows how to create a rotation matrix according to the 2-1-2 convention.

Matrix3f m; m = AngleAxisf(angle1, Vector3f::UnitZ()) * AngleAxisf(angle2, Vector3f::UnitY()) * AngleAxisf(angle3, Vector3f::UnitZ());