3D数学 RotationMatrix

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 <math.h>

/////////////////////////////////////////////////////////////////////////////
//
// 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 vector

inline 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 vectors

inline 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 symmetry

inline Vector3 operator *(float k, const Vector3 &v) {
    return Vector3(k*v.x, k*v.y, k*v.z);
}

// Compute the distance between two points

inline 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 slow

inline 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 constant

extern 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 matrix

void    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.1

void    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.3

void    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.3

void    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 space

Vector3 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 space

Vector3 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 <iostream>

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]