數學理論基礎請參閱3D中的方位和角位移。
處理變換是一件非常令人頭疼的事，矩陣更是棘手。如果你曾經編寫過關于矩陣的代碼并且沒有用設計良好的類，你會發現經常要處理負號、轉置矩陣或翻轉連接順序以使其能正常工作。
下面這幾個類正是為了消除在編程中經常遇到的這類問題而設計的。例如，很少需要直接訪問矩陣或四元數中的元素，因此特意限制了可用操作的數目以避免產生迷惑，再如，對cRotationMatrix類，沒有求逆和連接操作，因為如果按其本身的目的使用cRotationMatrix，這些操作是不應該出現或沒有意義的。
我們還使用了一系列簡單、常用的數學常數和實用工具函數，它們由MathUtil.h和MathUtil.cpp提供。
MathUtil.h：
#ifndef MATH_UTIL_H
#define MATH_UTIL_H
#include <math.h>
// declare a global constant for pi and a few multiples.
const float G_PI = 3.14159265f;
const float G_2PI = G_PI * 2.0f;
const float G_PI_OVER_2 = G_PI / 2.0f;
const float G_1_OVER_PI = 1.0f / G_PI;
const float G_1_OVER_2PI = 1.0f / G_2PI;
const float G_PI_OVER_180 = G_PI / 180.0f;
const float G_180_OVER_PI = 180.0f / G_PI;
float wrap_pi(float theta);
float safe_acos(float x);
// convert between degrees and radians
inline float deg_to_rad(float deg) { return deg * G_PI_OVER_180; }
inline float rad_to_deg(float rad) { return rad * G_180_OVER_PI; }
// compute the sin and cosine of an angle. on some platforms, if we know that we need
// both values, it can be computed faster than computing the two values seperately.
inline void sin_cos(float* ret_sin, float* ret_cos, float theta)
{
// for simplicity, we will just use the normal trig functions.
// note that on some platforms we may be able to do better.
*ret_sin = sin(theta);
*ret_cos = cos(theta);
}
// convert between "field of view" and "zoom", the fov angle is speficied in radians.
inline float fov_to_zoom(float fov) { return 1.0f / tan(fov * 0.5f); }
inline float zoom_to_fov(float zoom) { return 2.0f * atan(1.0f / zoom); }
#endif
MathUtil.cpp:
關于類cVector3的實現細節請參閱一個3D向量類。
#include "MathUtil.h"
#include "vector3.h"
const cVector3 g_zero_vector(0.0f, 0.0f, 0.0f);
float wrap_pi(float theta)
{
// "wrap" an angle in range -pi
pi by adding the correct multiple of 2 pi
theta += G_PI;
theta -= floor(theta * G_1_OVER_2PI) * G_2PI;
theta -= G_PI;
return theta;
}
float safe_acos(float x)
{
// Same as acos(x), but if x is out of range, it is "clamped" to the nearest valid value.
// The value returned is in range 0pi, the same as the standard C acos() function.
// check limit conditions
if(x <= -1.0f)
return G_PI;
if(x >= 1.0f)
return 0.0f;
// value is in the domain - use standard C function.
return acos(x);
}
cEulerAngles類：
cEulerAngles類用來以歐拉角形式保存方位，使用heading-pitch-bank約定。這個類非常直觀，為了簡單起見，我們沒有實現太多操作。特別沒有實現加、減、標量乘等運算。因為如果該類保存的不是方位而是角速度或變化率，那么這些運算才是有用的。
EulerAngles.h:
#ifndef EULER_ANGLES_H
#define EULER_ANGLES_H
class cQuaternion;
class cMatrix4x3;
class cRotationMatrix;
//---------------------------------------------------------------------------
// This class represents a heading-pitch-bank Euler angle triple.
//---------------------------------------------------------------------------
class cEulerAngles
{
public:
// store three angles, in radians.
float heading;
float pitch;
float bank;
public:
cEulerAngles() {}
cEulerAngles(float h, float p, float b)
{
heading = h;
pitch = p;
bank = b;
}
// set to identity triple (all zeors)
void identity()
{
pitch = bank = heading = 0.0f;
}
void canonize();
void from_object_to_inertial_quat(const cQuaternion& q);
void from_inertial_to_object_quat(const cQuaternion& q);
void from_object_to_world_matrix(const cMatrix4x3& m);
void from_world_to_object_matrix(const cMatrix4x3& m);
void from_rotation_matrix(const cRotationMatrix& m);
};
extern const cEulerAngles g_euler_angles_identity;
#endif
cEulerAngles類的用法也很直觀，只有幾個地方需要加以詳細說明：
（1）canonize()函數的作用是確保歐拉角位于"限制集"中。
（2）from_object_to_inertial_quat()和from_inertial_to_object_quat()函數根據四元數計算歐拉角，第一個函數的參數是代表從物體坐標系到慣性坐標系旋轉的四元數，第二個函數的參數是代表從慣性坐標系到物體坐標系旋轉的四元數。
（3）同樣，from_object_to_world_matrix()和from_world_to_object_matrix()函數把矩陣的旋轉部分的方位轉換為歐拉角，假設這個被轉換的矩陣是正交的。
EulerAngles.cpp:
#include <math.h>
#include "EulerAngles.h"
#include "Quaternion.h"
#include "MathUtil.h"
#include "Matrix4x3.h"
#include "RotationMatrix.h"
const cEulerAngles g_euler_angles_identity(0.0f, 0.0f, 0.0f);
//---------------------------------------------------------------------------
// Set the Euler angle triple to its "canonical" value. This does not change
// the meaning of the Euler angles as a representation of Orientation in 3D,
// but if the angles are for other purposes such as angular velocities, etc,
// then the operation might not be valid.
//---------------------------------------------------------------------------
void cEulerAngles::canonize()
{
// first, wrap pitch in range -pi pi
pitch = wrap_pi(pitch);
// now, check for "the back side" of the matrix, pitch outside the canonical range
// of -pi/2 pi/2
if(pitch < -G_PI_OVER_2)
{
pitch = -G_PI - pitch;
heading += G_PI;
bank += G_PI;
}
else if(pitch > G_PI_OVER_2)
{
pitch = G_PI - pitch;
heading += G_PI;
bank += G_PI;
}
// ok, now check for the gimbel lock case (within a slight tolerance)
if(fabs(pitch) > G_PI_OVER_2 - 1e-4)
{
// we are in gimbel lock, assign all rotation about the vertical axis to heading.
heading += bank;
bank = 0.0f;
}
else
{
// not in gimbel lock, wrap the bank angle in canonical range.
bank = wrap_pi(bank);
}
// wrap heading in canonical range
heading = wrap_pi(heading);
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given an object->inertial rotation quaternion.
//
// p = asin(-m23) = asin(-2(yz - wx))
//
// h = atan2(m13, m33) = atan2(xz + wy, 1/2 - x^2 - y^2) cosp != 0
// h = atan2(-m31, m11) = atan2(-xz + wy, 1/2 - y^2 - z^2) cosp == 0
//
// b = atan2(m21, m22) = atan2(xy + wz, 1/2 - x^2 - z^2) cosp != 0
// b = 0 cosp == 0
//---------------------------------------------------------------------------
void cEulerAngles::from_object_to_inertial_quat(const cQuaternion& q)
{
// extract sin(pitch)
float sin_pitch = -2.0f * (q.y * q.z - q.w * q.x);
// check for gimbel lock, giving slight tolerance for numerical imprecision.
if(fabs(sin_pitch) > 0.9999f)
{
// looking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-q.x * q.z + q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
// compute angles, we do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
pitch = asin(sin_pitch);
heading = atan2(q.x * q.z + q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = atan2(q.x * q.y + q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given an inertial->object rotation quaternion.
//
// p = asin(-m23) = asin(-2(yz + wx))
//
// h = atan2(m13, m33) = atan2(xz - wy, 1/2 - x^2 - y^2) cosp != 0
// h = atan2(-m31, m11) = atan2(-xz - wy, 1/2 - y^2 - z^2) cosp == 0
//
// b = atan2(m21, m22) = atan2(xy - wz, 1/2 - x^2 - z^2) cosp != 0
// b = 0 cosp == 0
//---------------------------------------------------------------------------
void cEulerAngles::from_inertial_to_object_quat(const cQuaternion& q)
{
// extract sin(pitch)
float sin_pitch = -2.0f * (q.y * q.z + q.w * q.x);
// check for gimbel lock, giving slight tolerance for numerical imprecision.
if(fabs(sin_pitch) > 0.9999f)
{
// looking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-q.x * q.z - q.w * q.y, 0.5f - q.y * q.y - q.z * q.z);
bank = 0.0f;
}
else
{
// compute angles, we do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
pitch = asin(sin_pitch);
heading = atan2(q.x * q.z - q.w * q.y, 0.5f - q.x * q.x - q.y * q.y);
bank = atan2(q.x * q.y - q.w * q.z, 0.5f - q.x * q.x - q.z * q.z);
}
}
//------------------------------------------------------------------------------------------------
// Setup the Euler angles, given an object->world transformation matrix.
// The matrix is assumed to be orthogonal. The translation portion is ignored.
//
// | cosh * cosb + sinh * sinp * sinb sinb * cosp -sinh * cosb + cosh * sinp * sinb |
// M = | -cosh * sinb + sinh * sinp * cosb cosb * cosp sinb * sinh + cosh * sinp * cosb |
// | sinh * cosp -sinp cosh * cosp |
//
// [1]: cosp != 0
//
// p = asin(-m32)
// h = atan2(m31, m33)
// b = atan2(m12, m22)
//
// [2]: cosp = 0, b = 0, sinb = 0, cosb = 1
//
// | cosh 0 -sinh |
// M = | sinh * sinp 0 cosh * sinp |
// | 0 -sinp 0 |
//
// p = pi/2 * (-m32)
// h = atan2(-m13, m11)
// b = 0
//------------------------------------------------------------------------------------------------
void cEulerAngles::from_object_to_world_matrix(const cMatrix4x3& m)
{
// extract sin(pitch) from m32
float sin_pitch = -m.m32;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m13, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
heading = atan2(m.m31, m.m33);
pitch = asin(sin_pitch);
bank = atan2(m.m12, m.m22);
}
}
//-----------------------------------------------------------------------------------------------------
// Setup the Euler angles, given a world->object transformation matrix.
// The matrix is assumed to be orthogonal. The translation portion is ignored.
//
// | cosh * cosb + sinh * sinp * sinb -cosh * sinb + sinh * sinp * cosb sinh * cosp |
// M = | sinb * cosp cosb * cosp -sinp |
// | -sinh * cosb + cosh * sinp * sinb sinb * sinh + cosh * sinp * cosb cosh * cosp |
//
// [1]: cosp != 0
//
// p = asin(-m23)
// h = atan2(m13, m33)
// b = atan2(m21, m22)
//
// [2]: cosp = 0, b = 0, sinb = 0, cosb = 1
//
// | cosh sinh * sinp 0 |
// M = | 0 0 -sinp |
// | -sinh cosh * sinp 0 |
//
// p = pi/2 * (-m23)
// h = atan2(-m31, m11)
// b = 0
//-----------------------------------------------------------------------------------------------------
void cEulerAngles::from_world_to_object_matrix(const cMatrix4x3& m)
{
// extract sin(pitch) from m23
float sin_pitch = -m.m23;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m31, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
heading = atan2(m.m13, m.m33);
pitch = asin(sin_pitch);
bank = atan2(m.m21, m.m22);
}
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given a rotation matrix.
//---------------------------------------------------------------------------
void cEulerAngles::from_rotation_matrix(const cRotationMatrix& m)
{
// extract sin(pitch) from m23
float sin_pitch = -m.m23;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m31, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
heading = atan2(m.m13, m.m33);
pitch = asin(sin_pitch);
bank = atan2(m.m21, m.m22);
}
}
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;
}
cRotationMatrix類的目的就是處理非常特殊的（也是極其常用的）物體和慣性坐標空間之間的旋轉。這個矩陣類不是一般的變換類，我們假定這個類只包含旋轉，因此，它是正交的。換句話說，該矩陣表達的是方位，而不是角位移。當你創建這樣的矩陣時，不必指定變換的方向（物體坐標空間到慣性坐標空間或是慣性坐標空間到物體坐標空間）。變換的方向在實際執行變換時指定，每個方向對應一個函數。
RotationMatrix.h:
#ifndef ROTATION_MATRIX_H
#define ROTATION_MATRIX_H
class cVector3;
class cEulerAngles;
class cQuaternion;
//---------------------------------------------------------------------------
// Implement a simple 3x3 matrix that is used for ROTATION ONLY. The
// matrix is assumed to be orthogonal. The direction of transformation
// is specified at the time of transformation.
//---------------------------------------------------------------------------
class cRotationMatrix
{
public:
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
public:
void identity();
// setup the matrix with a specified orientation
void setup(const cEulerAngles& orientation);
// setup the matrix from a quaternion, assuming the quaternion performs the
// rotation in the specified direction of transformation.
void from_inertial_to_object_quat(const cQuaternion& q);
void from_object_to_inertial_quat(const cQuaternion& q);
// perform rotations
cVector3 inertial_to_object(const cVector3& v) const;
cVector3 object_to_inertial(const cVector3& v) const;
};
#endif
因為cRotationMatrix
類很簡單，因此也非常容易使用。首先，用歐拉角或四元數設置矩陣。如果使用四元數，還必須指明該四元數代表哪種角位移。一旦創建了矩陣，就能用inertial_to_object()和object_to_inertial()函數執行旋轉。
cRotationMatrix.cpp：
#include "vector3.h"
#include "RotationMatrix.h"
#include "MathUtil.h"
#include "Quaternion.h"
#include "EulerAngles.h"
/////////////////////////////////////////////////////////////////////////////
//
// MATRIX ORGANIZATION
//
// A user of this class should rarely care how the matrix is organized.
// However, it is of course important that internally we keep everything
// straight.
//
// The matrix is assumed to be a rotation matrix only, and therefore
// orthoganal. The "forward" direction of transformation (if that really
// even applies in this case) will be from inertial to object space.
// To perform an object->inertial rotation, we will multiply by the
// transpose.
//
// In other words:
//
// Inertial to object:
//
// | m11 m12 m13 |
// [ ix iy iz ] | m21 m22 m23 | = [ ox oy oz ]
// | m31 m32 m33 |
//
// Object to inertial:
//
// | m11 m21 m31 |
// [ ox oy oz ] | m12 m22 m32 | = [ ix iy iz ]
// | m13 m23 m33 |
//
// Or, using column vector notation:
//
// Inertial to object:
//
// | m11 m21 m31 | | ix | | ox |
// | m12 m22 m32 | | iy | = | oy |
// | m13 m23 m33 | | iz | | oz |
//
// Object to inertial:
//
// | m11 m12 m13 | | ox | | ix |
// | m21 m22 m23 | | oy | = | iy |
// | m31 m32 m33 | | oz | | iz |
//
/////////////////////////////////////////////////////////////////////////////
//---------------------------------------------------------------------------
// Set the matrix to the identity matrix
//---------------------------------------------------------------------------
void cRotationMatrix::identity()
{
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
}
//-----------------------------------------------------------------------------------------------------
// Setup the matrix with the specified orientation
//
// | cosh * cosb + sinh * sinp * sinb -cosh * sinb + sinh * sinp * cosb sinh * cosp |
// M = | sinb * cosp cosb * cosp -sinp |
// | -sinh * cosb + cosh * sinp * sinb sinb * sinh + cosh * sinp * cosb cosh * cosp |
//-----------------------------------------------------------------------------------------------------
void cRotationMatrix::setup(const cEulerAngles& orientation)
{
// Fetch sine and cosine of angles
float sh,ch, sp,cp, sb,cb;
sin_cos(&sh, &ch, orientation.heading);
sin_cos(&sp, &cp, orientation.pitch);
sin_cos(&sb, &cb, orientation.bank);
// Fill in the matrix elements
m11 = ch * cb + sh * sp * sb;
m12 = -ch * sb + sh * sp * cb;
m13 = sh * cp;
m21 = sb * cp;
m22 = cb * cp;
m23 = -sp;
m31 = -sh * cb + ch * sp * sb;
m32 = sb * sh + ch * sp * cb;
m33 = ch * cp;
}
//-----------------------------------------------------------------------------------------
// Setup the matrix, given a quaternion that performs an inertial->object rotation.
//
// | 1 - 2(y^2 + z^2) 2(xy + wz) 2(xz - wy) |
// M = | 2(xy - wz) 1 - 2(x^2 + z^2) 2(yz + wx) |
// | 2(xz + wy) 2(yz - wx) 1 - 2(x^2 + y^2) |
//-----------------------------------------------------------------------------------------
void cRotationMatrix::from_inertial_to_object_quat(const cQuaternion& q)
{
// Fill in the matrix elements. This could possibly be optimized since there are
// many common subexpressions. We'll leave that up to the compiler
m11 = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
m12 = 2.0f * (q.x * q.y + q.w * q.z);
m13 = 2.0f * (q.x * q.z - q.w * q.y);
m21 = 2.0f * (q.x * q.y - q.w * q.z);
m22 = 1.0f - 2.0f * (q.x * q.x + q.z * q.z);
m23 = 2.0f * (q.y * q.z + q.w * q.x);
m31 = 2.0f * (q.x * q.z + q.w * q.y);
m32 = 2.0f * (q.y * q.z - q.w * q.x);
m33 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
}
//-----------------------------------------------------------------------------------------
// Setup the matrix, given a quaternion that performs an object->inertial rotation.
//
// | 1 - 2(y^2 + z^2) 2(xy - wz) 2(xz + wy) |
// M = | 2(xy + wz) 1 - 2(x^2 + z^2) 2(yz - wx) |
// | 2(xz - wy) 2(yz + wx) 1 - 2(x^2 + y^2) |
//-----------------------------------------------------------------------------------------
void cRotationMatrix::from_object_to_inertial_quat(const cQuaternion& q)
{
// Fill in the matrix elements. This could possibly be optimized since there are
// many common subexpressions. We'll leave that up to the compiler
m11 = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
m12 = 2.0f * (q.x * q.y - q.w * q.z);
m13 = 2.0f * (q.x * q.z + q.w * q.y);
m21 = 2.0f * (q.x * q.y + q.w * q.z);
m22 = 1.0f - 2.0f * (q.x * q.x + q.z * q.z);
m23 = 2.0f * (q.y * q.z - q.w * q.x);
m31 = 2.0f * (q.x * q.z - q.w * q.y);
m32 = 2.0f * (q.y * q.z + q.w * q.x);
m33 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
}
//---------------------------------------------------------------------------
// Rotate a vector from inertial to object space
//---------------------------------------------------------------------------
cVector3 cRotationMatrix::inertial_to_object(const cVector3& v) const
{
// perform the matrix multiplication in the "standard" way
return cVector3(m11 * v.x + m21 * v.y + m31 * v.z,
m12 * v.x + m22 * v.y + m32 * v.z,
m13 * v.x + m23 * v.y + m33 * v.z);
}
//---------------------------------------------------------------------------
// Rotate a vector from object to inertial space
//---------------------------------------------------------------------------
cVector3 cRotationMatrix::object_to_inertial(const cVector3& v) const
{
// Multiply by the transpose
return cVector3(m11 * v.x + m12 * v.y + m13 * v.z,
m21 * v.x + m22 * v.y + m23 * v.z,
m31 * v.x + m32 * v.y + m33 * v.z);
}
cRotationMatrix就其特殊目的來說是稱職的，但也正因為如此，它的廣泛應用受到了限制。cMatrix4x3類是一個更加一般化的矩陣，它被用來處理更加復雜的變換。這個矩陣類保存了一個一般仿射變換矩陣。旋轉、縮放、鏡像、投影和平移變換它都支持，該矩陣還能求逆和組合。
因此，cMatrix4x3類的語義和cRotationMatrix類完全不同。cRotationMatrix僅應用于特殊的物體空間和慣性空間，而cMatrix4x3有更一般的應用，所以我們使用更一般化的術語"源"和"目標"坐標空間。和cRotationMatrix不一樣，它的變換方向是在矩陣創建時指定的，之后點只能向那個方向（源到目標）變換。如果要向相反的方向變換，須先計算逆矩陣。
這里使用線性代數的乘法記法，operator*()被同時用來變換點和組合矩陣。因為我們的約定是行向量不是列向量，變換的順序和讀句子一樣，從左向右。
Matrix4x3.h:
#ifndef MATRIX_4X3_H
#define MATRIX_4X3_H
class cVector3;
class cEulerAngles;
class cQuaternion;
class cRotationMatrix;
#define ROTATE_AROUND_X 1
#define ROTATE_AROUND_Y 2
#define ROTATE_AROUND_Z 3
#define SHERE_AROUND_X 1
#define SHERE_AROUND_Y 2
#define SHERE_AROUND_Z 3
#define REFLECT_ABOUT_X 1
#define REFLECT_ABOUT_Y 2
#define REFLECT_ABOUT_Z 3
//---------------------------------------------------------------------------
// Implement a 4x3 transformation matrix. This class can represent
// any 3D affine transformation.
//---------------------------------------------------------------------------
class cMatrix4x3
{
public:
// The values of the matrix. Basically the upper 3x3 portion contains a linear transformation,
// and the last row is the translation portion.
float m11, m12, m13;
float m21, m22, m23;
float m31, m32, m33;
float tx, ty, tz;
public:
void identity();
// access the translation portion of the matrix directly
void zero_translation();
void set_translation(const cVector3& d);
void setup_translation(const cVector3& d);
// Setup the matrix to perform a specific transforms from parent <->
// local space, assuming the local space is in the specified position
// and orientation within the parent space. The orientation may be
// specified using either Euler angles, or a rotation matrix.
void setup_local_to_parent(const cVector3& pos, const cEulerAngles& orient);
void setup_local_to_parent(const cVector3& pos, const cRotationMatrix& orient);
void setup_parent_to_local(const cVector3& pos, const cEulerAngles& orient);
void setup_parent_to_local(const cVector3& pos, const cRotationMatrix& orient);
// setup the matrix to perform a rotation about a cardinal axis
void setup_rotate(int axis, float theta);
// setup the matrix to perform a rotation about ab arbitrary axis
void setup_rotate(const cVector3& axis, float theta);
// Setup the matrix to perform a rotation, given the angular displacement in quaternion form.
void from_quat(const cQuaternion& q);
// setup the matrix to perform scale on each axis
void setup_scale(const cVector3& s);
// setup the matrix to perform scale along an arbitrary axis
void setup_scale_along_axis(const cVector3& axis, float k);
// setup the matrix to perform a shear
void setup_shear(int axis, float s, float t);
// Setup the matrix to perform a projection onto a plane passing through the origin
void setup_project(const cVector3& n);
// Setup the matrix to perform a reflection about a plane parallel to a cardinal plane
void setup_reflect(int axis, float k);
// Setup the matrix to perform a reflection about an arbitrary plane through the origin
void setup_reflect(const cVector3& n);
};
// Operator* is used to transforms a point, and also concatenate matrices.
// The order of multiplications from left to right is the same as the order of transformations.
cVector3 operator *(const cVector3& p, const cMatrix4x3& m);
cMatrix4x3 operator *(const cMatrix4x3& a, const cMatrix4x3& b);
// operator *= for conformance to c++ standards
cVector3& operator *=(cVector3& p, const cMatrix4x3& m);
cMatrix4x3& operator *=(const cMatrix4x3& a, const cMatrix4x3& m);
// compute the determinant of the 3x3 portion of the matrix
float determinant(const cMatrix4x3& m);
// compute the inverse of a matrix
cMatrix4x3 inverse(const cMatrix4x3& m);
// extract the translaltion portion of the matrix
cVector3 get_translation(const cMatrix4x3& m);
// Extract the position/orientation from a local->parent matrix, or a parent->local matrix.
cVector3 get_position_from_parent_to_local_matrix(const cMatrix4x3& m);
cVector3 get_position_from_local_to_parent_matrix(const cMatrix4x3& m);
#endif
cMatrix4x3
類的所有成員函數都被設計成用某種基本變換來完成矩陣的轉置：
（1）identity()將矩陣設為單位矩陣。
（2）zero_translation()通過將最后一行設為[0, 0, 0]來取消矩陣的平移部分，線性變換部分(3x3部分)不受影響。
（3）set_translation() 將矩陣的平移部分設為指定值，不改變3x3部分。setup_translation()設置矩陣來執行平移，上面的3x3部分設為單位矩陣，平移部分設為指定向量。
（4）setup_local_to_parent()創建一個矩陣能將點從局部坐標空間變換到父坐標空間，需要給出局部坐標空間在父坐標空間中的位置和方向。最常用到該方法的可能是將點從物體坐標空間變換到世界坐標空間的時候。局部坐標空間的方位可以用歐拉角或旋轉矩陣定義，用旋轉矩陣更快一些，因為它沒有實數運算，只有矩陣元素的復制。setup_parent_to_local()設置用來執行相反變換的矩陣。
（5）setup_rotate()的兩個重載方法都創建一個繞軸旋轉的矩陣。如果軸是坐標軸，將使用一個數字來代表坐標軸。繞任意軸旋轉時，用第二個版本的setup_rotate()，它用一個單位向量代表旋轉軸。
（6）from_quat()將一個四元數轉換到矩陣形式，矩陣的平移部分為0。
（7）setup_scale()創建一個矩陣執行沿坐標軸的均勻或非均勻縮放。輸入向量包含沿x、y、z軸的縮放因子。對均勻縮放，使用一個每個軸的值都相同的向量。
（8）setup_scale_along_axis()創建一個矩陣執行沿任意方向縮放。這個縮放發生在一個穿過原點的平面上----該平面垂直于向量參數，向量是單位化的。
（9）setup_shear()創建一個切變矩陣。
（10）setup_project()創建一個投影矩陣，該矩陣向穿過原點且垂直于給定向量的平面投影。
（11）setup_reflect()創建一個沿平面鏡像的矩陣。第一個版本中，坐標軸用一個數字指定，平面不必穿過原點。第二個版本中指定任意的法向量，且平面必須穿過原點。（對于沿任意不穿過原點的平面鏡像，必須將該矩陣和適當的變換矩陣連接。）
determinant()函數計算矩陣的行列式。實際只使用了3x3部分，如果假設最后一列總為[0, 0, 0, 1]T，那么最后一行（平移部分）會被最后一列的0消去。
inverse()計算并返回矩陣的逆，理論上不可能對4x3矩陣求逆，只有方陣才能求逆。所以再聲明一次，假設的最后一列[0, 0, 0, 1]T保證了合法性。
get_translation()是一個輔助函數，幫助以向量形式提取矩陣的平移部分。
get_position_from_parent_to_local_matrix()和get_position_from_local_to_parent_matrix()是從父坐標空間中提取局部坐標空間位置的函數，需要傳入變換矩陣。在某種程度上，這兩個方法是setup_local_to_parent()和setup_parent_to_local()關于位置部分的逆向操作。當然，你可以對任意矩陣使用這兩個方法（假設它是一個剛體變換），而不僅僅限于setup_local_to_parent()和setup_parent_to_local()產生的矩陣。以歐拉角形式從變換矩陣中提取方位，需要使用cEulerAngles類的一個方法。
Matrix4x3.cpp：
#include <assert.h>
#include <math.h>
#include "Vector3.h"
#include "EulerAngles.h"
#include "Quaternion.h"
#include "RotationMatrix.h"
#include "Matrix4x3.h"
#include "MathUtil.h"
/////////////////////////////////////////////////////////////////////////////
//
// MATRIX ORGANIZATION
//
// The purpose of this class is so that a user might perform transformations
// without fiddling with plus or minus signs or transposing the matrix
// until the output "looks right." But of course, the specifics of the
// internal representation is important. Not only for the implementation
// in this file to be correct, but occasionally direct access to the
// matrix variables is necessary, or beneficial for optimization. Thus,
// we document our matrix conventions here.
//
// We use row vectors, so multiplying by our matrix looks like this:
//
// | m11 m12 m13 |
// [ x y z ] | m21 m22 m23 | = [ x' y' z' ]
// | m31 m32 m33 |
// | tx ty tz |
//
// Strict adherance to linear algebra rules dictates that this
// multiplication is actually undefined. To circumvent this, we can
// consider the input and output vectors as having an assumed fourth
// coordinate of 1. Also, since we cannot technically invert a 4x3 matrix
// according to linear algebra rules, we will also assume a rightmost
// column of [ 0 0 0 1 ]. This is shown below:
//
// | m11 m12 m13 0 |
// [ x y z 1 ] | m21 m22 m23 0 | = [ x' y' z' 1 ]
// | m31 m32 m33 0 |
// | tx ty tz 1 |
//
/////////////////////////////////////////////////////////////////////////////
//---------------------------------------------------------------------------
// Set the matrix to identity
//---------------------------------------------------------------------------
void cMatrix4x3::identity()
{
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f; ty = 0.0f; tz = 1.0f;
}
//---------------------------------------------------------------------------
// Zero the 4th row of the matrix, which contains the translation portion.
//---------------------------------------------------------------------------
void cMatrix4x3::zero_translation()
{
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::set_translation(const cVector3& d)
{
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Sets the translation portion of the matrix in vector form.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_translation(const cVector3& d)
{
// Set the linear transformation portion to identity
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
// Set the translation portion
tx = d.x; ty = d.y; tz = d.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a local -> parent transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a object -> world matrix.
// As an example, the transformation in this case is straightforward. We
// first rotate from object space into inertial space, then we translate
// into world space.
//
// We allow the orientation to be specified using either euler angles,
// or a cRotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_local_to_parent(const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// Setup the 4x3 matrix. Note: if we were really concerned with speed, we could
// create the matrix directly into these variables, without using the temporary
// cRotationMatrix object. This would save us a function call and a few copy operations.
setup_local_to_parent(pos, orient_matrix);
}
void cMatrix4x3::setup_local_to_parent(const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. According to the comments in RotationMatrix.cpp,
// the rotation matrix is "normally" an inertial->object matrix, which is parent->local.
// We want a local->parent rotation, so we must transpose while copying.
m11 = orient.m11; m12 = orient.m21; m13 = orient.m31;
m21 = orient.m12; m22 = orient.m22; m23 = orient.m32;
m31 = orient.m13; m32 = orient.m23; m33 = orient.m33;
// Now set the translation portion. Translation happens "after" the 3x3 portion,
// so we can simply copy the position field directly.
tx = pos.x; ty = pos.y; tz = pos.z;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a parent -> local transformation, given
// the position and orientation of the local reference frame within the
// parent reference frame.
//
// A very common use of this will be to construct a world -> object matrix.
// To perform this transformation, we would normally FIRST transform
// from world to inertial space, and then rotate from inertial space into
// object space. However, our 4x3 matrix always translates last. So
// we think about creating two matrices T and R, and then concatenating M = TR.
//
// We allow the orientation to be specified using either euler angles,
// or a RotationMatrix.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_parent_to_local(const cVector3& pos, const cEulerAngles& orient)
{
// create a rotation matrix
cRotationMatrix orient_matrix;
orient_matrix.setup(orient);
// setup the 4x3 matrix
setup_parent_to_local(pos, orient_matrix);
}
void cMatrix4x3::setup_parent_to_local(const cVector3& pos, const cRotationMatrix& orient)
{
// Copy the rotation portion of the matrix. We can copy the elements directly
// (without transposing) according to the layout as commented in RotationMatrix.cpp.
m11 = orient.m11; m12 = orient.m12; m13 = orient.m13;
m21 = orient.m21; m22 = orient.m22; m23 = orient.m23;
m31 = orient.m31; m32 = orient.m32; m33 = orient.m33;
// Now set the translation portion. Normally, we would translate by the negative of
// the position to translate from world to inertial space. However, we must correct
// for the fact that the rotation occurs "first." So we must rotate the translation portion.
// This is the same as create a translation matrix T to translate by -pos,
// and a rotation matrix R, and then creating the matrix as the concatenation of TR.
tx = -(pos.x * m11 + pos.y * m21 + pos.z * m31);
ty = -(pos.x * m12 + pos.y * m22 + pos.z * m32);
tz = -(pos.x * m13 + pos.y * m23 + pos.z * m33);
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation about a cardinal axis.
//
// theta is the amount of rotation, in radians. The left-hand rule is used to
// define "positive" rotation.
//
// The translation portion is reset.
//
// Rotate around x axis:
// | 1 0 0 |
// R(a) = | 0 cosa sina |
// | 0 -sina cosa |
//
// Rotate around y axis:
// | cosa 0 -sina |
// R(a) = | 0 1 0 |
// | sina 0 cosa |
//
// Rotate around z axis:
// | cosa sina 0 |
// R(a) = | -sina cosa 0 |
// | 0 0 1 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_rotate(int axis, float theta)
{
// get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// check which axis they are rotating about
switch(axis)
{
case ROTATE_AROUND_X:
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = c; m23 = s;
m31 = 0.0f; m32 = -s; m33 = c;
break;
case ROTATE_AROUND_Y:
m11 = c; m12 = 0.0f; m13 = -s;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = 0.0f; m33 = c;
break;
case ROTATE_AROUND_Z:
m11 = c; m12 = s; m13 = 0.0f;
m21 = -s; m22 = c; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break;
default: // bogus axis index
assert(false);
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//-------------------------------------------------------------------------------------
// Setup the matrix to perform a rotation about an arbitrary axis.
// The axis of rotation must pass through the origin.
//
// axis defines the axis of rotation, and must be a unit vector.
//
// theta is the amount of rotation, in radians. The left-hand rule is
// used to define "positive" rotation.
//
// The translation portion is reset.
//
// | x^2(1-cosa) + cosa xy(1-cosa) + zsina xz(1-cosa) - ysina |
// R(n, a) = | xy(1-cosa) - zsina y^2(1-cosa) + cosa yz(1-cosa) + xsina |
// | xz(1-cosa) + ysina yz(1-cosa) - xsina z^2(1-cosa) + cosa |
//-------------------------------------------------------------------------------------
void cMatrix4x3::setup_rotate(const cVector3& axis, float theta)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Get sin and cosine of rotation angle
float s, c;
sin_cos(&s, &c, theta);
// Compute 1 - cos(theta) and some common subexpressions
float a = 1.0f - c;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = ax * axis.x + c;
m12 = ax * axis.y + axis.z * s;
m13 = ax * axis.z - axis.y * s;
m21 = ay * axis.x - axis.z * s;
m22 = ay * axis.y + c;
m23 = ay * axis.z + axis.x * s;
m31 = az * axis.x + axis.y * s;
m32 = az * axis.y - axis.x * s;
m33 = az * axis.z + c;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a rotation, given the angular displacement
// in quaternion form.
//
// The translation portion is reset.
//
// | 1 - 2(y^2 + z^2) 2(xy + wz) 2(xz - wy) |
// M = | 2(xy - wz) 1 - 2(x^2 + z^2) 2(yz + wx) |
// | 2(xz + wy) 2(yz - wx) 1 - 2(x^2 + y^2) |
//---------------------------------------------------------------------------
void cMatrix4x3::from_quat(const cQuaternion& q)
{
// Compute a few values to optimize common subexpressions
float double_w = 2.0f * q.w;
float double_x = 2.0f * q.x;
float double_y = 2.0f * q.y;
float double_z = 2.0f * q.z;
// Set the matrix elements. There is still a little more opportunity for optimization
// due to the many common subexpressions. We'll let the compiler handle that
m11 = 1.0f - double_y * q.y - double_z * q.z;
m12 = double_x * q.y + double_w * q.z;
m13 = double_x * q.z - double_w * q.x;
m21 = double_x * q.y - double_w * q.z;
m22 = 1.0f - double_x * q.x - double_z * q.z;
m23 = double_y * q.z + double_w * q.x;
m31 = double_x * q.z + double_w * q.y;
m32 = double_y * q.z - double_w * q.x;
m33 = 1.0f - double_x * q.x - double_y * q.y;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale on each axis. For uniform scale by k,
// use a vector of the form Vector3(k,k,k)
//
// The translation portion is reset.
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale(const cVector3& s)
{
// Set the matrix elements. Pretty straightforward
m11 = s.x; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = s.y; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = s.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform scale along an arbitrary axis.
//
// The axis is specified using a unit vector, the translation portion is reset.
//
// | 1 + (k-1)x^2 (k-1)xy (k-1)xz |
// S(n, k) = | (k-1)xy 1 + (k-1)y^2 (k-1)yz |
// | (k-1)xz (k-1)yz 1 + (k-1)z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_scale_along_axis(const cVector3& axis, float k)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis.
assert(fabs(axis * axis - 1.0f) < 0.01f);
// Compute k-1 and some common subexpressions
float a = k - 1.0f;
float ax = a * axis.x;
float ay = a * axis.y;
float az = a * axis.z;
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = ax * axis.x + 1.0f;
m22 = ay * axis.y + 1.0f;
m32 = az * axis.z + 1.0f;
m12 = m21 = ax * axis.y;
m13 = m31 = ax * axis.z;
m23 = m32 = ay * axis.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_shear(int axis, float s, float t)
{
// Check which type of shear they want
switch (axis)
{
case SHERE_AROUND_X: // Shear y and z using x
m11 = 1.0f; m12 = s; m13 = t;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break;
case SHERE_AROUND_Y: // Shear x and z using y
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = s; m22 = 1.0f; m23 = t;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
break;
case SHERE_AROUND_Z: // Shear x and y using z
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = s; m32 = t; m33 = 1.0f;
break;
default:
// bogus axis index
assert(false);
}
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a projection onto a plane passing
// through the origin. The plane is perpendicular to the unit vector n.
//
// | 1-x^2 -xy -xz |
// P(n) = |-xy 1-y^2 -yz |
// |-xz -zy 1-z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_project(const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Fill in the matrix elements. We'll do the common subexpression optimization
// ourselves here, since diagonally opposite matrix elements are equal.
m11 = 1.0f - n.x * n.x;
m22 = 1.0f - n.y * n.y;
m33 = 1.0f - n.z * n.z;
m12 = m21 = -n.x * n.y;
m13 = m31 = -n.x * n.z;
m23 = m32 = -n.y * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about a plane parallel to a
// cardinal plane.
//
// axis is a 1-based index which specifies the plane to project about:
//
// REFLECT_ABOUT_X => reflect about the plane x=k
// REFLECT_ABOUT_Y => reflect about the plane y=k
// REFLECT_ABOUT_Z => reflect about the plane z=k
//
// The translation is set appropriately, since translation must occur if k != 0
//
// Reflect about x = 0:
//
// | -1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about y = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f -1.0f 0.0f |
// | 0.0f 0.0f 1.0f |
//
// Reflect about z = 0:
//
// | 1.0f 0.0f 0.0f |
// M = | 0.0f 1.0f 0.0f |
// | 0.0f 0.0f -1.0f |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect(int axis, float k)
{
// Check which plane they want to reflect about
switch (axis)
{
case REFLECT_ABOUT_X: // Reflect about the plane x=k
m11 = -1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 2.0f * k;
ty = 0.0f;
tz = 0.0f;
break;
case REFLECT_ABOUT_Y: // Reflect about the plane y=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = -1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = 1.0f;
tx = 0.0f;
ty = 2.0f * k;
tz = 0.0f;
break;
case REFLECT_ABOUT_Z: // Reflect about the plane z=k
m11 = 1.0f; m12 = 0.0f; m13 = 0.0f;
m21 = 0.0f; m22 = 1.0f; m23 = 0.0f;
m31 = 0.0f; m32 = 0.0f; m33 = -1.0f;
tx = 0.0f;
ty = 0.0f;
tz = 2.0f * k;
break;
default:
// bogus axis index
assert(false);
}
}
//---------------------------------------------------------------------------
// Setup the matrix to perform a reflection about an arbitrary plane
// through the origin. The unit vector n is perpendicular to the plane.
//
// The translation portion is reset.
//
// | 1 - 2x^2 -2xy -2xz |
// P(n) = S(n, -1) = | -2xy 1 - 2y^2 -2yz |
// | -2xz -2zy 1 - 2z^2 |
//---------------------------------------------------------------------------
void cMatrix4x3::setup_reflect(const cVector3& n)
{
// Quick sanity check to make sure they passed in a unit vector to specify the axis
assert(fabs(n * n - 1.0f) < 0.01f);
// Compute common subexpressions
float ax = -2.0f * n.x;
float ay = -2.0f * n.y;
float az = -2.0f * n.z;
// Fill in the matrix elements. We'll do the common subexpression optimization ourselves
// here, since diagonally opposite matrix elements are equal.
m11 = 1.0f + ax * n.x;
m22 = 1.0f + ay * n.y;
m33 = 1.0f + az * n.z;
m12 = m21 = ax * n.y;
m13 = m31 = ax * n.z;
m23 = m32 = ay * n.z;
// Reset the translation portion
tx = ty = tz = 0.0f;
}
//---------------------------------------------------------------------------
// Transform the point. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cVector3 operator *(const cVector3& p, const cMatrix4x3& m)
{
// Grind through the linear algebra.
return cVector3(p.x * m.m11 + p.y * m.m21 + p.z * m.m31 + m.tx,
p.x * m.m12 + p.y * m.m22 + p.z * m.m32 + m.ty,
p.x * m.m13 + p.y * m.m23 + p.z * m.m33 + m.tz);
}
cVector3& operator *=(cVector3& p, const cMatrix4x3& m)
{
p = p * m;
return p;
}
//---------------------------------------------------------------------------
// Matrix concatenation. This makes using the vector class look like it
// does with linear algebra notation on paper.
//
// We also provide a *= operator, as per C convention.
//---------------------------------------------------------------------------
cMatrix4x3 operator *(const cMatrix4x3& a, const cMatrix4x3& b)
{
cMatrix4x3 r;
// Compute the upper 3x3 (linear transformation) portion
r.m11 = a.m11 * b.m11 + a.m12 * b.m21 + a.m13 * b.m31;
r.m12 = a.m11 * b.m12 + a.m12 * b.m22 + a.m13 * b.m32;
r.m13 = a.m11 * b.m13 + a.m12 * b.m23 + a.m13 * b.m33;
r.m21 = a.m21 * b.m11 + a.m22 * b.m21 + a.m23 * b.m31;
r.m22 = a.m21 * b.m12 + a.m22 * b.m22 + a.m23 * b.m32;
r.m23 = a.m21 * b.m13 + a.m22 * b.m23 + a.m23 * b.m33;
r.m31 = a.m31 * b.m11 + a.m32 * b.m21 + a.m33 * b.m31;
r.m32 = a.m31 * b.m12 + a.m32 * b.m22 + a.m33 * b.m32;
r.m33 = a.m31 * b.m13 + a.m32 * b.m23 + a.m33 * b.m33;
// Compute the translation portion
r.tx = a.tx * b.m11 + a.ty * b.m21 + a.tz * b.m31 + b.tx;
r.ty = a.tx * b.m12 + a.ty * b.m22 + a.tz * b.m32 + b.ty;
r.tz = a.tx * b.m13 + a.ty * b.m23 + a.tz * b.m33 + b.tz;
// Return it. Ouch - involves a copy constructor call. If speed
// is critical, we may need a seperate function which places the
// result where we want it
return r;
}
cMatrix4x3& operator *=(cMatrix4x3& a, const cMatrix4x3& b)
{
a = a * b;
return a;
}
//---------------------------------------------------------------------------
// Compute the determinant of the 3x3 portion of the matrix.
//---------------------------------------------------------------------------
float determinant(const cMatrix4x3& m)
{
float result = m.m11 * (m.m22 * m.m33 - m.m23 * m.m32) +
m.m12 * (m.m23 * m.m31 - m.m21 * m.m33) +
m.m13 * (m.m21 * m.m32 - m.m22 * m.m31);
return result;
}
//---------------------------------------------------------------------------
// Compute the inverse of a matrix. We use the classical adjoint divided
// by the determinant method.
//---------------------------------------------------------------------------
cMatrix4x3 inverse(const cMatrix4x3& m)
{
float det = determinant(m);
// If we're singular, then the determinant is zero and there's no inverse.
assert(fabs(det) > 0.000001f);
// Compute one over the determinant, so we divide once and can multiply per element.
float one_over_det = 1.0f / det;
// Compute the 3x3 portion of the inverse, by dividing the adjoint by the determinant.
cMatrix4x3 r;
r.m11 = (m.m22 * m.m33 - m.m23 * m.m32) * one_over_det;
r.m12 = (m.m13 * m.m32 - m.m12 * m.m33) * one_over_det;
r.m13 = (m.m12 * m.m23 - m.m13 * m.m22) * one_over_det;
r.m21 = (m.m23 * m.m31 - m.m21 * m.m33) * one_over_det;
r.m22 = (m.m11 * m.m33 - m.m13 * m.m31) * one_over_det;
r.m23 = (m.m13 * m.m21 - m.m11 * m.m23) * one_over_det;
r.m31 = (m.m21 * m.m32 - m.m22 * m.m31) * one_over_det;
r.m32 = (m.m12 * m.m31 - m.m11 * m.m32) * one_over_det;
r.m33 = (m.m11 * m.m22 - m.m12 * m.m21) * one_over_det;
// Compute the translation portion of the inverse
r.tx = -(m.tx * r.m11 + m.ty * r.m21 + m.tz * r.m31);
r.ty = -(m.tx * r.m12 + m.ty * r.m22 + m.tz * r.m32);
r.tz = -(m.tx * r.m13 + m.ty * r.m23 + m.tz * r.m33);
// Return it. Ouch - involves a copy constructor call. If speed is critical,
// we may need a seperate function which places the result where we want it
return r;
}
//---------------------------------------------------------------------------
// Return the translation row of the matrix in vector form
//---------------------------------------------------------------------------
cVector3 get_translation(const cMatrix4x3& m)
{
return cVector3(m.tx, m.ty, m.tz);
}
//---------------------------------------------------------------------------
// Extract the position of an object given a parent -> local transformation
// matrix (such as a world -> object matrix).
//
// We assume that the matrix represents a rigid transformation. (No scale,
// skew, or mirroring).
//---------------------------------------------------------------------------
cVector3 get_position_from_parent_to_local_matrix(const cMatrix4x3& m)
{
// Multiply negative translation value by the transpose of the 3x3 portion.
// By using the transpose, we assume that the matrix is orthogonal.
// (This function doesn't really make sense for non-rigid transformations)
return cVector3(-(m.tx * m.m11 + m.ty * m.m12 + m.tz * m.m13),
-(m.tx * m.m21 + m.ty * m.m22 + m.tz * m.m23),
-(m.tx * m.m31 + m.ty * m.m32 + m.tz * m.m33));
}
//---------------------------------------------------------------------------
// Extract the position of an object given a local -> parent transformation
// matrix (such as an object -> world matrix).
//---------------------------------------------------------------------------
cVector3 get_position_from_local_to_parent_matrix(const cMatrix4x3& m)
{
// Position is simply the translation portion
return cVector3(m.tx, m.ty, m.tz);
}