2012
07-17

# Quaternion(四元数)和旋转以及Yaw, pitch, roll 的含义

55人阅读评论(0)收藏举报

 Quaternion(四元数)和旋转 本文介绍了四元数以及如何在OpenGL中使用四元数表示旋转。

#### Quaternion 的定义

    q=w+xi+yj+zk


    i*i=-1

j*j=-1

k*k=-1



    q=[w,v]


#### 四元组的优点1

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

#### Quaternion 的基本运算1

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


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


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


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



#### How to convert to/from quaternions1

##### 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 插值2

##### 线性插值

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


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


##### 球形线性插值

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



#### 用 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;

}

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

Vector3 yAxis;

{
// Rotate around local Y axis
yAxis = m_Orientation * Vector3::UNIT_Y;
}

}

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

Vector3 xAxis = m_Orientation * Vector3::UNIT_X;

}

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

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 的含义3

Yaw – Vertical axis：

 yaw

Pitch – Lateral axis

 pitch

Roll – Longitudinal axis

 roll

The Position of All three axes

 Yaw Pitch Roll

#### 四元数与欧拉角之间的转换

arctanarcsin的结果是，这并不能覆盖所有朝向(对于的取值范围已经满足)，因此需要用atan2来代替arctan

http://www.cppblog.com/Files/heath/Euler2Quaternion.rar
Demo渲染两个模型，左边使用欧拉角，右边使用四元数，方向键Up、Left、Right旋转模型。

Spatial rotations in three dimensions can be parametrized using both Euler angles and unit quaternions. This article explains how to convert between the two representations. Actually this simple use of “quaternions” was first presented by Euler some seventy years earlier than Hamilton to solve the problem of magic squares. For this reason the dynamics community commonly refers to quaternions in this application as “Euler parameters”.

[hide]

##  Definition

A unit quaternion can be described as:

$\mathbf{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$

$|\mathbf{q}|^2 = q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1$

We can associate a quaternion with a rotation around an axis by the following expression

$\mathbf{q}_0 = \cos(\alpha/2)$

$\mathbf{q}_1 = \sin(\alpha/2)\cos(\beta_x)$

$\mathbf{q}_2 = \sin(\alpha/2)\cos(\beta_y)$

$\mathbf{q}_3 = \sin(\alpha/2)\cos(\beta_z)$

where α is a simple rotation angle (the value in radians of the angle of rotation) and cos(βx), cos(βy) and cos(βz) are the “direction cosines” locating the axis of rotation (Euler’s Theorem).

##  Rotation matrices

Euler angles – The xyz (fixed) system is shown in blue, the XYZ (rotated) system is shown in red. The line of nodes, labelled N, is shown in green.

The orthogonal matrix (post-multiplying a column vector) corresponding to a clockwise/left-handed rotation by the unit quaternion $q=q_0+iq_1+jq_2+kq_3$ is given by the inhomogeneous expression

$\begin{bmatrix} 1- 2(q_2^2 + q_3^2) & 2(q_1 q_2 - q_0 q_3) & 2(q_0 q_2 + q_1 q_3) \\ 2(q_1 q_2 + q_0 q_3) & 1 - 2(q_1^2 + q_3^2) & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2( q_0 q_1 + q_2 q_3) & 1 - 2(q_1^2 + q_2^2) \end{bmatrix}$

or equivalently, by the homogeneous expression

$\begin{bmatrix} q_0^2 + q_1^2 - q_2^2 - q_3^2 & 2(q_1 q_2 - q_0 q_3) & 2(q_0 q_2 + q_1 q_3) \\ 2(q_1 q_2 + q_0 q_3) & q_0^2 - q_1^2 + q_2^2 - q_3^2 & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2( q_0 q_1 + q_2 q_3) & q_0^2 - q_1^2 - q_2^2 + q_3^2 \end{bmatrix}$

If $q_0+iq_1+jq_2+kq_3$ is not a unit quaternion then the homogeneous form is still a scalar multiple of a rotation matrix, while the inhomogeneous form is in general no longer an orthogonal matrix. This is why in numerical work the homogeneous form is to be preferred if distortion is to be avoided.

The orthogonal matrix (post-multiplying a column vector) corresponding to a clockwise/left-handed rotation with Euler angles φ, θ, ψ, with x-y-z convention, is given by:

$\begin{bmatrix} \cos\theta \cos\psi & -\cos\phi \sin\psi + \sin\phi \sin\theta \cos\psi & \sin\phi \sin\psi + \cos\phi \sin\theta \cos\psi \\ \cos\theta \sin\psi & \cos\phi \cos\psi + \sin\phi \sin\theta \sin\psi & -\sin\phi \cos\psi + \cos\phi \sin\theta \sin\psi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \\ \end{bmatrix}$

###  Conversion

By combining the quaternion representations of the Euler rotations we get

$\mathbf{q} = R_z(\psi)R_y(\theta)R_x(\phi) = [\cos (\psi /2) + \mathbf{k}\sin (\psi /2)][\cos (\theta /2) + \mathbf{j}\sin (\theta /2)][\cos (\phi /2) + \mathbf{i}\sin (\phi /2)]$
$\mathbf{q} = \begin{bmatrix} \cos (\phi /2) \cos (\theta /2) \cos (\psi /2) + \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin (\phi /2) \cos (\theta /2) \cos (\psi /2) - \cos (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) + \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos (\theta /2) \sin (\psi /2) - \sin (\phi /2) \sin (\theta /2) \cos (\psi /2) \\ \end{bmatrix}$

For Euler angles we get:

$\begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} \mbox{arctan} \frac {2(q_0 q_1 + q_2 q_3)} {1 - 2(q_1^2 + q_2^2)} \\ \mbox{arcsin} (2(q_0 q_2 - q_3 q_1)) \\ \mbox{arctan} \frac {2(q_0 q_3 + q_1 q_2)} {1 - 2(q_2^2 + q_3^2)} \end{bmatrix}$

arctan and arcsin have a result between −π/2 and π/2. With three rotations between −π/2 and π/2 you can’t have all possible orientations. You need to replace the arctan by atan2 to generate all the orientations.

$\begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} \mbox{atan2} (2(q_0 q_1 + q_2 q_3),1 - 2(q_1^2 + q_2^2)) \\ \mbox{arcsin} (2(q_0 q_2 - q_3 q_1)) \\ \mbox{atan2} (2(q_0 q_3 + q_1 q_2),1 - 2(q_2^2 + q_3^2)) \end{bmatrix}$

##  Relationship with Tait–Bryan angles

Tait–Bryan angles for an aircraft

Similarly for Euler angles, we use the Tait–Bryan angles (in terms of flight dynamics):

• Roll – $\phi$: rotation about the X-axis
• Pitch – $\theta$: rotation about the Y-axis
• Yaw – $\psi$: rotation about the Z-axis

where the X-axis points forward, Y-axis to the right and Z-axis downward and in the example to follow the rotation occurs in the order yaw, pitch, roll (about body-fixed axes).

##  Singularities

One must be aware of singularities in the Euler angle parametrization when the pitch approaches ±90° (north/south pole). These cases must be handled specially. The common name for this situation is gimbal lock.

Code to handle the singularities is derived on this site: www.euclideanspace.com