本文共 12712 字，大约阅读时间需要 42 分钟。

1. 如何使用矩阵表示方位

通过列出从一个坐标系到另一个坐标系的转变矩阵来表示两个坐标系之间的关系。

例如：从物体坐标系到惯性坐标系的转变矩阵，也可以通过逆矩阵从惯性坐标系转换回物体坐标系。

示例：

我们会看一下如何用一个矩阵将一个点从一个坐标系转变到另一个坐标系。

下图使用一个矩阵将飞机从物体坐标系转换到惯性坐标系。

我们用粗黑箭头指示矩阵的每一行和物体坐标轴的关系。

这个旋转矩阵包含了在惯性坐标系的物体轴。

同时，它是个旋转矩阵。可以用多个行向量跟矩阵相乘，将多个向量从物体坐标系转换到惯性坐标系。

因为旋转矩阵是正交的，因此如果我们想从惯性坐标系转换回物体坐标系，可以使用矩阵的转置（等同于矩阵的逆）作为转换矩阵。

但是，使用矩阵来表示方位有一个问题：不便于程序员阅读和检查代码问题。

2. C++代码实现

///////////////////////////////////////////////////////////////////////////////// 3D Math Primer for Games and Graphics Development//// Vector3.h - Declarations for 3D vector class//// Visit gamemath.com for the latest version of this file.//// For additional comments, see Chapter 6.///////////////////////////////////////////////////////////////////////////////#ifndef __VECTOR3_H_INCLUDED__#define __VECTOR3_H_INCLUDED__#include 
   
    ///////////////////////////////////////////////////////////////////////////////// class Vector3 - a simple 3D vector class///////////////////////////////////////////////////////////////////////////////class Vector3 {public:// Public representation:  Not many options here.    float x,y,z;// Constructors    // Default constructor leaves vector in    // an indeterminate state    Vector3() {}    // Copy constructor    Vector3(const Vector3 &a) : x(a.x), y(a.y), z(a.z) {}    // Construct given three values    Vector3(float nx, float ny, float nz) : x(nx), y(ny), z(nz) {}// Standard object maintenance    // Assignment.  We adhere to C convention and    // return reference to the lvalue    Vector3 &operator =(const Vector3 &a) {        x = a.x; y = a.y; z = a.z;        return *this;    }    // Check for equality    bool operator ==(const Vector3 &a) const {        return x==a.x && y==a.y && z==a.z;    }    bool operator !=(const Vector3 &a) const {        return x!=a.x || y!=a.y || z!=a.z;    }// Vector operations    // Set the vector to zero    void zero() { x = y = z = 0.0f; }    // Unary minus returns the negative of the vector    Vector3 operator -() const { return Vector3(-x,-y,-z); }    // Binary + and - add and subtract vectors    Vector3 operator +(const Vector3 &a) const {        return Vector3(x + a.x, y + a.y, z + a.z);    }    Vector3 operator -(const Vector3 &a) const {        return Vector3(x - a.x, y - a.y, z - a.z);    }    // Multiplication and division by scalar    Vector3 operator *(float a) const {        return Vector3(x*a, y*a, z*a);    }    Vector3 operator /(float a) const {        float   oneOverA = 1.0f / a; // NOTE: no check for divide by zero here        return Vector3(x*oneOverA, y*oneOverA, z*oneOverA);    }    // Combined assignment operators to conform to    // C notation convention    Vector3 &operator +=(const Vector3 &a) {        x += a.x; y += a.y; z += a.z;        return *this;    }    Vector3 &operator -=(const Vector3 &a) {        x -= a.x; y -= a.y; z -= a.z;        return *this;    }    Vector3 &operator *=(float a) {        x *= a; y *= a; z *= a;        return *this;    }    Vector3 &operator /=(float a) {        float   oneOverA = 1.0f / a;        x *= oneOverA; y *= oneOverA; z *= oneOverA;        return *this;    }    // Normalize the vector    void    normalize() {        float magSq = x*x + y*y + z*z;        if (magSq > 0.0f) { // check for divide-by-zero            float oneOverMag = 1.0f / sqrt(magSq);            x *= oneOverMag;            y *= oneOverMag;            z *= oneOverMag;        }    }    // Vector dot product.  We overload the standard    // multiplication symbol to do this    float operator *(const Vector3 &a) const {        return x*a.x + y*a.y + z*a.z;    }};///////////////////////////////////////////////////////////////////////////////// Nonmember functions///////////////////////////////////////////////////////////////////////////////// Compute the magnitude of a vectorinline float vectorMag(const Vector3 &a) {    return sqrt(a.x*a.x + a.y*a.y + a.z*a.z);}// Compute the cross product of two vectorsinline Vector3 crossProduct(const Vector3 &a, const Vector3 &b) {    return Vector3(        a.y*b.z - a.z*b.y,        a.z*b.x - a.x*b.z,        a.x*b.y - a.y*b.x    );}// Scalar on the left multiplication, for symmetryinline Vector3 operator *(float k, const Vector3 &v) {    return Vector3(k*v.x, k*v.y, k*v.z);}// Compute the distance between two pointsinline float distance(const Vector3 &a, const Vector3 &b) {    float dx = a.x - b.x;    float dy = a.y - b.y;    float dz = a.z - b.z;    return sqrt(dx*dx + dy*dy + dz*dz);}// Compute the distance between two points, squared.  Often useful// when comparing distances, since the square root is slowinline float distanceSquared(const Vector3 &a, const Vector3 &b) {    float dx = a.x - b.x;    float dy = a.y - b.y;    float dz = a.z - b.z;    return dx*dx + dy*dy + dz*dz;}///////////////////////////////////////////////////////////////////////////////// Global variables///////////////////////////////////////////////////////////////////////////////// We provide a global zero vector constantextern const Vector3 kZeroVector;/////////////////////////////////////////////////////////////////////////////#endif // #ifndef __VECTOR3_H_INCLUDED__
   

