新建網(wǎng)頁 1
cRotationMatrix類的目的就是處理非常特殊的（也是極其常用的）物體和慣性坐標(biāo)空間之間的旋轉(zhuǎn)。這個(gè)矩陣類不是一般的變換類，我們假定這個(gè)類只包含旋轉(zhuǎn)，因此，它是正交的。換句話說，該矩陣表達(dá)的是方位，而不是角位移。當(dāng)你創(chuàng)建這樣的矩陣時(shí)，不必指定變換的方向（物體坐標(biāo)空間到慣性坐標(biāo)空間或是慣性坐標(biāo)空間到物體坐標(biāo)空間）。變換的方向在實(shí)際執(zhí)行變換時(shí)指定，每個(gè)方向?qū)?yīng)一個(gè)函數(shù)。
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
因?yàn)?/span>cRotationMatrix類很簡(jiǎn)單，因此也非常容易使用。首先，用歐拉角或四元數(shù)設(shè)置矩陣。如果使用四元數(shù)，還必須指明該四元數(shù)代表哪種角位移。一旦創(chuàng)建了矩陣，就能用inertial_to_object()和object_to_inertial()函數(shù)執(zhí)行旋轉(zhuǎn)。
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);
}