天行健 君子当自强而不息

3D中的方位和角位移的C++实现(2)

新建网页 1

cQuaternion类用来以四元数形式保存方位或角位移,在能应用到四元数上的完整数学运算集合中,只有那些对单位四元数有意义的运算才对保存角位移有用,这里没有提供四元数的求负、加减、标量乘、对数操作。

Quaternion.h:

    #ifndef QUATERNION_H
   
#define QUATERNION_H
   
   
class cVector3;
   
class cEulerAngles;
   
    
//---------------------------------------------------------------------------
    // Implement a quaternion, for purposes of representing an angular
    // displacement (orientation) in 3D.
    //---------------------------------------------------------------------------
   
class cQuaternion
    {
   
public:
        
// The 4 values of the quaternion.  Normally, it will not be necessary to manipulate these 
        // directly.  However, we leave them public, since prohibiting direct access
        // makes some operations, such as file I/O, unnecessarily complicated.
   
    float    w, x, y, z;
   
   
public:
        
void identity()
        {
            w = 1.0f;
            x = y = z = 0.0f;
        }
   
        
// setup the quaternion to a specific rotation
   
    void set_to_rotate_about_x(float theta);
        
void set_to_rotate_about_y(float theta);
        
void set_to_rotate_about_z(float theta);
        
void set_to_rotate_about_axis(const cVector3& axis, float theta);
   
        
// setup to perform object<->inertial rotations, given orientation in Euler angle format.
   
    void set_to_rotate_object_to_inertial(const cEulerAngles& orientation);
        
void set_to_rotate_inertial_to_object(const cEulerAngles& orientation);
   
        
// cross product
   
    cQuaternion operator *(const cQuaternion& a) const;
   
        
// multiplication with assignment, as per c++ convention.
   
    cQuaternion& operator *=(const cQuaternion& a);
   
        
void normalize();
   
        
// extract and return the rotation angle and axis
   
    float     get_rotation_angle() const;
        cVector3 get_rotation_axis() 
const;
    };
   
   
extern const cQuaternion g_quat_identity;
   
   
float dot_product(const cQuaternion& a, const cQuaternion& b);
    cQuaternion slerp(
const cQuaternion& q0, const cQuaternion& q1, float t);
    cQuaternion conjugate(
const cQuaternion& q);
    cQuaternion pow(
const cQuaternion& q, float exponent);
   
   
#endif

为了创建一个代表特定角位移的四元数,需要使用set_to_xxx函数中的一个。set_to_rotate_object_to_inertial()set_to_rotate_inertial_to_object()用来将欧拉角转换到四元数形式。第一个函数创建一个四元数,表达从物体空间到惯性空间的旋转,后一个函数返回从惯性空间到物体空间的旋转。

一般使用函数来操作角位移,角位移连接使用operator*()(习惯上,连接顺序从左向右)。conjugate()函数返回一个四元数,该四元数代表的角位移与输入四元数代表的角位移相反。

使用get_rotation_angle()get_rotation_axis()可从四元数中提取旋转角和旋转轴。

normalize()用来处理浮点数误差扩大。如果要对同一四元数执行上百次连续运算,就可能需要调用这个方法。虽然欧拉角向四元数的转换只产生单位化的四元数,避免了误差扩大的可能。但是,矩阵和四元数间的转换却存在这一问题。

Quaternion.cpp:

    #include <assert.h>
    #include <math.h>
    #include "Quaternion.h"
    #include "MathUtil.h"
    #include "vector3.h"
    #include "EulerAngles.h"
   
   
// The global identity quaternion.  Notice that there are no constructors
    // to the Quaternion class, since we really don't need any.
   
const cQuaternion g_quat_identity = { 1.0f, 0.0f, 0.0f, 0.0f };
   
   
//---------------------------------------------------------------------------
    // Setup the quaternion to rotate about the specified axis
    //---------------------------------------------------------------------------
   

   
