新建網頁 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);
}
}