数学理论基础请参阅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);
}