///////////////////////////////////////////////////////////////////////////////// 3D Math Primer for Games and Graphics Development//// RotationMatrix.h - Declarations for class RotationMatrix//// Visit gamemath.com for the latest version of this file.//// For more details, see RotationMatrix.cpp///////////////////////////////////////////////////////////////////////////////#ifndef __ROTATIONMATRIX_H_INCLUDED__#define __ROTATIONMATRIX_H_INCLUDED__class Vector3;class EulerAngles;class Quaternion;//---------------------------------------------------------------------------// class RotationMatrix//// 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 RotationMatrix {public:// Public data    // The 9 values of the matrix.  See RotationMatrix.cpp file for    // the details of the layout    float   m11, m12, m13;    float   m21, m22, m23;    float   m31, m32, m33;// Public operations    // Set to identity    void    identity();    // Setup the matrix with a specified orientation    void    setup(const EulerAngles &orientation);    // Setup the matrix from a quaternion, assuming the    // quaternion performs the rotation in the    // specified direction of transformation    void    fromInertialToObjectQuaternion(const Quaternion &q);    void    fromObjectToInertialQuaternion(const Quaternion &q);    // Perform rotations    Vector3 inertialToObject(const Vector3 &v) const;    Vector3 objectToInertial(const Vector3 &v) const;};/////////////////////////////////////////////////////////////////////////////#endif // #ifndef __ROTATIONMATRIX_H_INCLUDED__

///////////////////////////////////////////////////////////////////////////////// 3D Math Primer for Games and Graphics Development//// RotationMatrix.cpp - Implementation of class RotationMatrix//// Visit gamemath.com for the latest version of this file.//// For more details see section 11.4.///////////////////////////////////////////////////////////////////////////////#include "vector3.h"#include "RotationMatrix.h"#include "MathUtil.h"#include "Quaternion.h"#include "EulerAngles.h"///////////////////////////////////////////////////////////////////////////////// class RotationMatrix////---------------------------------------------------------------------------//// 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 |/////////////////////////////////////////////////////////////////////////////////---------------------------------------------------------------------------// RotationMatrix::identity//// Set the matrix to the identity matrixvoid    RotationMatrix::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;}//---------------------------------------------------------------------------// RotationMatrix::setup//// Setup the matrix with the specified orientation//// See 10.6.1void    RotationMatrix::setup(const EulerAngles &orientation) {    // Fetch sine and cosine of angles    float   sh,ch, sp,cp, sb,cb;    sinCos(&sh, &ch, orientation.heading);    sinCos(&sp, &cp, orientation.pitch);    sinCos(&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;}//---------------------------------------------------------------------------// RotationMatrix::fromInertialToObjectQuaternion//// Setup the matrix, given a quaternion that performs an inertial->object// rotation//// See 10.6.3void    RotationMatrix::fromInertialToObjectQuaternion(const Quaternion &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);}//---------------------------------------------------------------------------// RotationMatrix::fromObjectToInertialQuaternion//// Setup the matrix, given a quaternion that performs an object->inertial// rotation//// See 10.6.3void    RotationMatrix::fromObjectToInertialQuaternion(const Quaternion &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);}//---------------------------------------------------------------------------// RotationMatrix::inertialToObject//// Rotate a vector from inertial to object spaceVector3 RotationMatrix::inertialToObject(const Vector3 &v) const {    // Perform the matrix multiplication in the "standard" way.    return Vector3(        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    );}//---------------------------------------------------------------------------// RotationMatrix::objectToInertial//// Rotate a vector from object to inertial spaceVector3 RotationMatrix::objectToInertial(const Vector3 &v) const {    // Multiply by the transpose    return Vector3(        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    );}

#include "RotationMatrix.h"#include 
   
    using namespace std;void prfloatVec(Vector3 &vec){    cout << "["         << vec.x << ","         << vec.y << ","         << vec.z << "]" << endl;}int main(){    cout << "hello 矩阵的逆" << endl;    RotationMatrix m;    m.m11 = 0.866f; m.m12 = 0.0f;   m.m13 = -0.5f;    m.m21 = 0.0f;   m.m22 = 1.0f;   m.m23 = 0.0f;    m.m31 = 0.5f;   m.m32 = 0.0f;   m.m33 = 0.866f;    Vector3 v(10,20,30);    Vector3 v2;    v2 = m.inertialToObject(v);    prfloatVec(v2);    v2 = m.objectToInertial(v);    prfloatVec(v2);}
   

3. 程序运行结果

hello 矩阵的逆

[23.66,20,20.98]

[-6.34,20,30.98]