void cQuaternion::set_to_rotate_about_x(float theta)
    {
        
float half_theta = theta * 0.5f;
   
        w = cos(half_theta);
        x = sin(half_theta);
        y = 0.0f;
        z = 0.0f;
    }
   
   
void cQuaternion::set_to_rotate_about_y(float theta)
    {
        
float half_theta = theta * 0.5f;
   
        w = cos(half_theta);
        x = 0.0f;
        y = sin(half_theta);
        z = 0.0f;
    }
   
   
void cQuaternion::set_to_rotate_about_z(float theta)
    {
        
float half_theta = theta * 0.5f;
   
        w = cos(half_theta);
        x = 0.0f;
        y = 0.0f;
        z = sin(half_theta);
    }
   
   
void cQuaternion::set_to_rotate_about_axis(const cVector3& axis, float theta)
    {
        
// the axis of rotation must be normalized
   
    assert(fabs(vector_mag(axis) - 1.0f) < 0.01f);
   
        
// compute the half angle and its sin
   
    float half_theta = theta * 0.5f;
        
float sin_half_theta = sin(half_theta);
   
        w = cos(half_theta);
        x = axis.x * sin_half_theta;
        y = axis.y * sin_half_theta;
        z = axis.z * sin_half_theta;
    }
   
   
//---------------------------------------------------------------------------
    // Setup the quaternion to perform an object->inertial rotation, given the
    // orientation in Euler angle format.
    //
    //        | cos(h/2)cos(p/2)cos(b/2) + sin(h/2)sin(p/2)sin(b/2) |
    //    M = | cos(h/2)sin(p/2)cos(b/2) + sin(h/2)cos(p/2)sin(b/2) |
    //        | sin(h/2)cos(p/2)cos(b/2) - cos(h/2)sin(p/2)sin(b/2) |
    //        | cos(h/2)cos(p/2)sin(b/2) - sin(h/2)sin(p/2)cos(b/2) |
    //---------------------------------------------------------------------------
   
void cQuaternion::set_to_rotate_object_to_inertial(const cEulerAngles& orientation)
    {
        
// compute sine and cosine of the half angles
   

        
float sp, sb, sh;
        
float cp, cb, ch;
   
        sin_cos(&sp, &cp, orientation.pitch * 0.5f);
        sin_cos(&sb, &cb, orientation.bank * 0.5f);
        sin_cos(&sh, &ch, orientation.heading * 0.5f);
   
        w =  ch * cp * cb + sh * sp * sb;
        x =  ch * sp * cb + sh * cp * sb;
        y = -ch * sp * sb + sh * cp * cb;
        z = -sh * sp * cb + ch * cp * sb;
    }
   
   
//---------------------------------------------------------------------------
    // Setup the quaternion to perform an object->inertial rotation, given the
    // orientation in Euler angle format.
    //
    //        |  cos(h/2)cos(p/2)cos(b/2) + sin(h/2)sin(p/2)sin(b/2) |
    //    M = | -cos(h/2)sin(p/2)cos(b/2) - sin(h/2)cos(p/2)sin(b/2) |
    //        |  cos(h/2)sin(p/2)sin(b/2) - sin(h/2)cos(p/2)cos(b/2) |
    //        |  sin(h/2)sin(p/2)cos(b/2) - cos(h/2)cos(p/2)sin(b/2) |
    //---------------------------------------------------------------------------
   
void cQuaternion::set_to_rotate_inertial_to_object(const cEulerAngles& orientation)
    {
        
// compute sine and cosine of the half angles
   

        
float sp, sb, sh;
        
float cp, cb, ch;
   
        sin_cos(&sp, &cp, orientation.pitch * 0.5f);
        sin_cos(&sb, &cb, orientation.bank * 0.5f);
        sin_cos(&sh, &ch, orientation.heading * 0.5f);
   
        w =  ch * cp * cb + sh * sp * sb;
        x = -ch * sp * cb - sh * cp * sb;
        y =  ch * sp * sb - sh * cp * cb;
        z =  sh * sp * cb - ch * cp * sb;
    }
   
   
