新建網頁 1
cQuaternion類用來以四元數形式保存方位或角位移，在能應用到四元數上的完整數學運算集合中，只有那些對單位四元數有意義的運算才對保存角位移有用，這里沒有提供四元數的求負、加減、標量乘、對數操作。
Quaternion.h:
#ifndef QUATERNION_H
#define QUATERNION_H
class cVector3;
class cEulerAngles;
//---------------------------------------------------------------------------
// Implement a quaternion, for purposes of representing an angular
// displacement (orientation) in 3D.
//---------------------------------------------------------------------------
class cQuaternion
{
public:
// The 4 values of the quaternion. Normally, it will not be necessary to manipulate these
// directly. However, we leave them public, since prohibiting direct access
// makes some operations, such as file I/O, unnecessarily complicated.
float w, x, y, z;
public:
void identity()
{
w = 1.0f;
x = y = z = 0.0f;
}
// setup the quaternion to a specific rotation
void set_to_rotate_about_x(float theta);
void set_to_rotate_about_y(float theta);
void set_to_rotate_about_z(float theta);
void set_to_rotate_about_axis(const cVector3& axis, float theta);
// setup to perform object<->inertial rotations, given orientation in Euler angle format.
void set_to_rotate_object_to_inertial(const cEulerAngles& orientation);
void set_to_rotate_inertial_to_object(const cEulerAngles& orientation);
// cross product
cQuaternion operator *(const cQuaternion& a) const;
// multiplication with assignment, as per c++ convention.
cQuaternion& operator *=(const cQuaternion& a);
void normalize();
// extract and return the rotation angle and axis
float get_rotation_angle() const;
cVector3 get_rotation_axis() const;
};
extern const cQuaternion g_quat_identity;
float dot_product(const cQuaternion& a, const cQuaternion& b);
cQuaternion slerp(const cQuaternion& q0, const cQuaternion& q1, float t);
cQuaternion conjugate(const cQuaternion& q);
cQuaternion pow(const cQuaternion& q, float exponent);
#endif
為了創建一個代表特定角位移的四元數，需要使用set_to_xxx
函數中的一個。set_to_rotate_object_to_inertial()和set_to_rotate_inertial_to_object()用來將歐拉角轉換到四元數形式。第一個函數創建一個四元數，表達從物體空間到慣性空間的旋轉，后一個函數返回從慣性空間到物體空間的旋轉。
一般使用函數來操作角位移，角位移連接使用operator*()（習慣上，連接順序從左向右）。conjugate()函數返回一個四元數，該四元數代表的角位移與輸入四元數代表的角位移相反。
使用get_rotation_angle()和get_rotation_axis()可從四元數中提取旋轉角和旋轉軸。
normalize()用來處理浮點數誤差擴大。如果要對同一四元數執行上百次連續運算，就可能需要調用這個方法。雖然歐拉角向四元數的轉換只產生單位化的四元數，避免了誤差擴大的可能。但是，矩陣和四元數間的轉換卻存在這一問題。
Quaternion.cpp:
#include <assert.h>
#include <math.h>
#include "Quaternion.h"
#include "MathUtil.h"
#include "vector3.h"
#include "EulerAngles.h"
// The global identity quaternion. Notice that there are no constructors
// to the Quaternion class, since we really don't need any.
const cQuaternion g_quat_identity = { 1.0f, 0.0f, 0.0f, 0.0f };
//---------------------------------------------------------------------------
// Setup the quaternion to rotate about the specified axis
//---------------------------------------------------------------------------
void cQuaternion::set_to_rotate_about_x(float theta)
{
float half_theta = theta * 0.5f;
w = cos(half_theta);
x = sin(half_theta);
y = 0.0f;
z = 0.0f;
}
void cQuaternion::set_to_rotate_about_y(float theta)
{
float half_theta = theta * 0.5f;
w = cos(half_theta);
x = 0.0f;
y = sin(half_theta);
z = 0.0f;
}
void cQuaternion::set_to_rotate_about_z(float theta)
{
float half_theta = theta * 0.5f;
w = cos(half_theta);
x = 0.0f;
y = 0.0f;
z = sin(half_theta);
}
void cQuaternion::set_to_rotate_about_axis(const cVector3& axis, float theta)
{
// the axis of rotation must be normalized
assert(fabs(vector_mag(axis) - 1.0f) < 0.01f);
// compute the half angle and its sin
float half_theta = theta * 0.5f;
float sin_half_theta = sin(half_theta);
w = cos(half_theta);
x = axis.x * sin_half_theta;
y = axis.y * sin_half_theta;
z = axis.z * sin_half_theta;
}
//---------------------------------------------------------------------------
// Setup the quaternion to perform an object->inertial rotation, given the
// orientation in Euler angle format.
//
// | cos(h/2)cos(p/2)cos(b/2) + sin(h/2)sin(p/2)sin(b/2) |
// M = | cos(h/2)sin(p/2)cos(b/2) + sin(h/2)cos(p/2)sin(b/2) |
// | sin(h/2)cos(p/2)cos(b/2) - cos(h/2)sin(p/2)sin(b/2) |
// | cos(h/2)cos(p/2)sin(b/2) - sin(h/2)sin(p/2)cos(b/2) |
//---------------------------------------------------------------------------
void cQuaternion::set_to_rotate_object_to_inertial(const cEulerAngles& orientation)
{
// compute sine and cosine of the half angles
float sp, sb, sh;
float cp, cb, ch;
sin_cos(&sp, &cp, orientation.pitch * 0.5f);
sin_cos(&sb, &cb, orientation.bank * 0.5f);
sin_cos(&sh, &ch, orientation.heading * 0.5f);
w = ch * cp * cb + sh * sp * sb;
x = ch * sp * cb + sh * cp * sb;
y = -ch * sp * sb + sh * cp * cb;
z = -sh * sp * cb + ch * cp * sb;
}
//---------------------------------------------------------------------------
// Setup the quaternion to perform an object->inertial rotation, given the
// orientation in Euler angle format.
//
// | cos(h/2)cos(p/2)cos(b/2) + sin(h/2)sin(p/2)sin(b/2) |
// M = | -cos(h/2)sin(p/2)cos(b/2) - sin(h/2)cos(p/2)sin(b/2) |
// | cos(h/2)sin(p/2)sin(b/2) - sin(h/2)cos(p/2)cos(b/2) |
// | sin(h/2)sin(p/2)cos(b/2) - cos(h/2)cos(p/2)sin(b/2) |
//---------------------------------------------------------------------------
void cQuaternion::set_to_rotate_inertial_to_object(const cEulerAngles& orientation)
{
// compute sine and cosine of the half angles
float sp, sb, sh;
float cp, cb, ch;
sin_cos(&sp, &cp, orientation.pitch * 0.5f);
sin_cos(&sb, &cb, orientation.bank * 0.5f);
sin_cos(&sh, &ch, orientation.heading * 0.5f);
w = ch * cp * cb + sh * sp * sb;
x = -ch * sp * cb - sh * cp * sb;
y = ch * sp * sb - sh * cp * cb;
z = sh * sp * cb - ch * cp * sb;
}
//---------------------------------------------------------------------------
// Quaternion cross product, which concatenates multiple angular
// displacements. The order of multiplication, from left to right,
// corresponds to the order that the angular displacements are
// applied. This is backwards from the *standard* definition of
// quaternion multiplication.
//---------------------------------------------------------------------------
cQuaternion cQuaternion::operator *(const cQuaternion& a) const
{
cQuaternion result;
result.w = w * a.w - x * a.x - y * a.y - z * a.z;
result.x = w * a.x + x * a.w + z * a.y - y * a.z;
result.y = w * a.y + y * a.w + x * a.z - z * a.x;
result.z = w * a.z + z * a.w + y * a.x - x * a.y;
return result;
}
//---------------------------------------------------------------------------
// Combined cross product and assignment, as per C++ convention.
//---------------------------------------------------------------------------
cQuaternion& cQuaternion::operator *=(const cQuaternion& a)
{
*this = *this * a;
return *this;
}
//---------------------------------------------------------------------------
// "Normalize" a quaternion. Note that normally, quaternions
// are always normalized (within limits of numerical precision).
//
// This function is provided primarily to combat floating point "error
// creep," which can occur when many successive quaternion operations
// are applied.
//---------------------------------------------------------------------------
void cQuaternion::normalize()
{
// compute magnitude of the quaternion
float mag = sqrt(w * w + x * x + y * y + z * z);
// check for bogus length, to protect against divide by zero.
if(mag > 0.0f)
{
// normalize it
float one_over_mag = 1.0f / mag;
w *= one_over_mag;
x *= one_over_mag;
y *= one_over_mag;
z *= one_over_mag;
}
else
{
// houston, we have a problem.
assert(false);
// in a release build, just slam it to something.
identity();
}
}
//---------------------------------------------------------------------------
// Return the rotation angle theta
//---------------------------------------------------------------------------
float cQuaternion::get_rotation_angle() const
{
// compute the half angle, remember that w = cos(theta / 2)
float half_theta = safe_acos(w);
return half_theta * 2.0f;
}
//---------------------------------------------------------------------------
// Return the rotation axis
//---------------------------------------------------------------------------
cVector3 cQuaternion::get_rotation_axis() const
{
// compute sin^2(theta/2), remember that w = cos(theta/2), and sin^2(x) + cos^2(x) = 1.
float sin_theta_square = 1.0f - w * w;
// protect against numerical imprecision
if(sin_theta_square <= 0.0f)
{
// identity quaterion, or numerical imprecision.
// just return any valid vector, since it does not matter.
return cVector3(1.0f, 0.0f, 0.0f);
}
// compute 1 / sin(theta/2)
float k = 1.0f / sqrt(sin_theta_square);
// return axis of rotation
return cVector3(x * k, y * k, z * k);
}
////////////////////////////////////// Nonmember functions ////////////////////////////////////////
//---------------------------------------------------------------------------
// Quaternion dot product. We use a nonmember function so we can
// pass quaternion expressions as operands without having "funky syntax"
//---------------------------------------------------------------------------
float dot_product(const cQuaternion& a, const cQuaternion& b)
{
return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
}
//---------------------------------------------------------------------------
// Spherical linear interpolation.
//---------------------------------------------------------------------------
cQuaternion slerp(const cQuaternion& q0, const cQuaternion& q1, float t)
{
// check for out-of range parameter and return edge points if so
if(t <= 0.0f) return q0;
if(t >= 1.0f) return q1;
// compute "cosine of angle between quaternions" using dot product
float cos_omega = dot_product(q0, q1);
// If negative dot, use -q1. Two quaternions q and -q
// represent the same rotation, but may produce different slerp.
// We chose q or -q to rotate using the acute angle.
float q1w = q1.w;
float q1x = q1.x;
float q1y = q1.y;
float q1z = q1.z;
if(cos_omega < 0.0f)
{
q1w = -q1w;
q1x = -q1x;
q1y = -q1y;
q1z = -q1z;
cos_omega = -cos_omega;
}
// we should have two unit quaternions, so dot should be <= 1.0
assert(cos_omega < 1.1f);
// compute interpolation fraction, checking for quaternions almost exactly the same.
float k0, k1;
if(cos_omega > 0.9999f)
{
// very close - just use linear interpolation, which will protect against a divide by zero.
k0 = 1.0f - t;
k1 = t;
}
else
{
// compute the sin of the angle using the trig identity sin^2(omega) + cos^2(omega) = 1
float sin_omega = sqrt(1.0f - cos_omega * cos_omega);
// compute the angle from its sin and cosin
float omega = atan2(sin_omega, cos_omega);
// compute inverse of denominator, so we only have to divice once.
float k = 1.0f / sin_omega;
// compute interpolation perameters
k0 = sin((1.0f - t) * omega) * k;
k1 = sin(t * omega) * k;
}
cQuaternion result;
result.x = k0 * q0.x + k1 * q1x;
result.y = k0 * q0.y + k1 * q1y;
result.z = k0 * q0.z + k1 * q1z;
result.w = k0 * q0.w + k1 * q1w;
return result;
}
//---------------------------------------------------------------------------
// Compute the quaternion conjugate. This is the quaternian
// with the opposite rotation as the original quaternian.
//---------------------------------------------------------------------------
cQuaternion conjugate(const cQuaternion& q)
{
cQuaternion result;
// same rotation amount
result.w = q.w;
// opposite axis of rotation
result.x = -q.x;
result.y = -q.y;
result.z = -q.z;
return result;
}
//---------------------------------------------------------------------------
// Quaternion exponentiation.
//---------------------------------------------------------------------------
cQuaternion pow(const cQuaternion& q, float exponent)
{
// check for the case of an identity quaternion.
// this will protect against divide by zero.
if(fabs(q.w) > 0.9999f)
return q;
// extract the half angle alpha (alpha = theta/2)
float alpha = acos(q.w);
// compute new alpha value
float new_alpha = alpha * exponent;
// compute new w value
cQuaternion result;
result.w = cos(new_alpha);
// compute new xyz values
float mult = sin(new_alpha) / sin(alpha);
result.x = q.x * mult;
result.y = q.y * mult;
result.z = q.z * mult;
return result;
}