3D数学 欧拉角编程

欧拉角编程

1. 欧拉角转换到矩阵

欧拉角描述了一个旋转序列。分别计算出每个旋转的矩阵再将它们连成一个矩阵，这个矩阵就代表了整个角位移。注意，要区分物体-惯性矩阵还是惯性-物体矩阵，它们互逆（也互为转置矩阵）。

void RotationMatrix::setup(const EulerAngles& orientation)
{
    //计算角度的sin和cos值
    float sh, ch, sp, cp, sb, cb;
    sinCos(&sh,&ch,orientation.heading);
    sinCos(&sp,&cp,orientation.pitch);
    sinCos(&sb,&cb,orientation.bank);

    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;

}

2. 矩阵转换到欧拉角

注意以下几点：

• 必须知道矩阵代表物体-惯性坐标系还是惯性-物体坐标系。
• 因为“别名”问题，要限制欧拉角。
• 要处理浮点数精度的误差。
• 要处理万向锁。
• 这里，只讨论旋转矩阵上的转换。

void EulerAngles::fromObjectToWorldMatrix(const Matrix4x3 &m)
{
    float sp = - m.m32;

    //检测万向锁
    if (fabs(sp)>0.99999f)
    {
        //向上看或向下看
        //将bank置为零，赋值给heading
        pitch = kPiOver2 * sp;
        //bank置为零，计算heading
        bank = 0.0f;
        heading = atan2(-m.m23, m.m11);
    }
    else
    {
        //计算角度
        heading = atan2(m.m31, m.m33);
        pitch = asin(sp);
        bank = atan2(m.m12, m.m22);
    }
}

void EulerAngles::fromWorldToBojectMatrix(const Matrix4x3 &m)
{
    //根据m32计算sin(pitch)
    float sp = -m.m32;

    //检查万向锁

    if (fabs(sp) > 0.99999f)
    {
        //向正上看或正下方看
        pitch = kPiOver2 * sp;
        //bank置零，计算heading
        bank = 0.0f;
        heading = atan2(-m.m31, m.m11);
    }
    else
    {
        //计算角度

        heading = atan2(m.m13,m.m33);
        pitch = asin(sp);
        bank = atan2(m.m21,m.m22);
    }

}

void EulerAngles::fromRotationMatrix(const RotationMatrix &m)
{
    //根据m32计算sin(pitch)
    float sp = -m.m32;

    //检查万向锁

    if (fabs(sp) > 0.99999f)
    {
        //向正上看或正下方看
        pitch = kPiOver2 * sp;
        //bank置零，计算heading
        bank = 0.0f;
        heading = atan2(-m.m31, m.m11);
    }
    else
    {
        //计算角度

        heading = atan2(m.m13,m.m33);
        pitch = asin(sp);
        bank = atan2(m.m21,m.m22);
    }
}

3. 欧拉角编程

/////////////////////////////////////////////////////////////////////////////
//
// 3D Math Primer for Games and Graphics Development
//
// EulerAngles.h - Declarations for class EulerAngles
//
// Visit gamemath.com for the latest version of this file.
//
// For more details, see EulerAngles.cpp
//
/////////////////////////////////////////////////////////////////////////////

#ifndef __EULERANGLES_H_INCLUDED__
#define __EULERANGLES_H_INCLUDED__

// Forward declarations

class Quaternion;
class Matrix4x3;
class RotationMatrix;

//---------------------------------------------------------------------------
// class EulerAngles
//
// This class represents a heading-pitch-bank Euler angle triple.

class EulerAngles {
public:

// Public data

    // Straightforward representation.  Store the three angles, in
    // radians

    float   heading;
    float   pitch;
    float   bank;

// Public operations

    // Default constructor does nothing

    EulerAngles() {}

    // Construct from three values

    EulerAngles(float h, float p, float b) :
        heading(h), pitch(p), bank(b) {}

    // Set to identity triple (all zeros)

    void    identity() { pitch = bank = heading = 0.0f; }

    // Determine "canonical" Euler angle triple

    void    canonize();

    // Convert the quaternion to Euler angle format.  The input quaternion
    // is assumed to perform the rotation from object-to-inertial
    // or inertial-to-object, as indicated.

    void    fromObjectToInertialQuaternion(const Quaternion &q);
    void    fromInertialToObjectQuaternion(const Quaternion &q);

    // Convert the transform matrix to Euler angle format.  The input
    // matrix is assumed to perform the transformation from
    // object-to-world, or world-to-object, as indicated.  The
    // translation portion of the matrix is ignored.  The
    // matrix is assumed to be orthogonal.

    void    fromObjectToWorldMatrix(const Matrix4x3 &m);
    void    fromWorldToObjectMatrix(const Matrix4x3 &m);

    // Convert a rotation matrix to Euler Angle form.

    void    fromRotationMatrix(const RotationMatrix &m);
};

// A global "identity" Euler angle constant

extern const EulerAngles kEulerAnglesIdentity;

/////////////////////////////////////////////////////////////////////////////
#endif // #ifndef __EULERANGLES_H_INCLUDED__

/////////////////////////////////////////////////////////////////////////////
//
// 3D Math Primer for Games and Graphics Development
//
// EulerAngles.cpp - Implementation of class EulerAngles
//
// Visit gamemath.com for the latest version of this file.
//
/////////////////////////////////////////////////////////////////////////////

#include <math.h>

#include "EulerAngles.h"
#include "Quaternion.h"
#include "MathUtil.h"
#include "Matrix4x3.h"
#include "RotationMatrix.h"