//---------------------------------------------------------------------------
    // Quaternion cross product, which concatenates multiple angular
    // displacements.  The order of multiplication, from left to right,
    // corresponds to the order that the angular displacements are
    // applied.  This is backwards from the *standard* definition of
    // quaternion multiplication. 
    //---------------------------------------------------------------------------
   
cQuaternion cQuaternion::operator *(const cQuaternion& a) const
    {
        cQuaternion result;
   
        result.w = w * a.w - x * a.x - y * a.y - z * a.z;
        result.x = w * a.x + x * a.w + z * a.y - y * a.z;
        result.y = w * a.y + y * a.w + x * a.z - z * a.x;
        result.z = w * a.z + z * a.w + y * a.x - x * a.y;
   
        
return result;
    }
     
   
//---------------------------------------------------------------------------
    // Combined cross product and assignment, as per C++ convention.
    //---------------------------------------------------------------------------
   
cQuaternion& cQuaternion::operator *=(const cQuaternion& a)
    {
        *
this = *this * a;
   
        
return *this;
    }
   
   
//---------------------------------------------------------------------------
    // "Normalize" a quaternion.  Note that normally, quaternions
    // are always normalized (within limits of numerical precision).
    //
    // This function is provided primarily to combat floating point "error
    // creep," which can occur when many successive quaternion operations
    // are applied.
    //---------------------------------------------------------------------------
   
void cQuaternion::normalize()
    {
        
// compute magnitude of the quaternion
   
    float mag = sqrt(w * w + x * x + y * y + z * z);
   
        
// check for bogus length, to protect against divide by zero.
   
    if(mag > 0.0f)
        {
            
// normalize it
   

            
float one_over_mag = 1.0f / mag;
   
            w *= one_over_mag;
            x *= one_over_mag;
            y *= one_over_mag;
            z *= one_over_mag;
        }
        
else
        {
            
// houston, we have a problem.
   
        assert(false);
   
            
// in a release build, just slam it to something.
   
        identity();
        }
    }
   
   
//---------------------------------------------------------------------------
    // Return the rotation angle theta
    //---------------------------------------------------------------------------
   
float cQuaternion::get_rotation_angle() const
    {
        
// compute the half angle, remember that w = cos(theta / 2)
   
    float half_theta = safe_acos(w);
   
        
return half_theta * 2.0f;
    }
   
   
//---------------------------------------------------------------------------
    // Return the rotation axis
    //---------------------------------------------------------------------------
   
cVector3 cQuaternion::get_rotation_axis() const
    {
        
// compute sin^2(theta/2), remember that w = cos(theta/2), and sin^2(x) + cos^2(x) = 1.
   
    float sin_theta_square = 1.0f - w * w;
   
        
// protect against numerical imprecision
   
    if(sin_theta_square <= 0.0f)
        {
            
// identity quaterion, or numerical imprecision.
            // just return any valid vector, since it does not matter.
   
        return cVector3(1.0f, 0.0f, 0.0f);
        }
   
        
// compute 1 / sin(theta/2)
   
    float k = 1.0f / sqrt(sin_theta_square);
   
        
// return axis of rotation
   
    return cVector3(x * k, y * k, z * k);
    }
   
   
//////////////////////////////////////  Nonmember functions ////////////////////////////////////////
   

    
//---------------------------------------------------------------------------
    // Quaternion dot product.  We use a nonmember function so we can
    // pass quaternion expressions as operands without having "funky syntax"
    //---------------------------------------------------------------------------
   
float dot_product(const cQuaternion& a, const cQuaternion& b)
    {
        
return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
    }
   
    
//---------------------------------------------------------------------------
    // Spherical linear interpolation.
    //---------------------------------------------------------------------------
   
