新建网页 1
数学理论基础请参阅3D中的方位和角位移。
处理变换是一件非常令人头疼的事，矩阵更是棘手。如果你曾经编写过关于矩阵的代码并且没有用设计良好的类，你会发现经常要处理负号、转置矩阵或翻转连接顺序以使其能正常工作。
下面这几个类正是为了消除在编程中经常遇到的这类问题而设计的。例如，很少需要直接访问矩阵或四元数中的元素，因此特意限制了可用操作的数目以避免产生迷惑，再如，对cRotationMatrix类，没有求逆和连接操作，因为如果按其本身的目的使用cRotationMatrix，这些操作是不应该出现或没有意义的。
我们还使用了一系列简单、常用的数学常数和实用工具函数，它们由MathUtil.h和MathUtil.cpp提供。
MathUtil.h：
#ifndef MATH_UTIL_H
#define MATH_UTIL_H
#include <math.h>
// declare a global constant for pi and a few multiples.
const float G_PI = 3.14159265f;
const float G_2PI = G_PI * 2.0f;
const float G_PI_OVER_2 = G_PI / 2.0f;
const float G_1_OVER_PI = 1.0f / G_PI;
const float G_1_OVER_2PI = 1.0f / G_2PI;
const float G_PI_OVER_180 = G_PI / 180.0f;
const float G_180_OVER_PI = 180.0f / G_PI;
float wrap_pi(float theta);
float safe_acos(float x);
// convert between degrees and radians
inline float deg_to_rad(float deg) { return deg * G_PI_OVER_180; }
inline float rad_to_deg(float rad) { return rad * G_180_OVER_PI; }
// compute the sin and cosine of an angle. on some platforms, if we know that we need
// both values, it can be computed faster than computing the two values seperately.
inline void sin_cos(float* ret_sin, float* ret_cos, float theta)
{
// for simplicity, we will just use the normal trig functions.
// note that on some platforms we may be able to do better.
*ret_sin = sin(theta);
*ret_cos = cos(theta);
}
// convert between "field of view" and "zoom", the fov angle is speficied in radians.
inline float fov_to_zoom(float fov) { return 1.0f / tan(fov * 0.5f); }
inline float zoom_to_fov(float zoom) { return 2.0f * atan(1.0f / zoom); }
#endif
MathUtil.cpp:
关于类cVector3的实现细节请参阅一个3D向量类。
#include "MathUtil.h"
#include "vector3.h"
const cVector3 g_zero_vector(0.0f, 0.0f, 0.0f);
float wrap_pi(float theta)
{
// "wrap" an angle in range -pi
pi by adding the correct multiple of 2 pi
theta += G_PI;
theta -= floor(theta * G_1_OVER_2PI) * G_2PI;
theta -= G_PI;
return theta;
}
float safe_acos(float x)
{
// Same as acos(x), but if x is out of range, it is "clamped" to the nearest valid value.
// The value returned is in range 0pi, the same as the standard C acos() function.
// check limit conditions
if(x <= -1.0f)
return G_PI;
if(x >= 1.0f)
return 0.0f;
// value is in the domain - use standard C function.
return acos(x);
}
cEulerAngles类：
cEulerAngles类用来以欧拉角形式保存方位，使用heading-pitch-bank约定。这个类非常直观，为了简单起见，我们没有实现太多操作。特别没有实现加、减、标量乘等运算。因为如果该类保存的不是方位而是角速度或变化率，那么这些运算才是有用的。
EulerAngles.h:
#ifndef EULER_ANGLES_H
#define EULER_ANGLES_H
class cQuaternion;
class cMatrix4x3;
class cRotationMatrix;
//---------------------------------------------------------------------------
// This class represents a heading-pitch-bank Euler angle triple.
//---------------------------------------------------------------------------
class cEulerAngles
{
public:
// store three angles, in radians.
float heading;
float pitch;
float bank;
public:
cEulerAngles() {}
cEulerAngles(float h, float p, float b)
{
heading = h;
pitch = p;
bank = b;
}
// set to identity triple (all zeors)
void identity()
{
pitch = bank = heading = 0.0f;
}
void canonize();
void from_object_to_inertial_quat(const cQuaternion& q);
void from_inertial_to_object_quat(const cQuaternion& q);
void from_object_to_world_matrix(const cMatrix4x3& m);
void from_world_to_object_matrix(const cMatrix4x3& m);
void from_rotation_matrix(const cRotationMatrix& m);
};
extern const cEulerAngles g_euler_angles_identity;
#endif
cEulerAngles类的用法也很直观，只有几个地方需要加以详细说明：
（1）canonize()函数的作用是确保欧拉角位于"限制集"中。
（2）from_object_to_inertial_quat()和from_inertial_to_object_quat()函数根据四元数计算欧拉角，第一个函数的参数是代表从物体坐标系到惯性坐标系旋转的四元数，第二个函数的参数是代表从惯性坐标系到物体坐标系旋转的四元数。
（3）同样，from_object_to_world_matrix()和from_world_to_object_matrix()函数把矩阵的旋转部分的方位转换为欧拉角，假设这个被转换的矩阵是正交的。
EulerAngles.cpp:
#include <math.h>
#include "EulerAngles.h"
#include "Quaternion.h"
#include "MathUtil.h"
#include "Matrix4x3.h"
#include "RotationMatrix.h"
const cEulerAngles g_euler_angles_identity(0.0f, 0.0f, 0.0f);
//---------------------------------------------------------------------------
// 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.
//---------------------------------------------------------------------------
void cEulerAngles::canonize()
{
// first, wrap pitch in range -pi pi
pitch = wrap_pi(pitch);
// now, check for "the back side" of the matrix, pitch outside the canonical range
// of -pi/2 pi/2
if(pitch < -G_PI_OVER_2)
{
pitch = -G_PI - pitch;
heading += G_PI;
bank += G_PI;
}
else if(pitch > G_PI_OVER_2)
{
pitch = G_PI - pitch;
heading += G_PI;
bank += G_PI;
}
// ok, now check for the gimbel lock case (within a slight tolerance)
if(fabs(pitch) > G_PI_OVER_2 - 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 = wrap_pi(bank);
}
// wrap heading in canonical range
heading = wrap_pi(heading);
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given an object->inertial rotation quaternion.
//
// p = asin(-m23) = asin(-2(yz - wx))
//
// h = atan2(m13, m33) = atan2(xz + wy, 1/2 - x^2 - y^2) cosp != 0
// h = atan2(-m31, m11) = atan2(-xz + wy, 1/2 - y^2 - z^2) cosp == 0
//
// b = atan2(m21, m22) = atan2(xy + wz, 1/2 - x^2 - z^2) cosp != 0
// b = 0 cosp == 0
//---------------------------------------------------------------------------
void cEulerAngles::from_object_to_inertial_quat(const cQuaternion& q)
{
// extract sin(pitch)
float sin_pitch = -2.0f * (q.y * q.z - q.w * q.x);
// check for gimbel lock, giving slight tolerance for numerical imprecision.
if(fabs(sin_pitch) > 0.9999f)
{
// looking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// 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 do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
pitch = asin(sin_pitch);
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);
}
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given an inertial->object rotation quaternion.
//
// p = asin(-m23) = asin(-2(yz + wx))
//
// h = atan2(m13, m33) = atan2(xz - wy, 1/2 - x^2 - y^2) cosp != 0
// h = atan2(-m31, m11) = atan2(-xz - wy, 1/2 - y^2 - z^2) cosp == 0
//
// b = atan2(m21, m22) = atan2(xy - wz, 1/2 - x^2 - z^2) cosp != 0
// b = 0 cosp == 0
//---------------------------------------------------------------------------
void cEulerAngles::from_inertial_to_object_quat(const cQuaternion& q)
{
// extract sin(pitch)
float sin_pitch = -2.0f * (q.y * q.z + q.w * q.x);
// check for gimbel lock, giving slight tolerance for numerical imprecision.
if(fabs(sin_pitch) > 0.9999f)
{
// looking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// 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 do not have to use the "safe" asin function because we already
// checked for range errors when checking for gimbel lock.
pitch = asin(sin_pitch);
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);
}
}
//------------------------------------------------------------------------------------------------
// Setup the Euler angles, given an object->world transformation matrix.
// The matrix is assumed to be orthogonal. The translation portion is ignored.
//
// | cosh * cosb + sinh * sinp * sinb sinb * cosp -sinh * cosb + cosh * sinp * sinb |
// M = | -cosh * sinb + sinh * sinp * cosb cosb * cosp sinb * sinh + cosh * sinp * cosb |
// | sinh * cosp -sinp cosh * cosp |
//
// [1]: cosp != 0
//
// p = asin(-m32)
// h = atan2(m31, m33)
// b = atan2(m12, m22)
//
// [2]: cosp = 0, b = 0, sinb = 0, cosb = 1
//
// | cosh 0 -sinh |
// M = | sinh * sinp 0 cosh * sinp |
// | 0 -sinp 0 |
//
// p = pi/2 * (-m32)
// h = atan2(-m13, m11)
// b = 0
//------------------------------------------------------------------------------------------------
void cEulerAngles::from_object_to_world_matrix(const cMatrix4x3& m)
{
// extract sin(pitch) from m32
float sin_pitch = -m.m32;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m13, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not 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(sin_pitch);
bank = atan2(m.m12, m.m22);
}
}
//-----------------------------------------------------------------------------------------------------
// Setup the Euler angles, given a world->object transformation matrix.
// The matrix is assumed to be orthogonal. The translation portion is ignored.
//
// | 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 |
//
// [1]: cosp != 0
//
// p = asin(-m23)
// h = atan2(m13, m33)
// b = atan2(m21, m22)
//
// [2]: cosp = 0, b = 0, sinb = 0, cosb = 1
//
// | cosh sinh * sinp 0 |
// M = | 0 0 -sinp |
// | -sinh cosh * sinp 0 |
//
// p = pi/2 * (-m23)
// h = atan2(-m31, m11)
// b = 0
//-----------------------------------------------------------------------------------------------------
void cEulerAngles::from_world_to_object_matrix(const cMatrix4x3& m)
{
// extract sin(pitch) from m23
float sin_pitch = -m.m23;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m31, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not 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(sin_pitch);
bank = atan2(m.m21, m.m22);
}
}
//---------------------------------------------------------------------------
// Setup the Euler angles, given a rotation matrix.
//---------------------------------------------------------------------------
void cEulerAngles::from_rotation_matrix(const cRotationMatrix& m)
{
// extract sin(pitch) from m23
float sin_pitch = -m.m23;
// check for gimbel lock
if(fabs(sin_pitch) > 0.99999f)
{
// locking straight up or down
pitch = G_PI_OVER_2 * sin_pitch;
// compute heading, slam bank to zero.
heading = atan2(-m.m31, m.m11);
bank = 0.0f;
}
else
{
// compute angles, we do not 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(sin_pitch);
bank = atan2(m.m21, m.m22);
}
}