/////////////////////////////////////////////////////////////////////////////
//
// Notes:
//
// See Chapter 11 for more information on class design decisions.
//
// See section 10.3 for more information on the Euler angle conventions
// assumed.
//
/////////////////////////////////////////////////////////////////////////////

/////////////////////////////////////////////////////////////////////////////
//
// global data
//
/////////////////////////////////////////////////////////////////////////////

// The global "identity" Euler angle constant.  Now we may not know exactly
// when this object may get constructed, in relation to other objects, so
// it is possible for the object to be referenced before it is initialized.
// However, on most implementations, it will be zero-initialized at program
// startup anyway, before any other objects are constructed.

const EulerAngles kEulerAnglesIdentity(0.0f, 0.0f, 0.0f);

/////////////////////////////////////////////////////////////////////////////
//
// class EulerAngles Implementation
//
/////////////////////////////////////////////////////////////////////////////

//---------------------------------------------------------------------------
// EulerAngles::canonize
//
// 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.
//
// See section 10.3 for more information.

void    EulerAngles::canonize() {

    // First, wrap pitch in range -pi ... pi

    pitch = wrapPi(pitch);

    // Now, check for "the back side" of the matrix, pitch outside
    // the canonical range of -pi/2 ... pi/2

    if (pitch < -kPiOver2) {
        pitch = -kPi - pitch;
        heading += kPi;
        bank += kPi;
    } else if (pitch > kPiOver2) {
        pitch = kPi - pitch;
        heading += kPi;
        bank += kPi;
    }

    // OK, now check for the gimbel lock case (within a slight
    // tolerance)

    if (fabs(pitch) > kPiOver2 - 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 = wrapPi(bank);
    }

    // Wrap heading in canonical range

    heading = wrapPi(heading);
}

//---------------------------------------------------------------------------
// EulerAngles::fromObjectToInertialQuaternion
//
// Setup the Euler angles, given an object->inertial rotation quaternion
//
// See 10.6.6 for more information.

void    EulerAngles::fromObjectToInertialQuaternion(const Quaternion &q) {

    // Extract sin(pitch)

    float sp = -2.0f * (q.y*q.z - q.w*q.x);

    // Check for Gimbel lock, giving slight tolerance for numerical imprecision

    if (fabs(sp) > 0.9999f) {

        // Looking straight up or down

        pitch = kPiOver2 * sp;

        // 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 don't have to use the "safe" asin
        // function because we already checked for range errors when
        // checking for Gimbel lock

        pitch   = asin(sp);
        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);
    }
}

//---------------------------------------------------------------------------
// EulerAngles::fromInertialToObjectQuaternion
//
// Setup the Euler angles, given an inertial->object rotation quaternion
//
// See 10.6.6 for more information.

void    EulerAngles::fromInertialToObjectQuaternion(const Quaternion &q) {

    // Extract sin(pitch)

    float sp = -2.0f * (q.y*q.z + q.w*q.x);

    // Check for Gimbel lock, giving slight tolerance for numerical imprecision

    if (fabs(sp) > 0.9999f) {

        // Looking straight up or down

        pitch = kPiOver2 * sp;

        // 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 don't have to use the "safe" asin
        // function because we already checked for range errors when
        // checking for Gimbel lock

        pitch   = asin(sp);
        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);
    }
}

//---------------------------------------------------------------------------
// EulerAngles::fromObjectToWorldMatrix
//
// Setup the Euler angles, given an object->world transformation matrix.
//
// The matrix is assumed to be orthogonal.  The translation portion is
// ignored.
//
// See 10.6.2 for more information.

void    EulerAngles::fromObjectToWorldMatrix(const Matrix4x3 &m) {

    // Extract sin(pitch) from m32.

    float   sp = -m.m32;

    // Check for Gimbel lock

    if (fabs(sp) > 9.99999f) {

        // Looking straight up or down

        pitch = kPiOver2 * sp;

        // Compute heading, slam bank to zero

        heading = atan2(-m.m23, m.m11);
        bank = 0.0f;

    } else {

        // Compute angles.  We don't 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(sp);
        bank = atan2(m.m12, m.m22);
    }
}

//---------------------------------------------------------------------------
// EulerAngles::fromWorldToObjectMatrix
//
// Setup the Euler angles, given a world->object transformation matrix.
//
// The matrix is assumed to be orthogonal.  The translation portion is
// ignored.
//
// See 10.6.2 for more information.

void    EulerAngles::fromWorldToObjectMatrix(const Matrix4x3 &m) {

    // Extract sin(pitch) from m23.

    float   sp = -m.m23;

    // Check for Gimbel lock

    if (fabs(sp) > 9.99999f) {

        // Looking straight up or down

        pitch = kPiOver2 * sp;

        // Compute heading, slam bank to zero

        heading = atan2(-m.m31, m.m11);
        bank = 0.0f;

    } else {

        // Compute angles.  We don't 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(sp);
        bank = atan2(m.m21, m.m22);
    }
}

//---------------------------------------------------------------------------
// EulerAngles::fromRotationMatrix
//
// Setup the Euler angles, given a rotation matrix.
//
// See 10.6.2 for more information.

void    EulerAngles::fromRotationMatrix(const RotationMatrix &m) {

    // Extract sin(pitch) from m23.

    float   sp = -m.m23;

    // Check for Gimbel lock

    if (fabs(sp) > 9.99999f) {

        // Looking straight up or down

        pitch = kPiOver2 * sp;

        // Compute heading, slam bank to zero

        heading = atan2(-m.m31, m.m11);
        bank = 0.0f;

    } else {

        // Compute angles.  We don't 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(sp);
        bank = atan2(m.m21, m.m22);
    }
}