cQuaternion slerp(const cQuaternion& q0, const cQuaternion& q1, float t)
    {
        
// check for out-of range parameter and return edge points if so
   
    if(t <= 0.0f)    return q0;
        
if(t >= 1.0f)    return q1;
   
        
// compute "cosine of angle between quaternions" using dot product
   
    float cos_omega = dot_product(q0, q1);
   
        
// If negative dot, use -q1.  Two quaternions q and -q
        // represent the same rotation, but may produce different slerp.  
        // We chose q or -q to rotate using the acute angle.
   

        
float q1w = q1.w;
        
float q1x = q1.x;
        
float q1y = q1.y;
        
float q1z = q1.z;
   
        
if(cos_omega < 0.0f)
        {
            q1w = -q1w;
            q1x = -q1x;
            q1y = -q1y;
            q1z = -q1z;
   
            cos_omega = -cos_omega;
        }
   
        
// we should have two unit quaternions, so dot should be <= 1.0
   
    assert(cos_omega < 1.1f);
   
        
// compute interpolation fraction, checking for quaternions almost exactly the same.
   

        
float k0, k1;
        
        
if(cos_omega > 0.9999f)
        {
            
// very close - just use linear interpolation, which will protect against a divide by zero.
   
        k0 = 1.0f - t;
            k1 = t;
        }
        
else
        {
            
// compute the sin of the angle using the trig identity sin^2(omega) + cos^2(omega) = 1
   
        float sin_omega = sqrt(1.0f - cos_omega * cos_omega);
   
            
// compute the angle from its sin and cosin
   
        float omega = atan2(sin_omega, cos_omega);
   
            
// compute inverse of denominator, so we only have to divice once.
   
        float k = 1.0f / sin_omega;
   
            
// compute interpolation perameters
   
            k0 = sin((1.0f - t) * omega) * k;
            k1 = sin(t * omega) * k;
        }
   
        cQuaternion result;
   
        result.x = k0 * q0.x + k1 * q1x;
        result.y = k0 * q0.y + k1 * q1y;
        result.z = k0 * q0.z + k1 * q1z;
        result.w = k0 * q0.w + k1 * q1w;
   
        
return result;
    }
   
   
//---------------------------------------------------------------------------
    // Compute the quaternion conjugate.  This is the quaternian
    // with the opposite rotation as the original quaternian.
    //---------------------------------------------------------------------------
   
cQuaternion conjugate(const cQuaternion& q)
    {
        cQuaternion result;
   
        
// same rotation amount
   
    result.w = q.w;
   
        
// opposite axis of rotation
   
    result.x = -q.x;
        result.y = -q.y;
        result.z = -q.z;
        
        
return result;
    }
   
   
//---------------------------------------------------------------------------
    // Quaternion exponentiation.
    //---------------------------------------------------------------------------
   
cQuaternion pow(const cQuaternion& q, float exponent)
    {
        
// check for the case of an identity quaternion.
        // this will protect against divide by zero.
   

        
if(fabs(q.w) > 0.9999f)
            
return q;
   
        
// extract the half angle alpha (alpha = theta/2)
   
    float alpha = acos(q.w);
   
        
// compute new alpha value
   
    float new_alpha = alpha * exponent;
   
        
// compute new w value
   
    cQuaternion result;
        result.w = cos(new_alpha);
   
        
// compute new xyz values
   

        
float mult = sin(new_alpha) / sin(alpha);
   
        result.x = q.x * mult;
        result.y = q.y * mult;
        result.z = q.z * mult;
   
        
return result;
    }

posted on 2008-02-18 19:31 lovedday 阅读(733) 评论(0)  编辑 收藏 引用


只有注册用户登录后才能发表评论。
网站导航: 博客园   IT新闻   BlogJava   知识库   博问   管理


公告

导航

统计

常用链接

随笔分类(178)

3D游戏编程相关链接

搜索

最新评论