3D中的方位和角位移的C++实现（2）

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;
    }