天行健 君子当自强而不息

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

新建网页 1

 
cRotationMatrix类的目的就是处理非常特殊的(也是极其常用的)物体和惯性坐标空间之间的旋转。这个矩阵类不是一般的变换类,我们假定这个类只包含旋转,因此,它是正交的。换句话说,该矩阵表达的是方位,而不是角位移。当你创建这样的矩阵时,不必指定变换的方向(物体坐标空间到惯性坐标空间或是惯性坐标空间到物体坐标空间)。变换的方向在实际执行变换时指定,每个方向对应一个函数。

RotationMatrix.h:

    #ifndef ROTATION_MATRIX_H
   
#define ROTATION_MATRIX_H
   
   
class cVector3;
   
class cEulerAngles;
   
class cQuaternion;
   
   
//---------------------------------------------------------------------------
    // Implement a simple 3x3 matrix that is used for ROTATION ONLY.  The
    // matrix is assumed to be orthogonal.  The direction of transformation
    // is specified at the time of transformation.
    //---------------------------------------------------------------------------
   
class cRotationMatrix
    {
   
public:
        
float    m11, m12, m13;
        
float    m21, m22, m23;
        
float    m31, m32, m33;    
   
   
public:
        
void identity();
   
        
// setup the matrix with a specified orientation
   
    void setup(const cEulerAngles& orientation);
   
        
// setup the matrix from a quaternion, assuming the quaternion performs the
        // rotation in the specified direction of transformation.
   
    void from_inertial_to_object_quat(const cQuaternion& q);
        
void from_object_to_inertial_quat(const cQuaternion& q);
   
        
// perform rotations
   
    cVector3 inertial_to_object(const cVector3& v) const;
        cVector3 object_to_inertial(
const cVector3& v) const;
    };
   
   
#endif

因为cRotationMatrix类很简单,因此也非常容易使用。首先,用欧拉角或四元数设置矩阵。如果使用四元数,还必须指明该四元数代表哪种角位移。一旦创建了矩阵,就能用inertial_to_object()object_to_inertial()函数执行旋转。

cRotationMatrix.cpp

    #include "vector3.h"
    #include "RotationMatrix.h"
    #include "MathUtil.h"
    #include "Quaternion.h"
    #include "EulerAngles.h"
   
   
/////////////////////////////////////////////////////////////////////////////
   
//
    // MATRIX ORGANIZATION
    //
    // A user of this class should rarely care how the matrix is organized.
    // However, it is of course important that internally we keep everything
    // straight.
    //
    // The matrix is assumed to be a rotation matrix only, and therefore
    // orthoganal.  The "forward" direction of transformation (if that really
    // even applies in this case) will be from inertial to object space.
    // To perform an object->inertial rotation, we will multiply by the
    // transpose.
    //
    // In other words:
    //
    // Inertial to object:
    //
    //                  | m11 m12 m13 |
    //     [ ix iy iz ] | m21 m22 m23 | = [ ox oy oz ]
    //                  | m31 m32 m33 |
    //
    // Object to inertial:
    //
    //                  | m11 m21 m31 |
    //     [ ox oy oz ] | m12 m22 m32 | = [ ix iy iz ]
    //                  | m13 m23 m33 |
    //
    // Or, using column vector notation:
    //
    // Inertial to object:
    //
    //     | m11 m21 m31 | | ix |    | ox |
    //     | m12 m22 m32 | | iy | = | oy |
    //     | m13 m23 m33 | | iz |    | oz |
    //
    // Object to inertial:
    //
    //     | m11 m12 m13 | | ox |    | ix |
    //     | m21 m22 m23 | | oy | = | iy |
    //     | m31 m32 m33 | | oz |    | iz |
    //
   
/////////////////////////////////////////////////////////////////////////////
   

   
//---------------------------------------------------------------------------
    // Set the matrix to the identity matrix
    //---------------------------------------------------------------------------
   
void cRotationMatrix::identity()
    {
        m11 = 1.0f;    m12 = 0.0f; m13 = 0.0f;
        m21 = 0.0f;    m22 = 1.0f; m23 = 0.0f;
        m31 = 0.0f;    m32 = 0.0f; m33 = 1.0f;
    }
   
   
//-----------------------------------------------------------------------------------------------------
    // Setup the matrix with the specified orientation
    //
    //     | 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  |
    //-----------------------------------------------------------------------------------------------------
   
