本文共 11841 字，大约阅读时间需要 39 分钟。
欧拉角描述了一个旋转序列。分别计算出每个旋转的矩阵再将它们连成一个矩阵，这个矩阵就代表了整个角位移。注意，要区分物体-惯性矩阵还是惯性-物体矩阵，它们互逆（也互为转置矩阵）。
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;} 注意以下几点：
- 必须知道矩阵代表物体-惯性坐标系还是惯性-物体坐标系。
- 因为“别名”问题，要限制欧拉角。
- 要处理浮点数精度的误差。
- 要处理万向锁。
- 这里，只讨论旋转矩阵上的转换。
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); }} ///////////////////////////////////////////////////////////////////////////////// 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 declarationsclass 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 constantextern 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#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); }}
转载地址：http://diyii.baihongyu.com/