void cRotationMatrix::setup(const cEulerAngles& orientation)
    {
        
// Fetch sine and cosine of angles
   

        
float sh,ch, sp,cp, sb,cb;
   
        sin_cos(&sh, &ch, orientation.heading);
        sin_cos(&sp, &cp, orientation.pitch);
        sin_cos(&sb, &cb, orientation.bank);
   
        
// Fill in the matrix elements
   

        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; 
    }
   
   
//-----------------------------------------------------------------------------------------
    // Setup the matrix, given a quaternion that performs an inertial->object rotation.
    //
    //         | 1 - 2(y^2 + z^2)        2(xy + wz)            2(xz - wy)         |
    // M = | 2(xy - wz)                1 - 2(x^2 + z^2)    2(yz + wx)         |
    //         | 2(xz + wy)                2(yz - wx)            1 - 2(x^2 + y^2) |
    //-----------------------------------------------------------------------------------------
   
void cRotationMatrix::from_inertial_to_object_quat(const cQuaternion& q)
    {
        
// Fill in the matrix elements.  This could possibly be optimized since there are 
        // many common subexpressions. We'll leave that up to the compiler
   

        m11 = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
        m12 = 2.0f * (q.x * q.y + q.w * q.z);
        m13 = 2.0f * (q.x * q.z - q.w * q.y);
   
        m21 = 2.0f * (q.x * q.y - q.w * q.z);
        m22 = 1.0f - 2.0f * (q.x * q.x + q.z * q.z);
        m23 = 2.0f * (q.y * q.z + q.w * q.x);
   
        m31 = 2.0f * (q.x * q.z + q.w * q.y);
        m32 = 2.0f * (q.y * q.z - q.w * q.x);
        m33 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
    }
   
   
//-----------------------------------------------------------------------------------------
    // Setup the matrix, given a quaternion that performs an object->inertial rotation.
    //
    //         | 1 - 2(y^2 + z^2)        2(xy - wz)            2(xz + wy)         |
    // M = | 2(xy + wz)                1 - 2(x^2 + z^2)    2(yz - wx)         |
    //         | 2(xz - wy)                2(yz + wx)            1 - 2(x^2 + y^2) |
    //-----------------------------------------------------------------------------------------
   
void cRotationMatrix::from_object_to_inertial_quat(const cQuaternion& q)
    {
        
// Fill in the matrix elements.  This could possibly be optimized since there are 
        // many common subexpressions. We'll leave that up to the compiler
   

        m11 = 1.0f - 2.0f * (q.y * q.y + q.z * q.z);
        m12 = 2.0f * (q.x * q.y - q.w * q.z);
        m13 = 2.0f * (q.x * q.z + q.w * q.y);
   
        m21 = 2.0f * (q.x * q.y + q.w * q.z);
        m22 = 1.0f - 2.0f * (q.x * q.x + q.z * q.z);
        m23 = 2.0f * (q.y * q.z - q.w * q.x);
   
        m31 = 2.0f * (q.x * q.z - q.w * q.y);
        m32 = 2.0f * (q.y * q.z + q.w * q.x);
        m33 = 1.0f - 2.0f * (q.x * q.x + q.y * q.y);
    }
   
   
//---------------------------------------------------------------------------
    // Rotate a vector from inertial to object space
    //---------------------------------------------------------------------------
   
cVector3 cRotationMatrix::inertial_to_object(const cVector3& v) const
    {
        
// perform the matrix multiplication in the "standard" way
   
    return cVector3(m11 * v.x + m21 * v.y + m31 * v.z,
                        m12 * v.x + m22 * v.y + m32 * v.z,
                        m13 * v.x + m23 * v.y + m33 * v.z);
    }
   
   
//---------------------------------------------------------------------------
    // Rotate a vector from object to inertial space
    //---------------------------------------------------------------------------
   
cVector3 cRotationMatrix::object_to_inertial(const cVector3& v) const
    {
        
// Multiply by the transpose
   
    return cVector3(m11 * v.x + m12 * v.y + m13 * v.z,
                        m21 * v.x + m22 * v.y + m23 * v.z,
                        m31 * v.x + m32 * v.y + m33 * v.z);
    }

posted on 2008-02-19 09:49 lovedday 阅读(494) 评论(0)  编辑 收藏 引用


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


公告

导航

统计

常用链接

随笔分类(178)

3D游戏编程相关链接

搜索

最新评论