四元数与欧拉角之间的转换及程序源码

　  硕士期间，参与的是一个有关于图形学的项目，这个项目中会涉及到底层的四元数、欧拉角等转换。自己查找了一些相关的资料，然后进行总结如下：

      在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。本文主要归纳了两种表达方式的转换，计算公式采用3D笛卡尔坐标系：

     

图1 3D Cartesian coordinate System (from wikipedia)

    定义分别为绕Z轴、Y轴、X轴的旋转角度，如果用Tait-Bryan angle表示，分别为Yaw、Pitch、Roll。

图2 Tait-Bryan angles (from wikipedia)

一、四元数的定义

      

       通过旋转轴和绕该轴旋转的角度可以构造一个四元数：

       其中是绕旋转轴旋转的角度，为旋转轴在x,y,z方向的分量（由此确定了旋转轴）。

        为何这么构造？那是一位数学家的奇思妙思。

二、欧拉角到四元数的转换

三、四元数到欧拉角的转换

       arctan和arcsin的结果是，这并不能覆盖所有朝向(对于角的取值范围已经满足)，因此需要用atan2来代替arctan。

四、在其他坐标系下使用

     在其他坐标系下，需根据坐标轴的定义，调整一下以上公式。如在Direct3D中，笛卡尔坐标系的X轴变为Z轴，Y轴变为X轴，Z轴变为Y轴（无需考虑方向）。

      编写相应代码如下：

//ps: x,y,z,w 分别是四元素的四个值。稍微修改下就可以用。
  // 由旋转矩阵创建四元数
  inlineCQuaternion(const_Matrix4& m)
  {
   floattr, s, q[4];
   inti, j, k;
    
   intnxt[3] = {1, 2, 0 };
   // 计算矩阵轨迹
   tr = m._11 + m._22 + m._33;
    
   // 检查矩阵轨迹是正还是负
   if(tr>0.0f)
   {
    s = sqrt(tr + 1.0f);
    this->w = s / 2.0f;
    s = 0.5f / s;
    this->x = (m._23 - m._32) * s;
    this->y = (m._31 - m._13) * s;
    this->z = (m._12 - m._21) * s;
   }
   else
   {
    // 轨迹是负
    // 寻找m11 m22 m33中的最大分量
    i = 0;
    if(m.m[1][1]>m.m[0][0]) i = 1;
    if(m.m[2][2]>m.m[i][i]) i = 2;
    j = nxt[i];
    k = nxt[j];
     
    s = sqrt((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1.0f);
    q[i] = s * 0.5f;
    if( s!= 0.0f) s = 0.5f / s;
    q[3] = (m.m[j][k] - m.m[k][j]) * s;
    q[j] = (m.m[i][j] - m.m[j][i]) * s;
    q[k] = (m.m[i][k] - m.m[k][i]) * s;
    this->x = q[0];
    this->y = q[1];
    this->z = q[2];
    this->w = q[3];
   }
  };
 
  // 由欧拉角创建四元数
  inlineCQuaternion(const_Vector3& angle)
  {
   floatcx = cos(angle.x/2);
   floatsx = sin(angle.x/2);
   floatcy = cos(angle.y/2);
   floatsy = sin(angle.y/2);
   floatcz = cos(angle.z/2);
   floatsz = sin(angle.z/2);
 
   this->w = cx*cy*cz + sx*sy*sz;
   this->x = sx*cy*cz - cx*sy*sz;
   this->y = cx*sy*cz + sx*cy*sz;
   this->z = cx*cy*sz - sx*sy*cz;
  };
 
  // 给定角度和轴创建四元数
  inlineCQuaternion(_Vector3 anxi, constfloat& angle)
  {
   CVector3 t;
   t.x = anxi.x;
   t.y = anxi.y;
   t.z = anxi.z;
   t.Normalize();
   floatcosa = cos(angle);
   floatsina = sin(angle);
   this->w = cosa;
   this->x = sina * t.x;
   this->y = sina * t.y;
   this->z = sina * t.z;
  };
 
// 由旋转四元数推导出矩阵
  inlineCMatrix4 GetMatrixLH()
  {
   CMatrix4 ret;
   floatxx = x*x;
   floatyy = y*y;
   floatzz = z*z;
   floatxy = x*y;
   floatwz = w*z;
   floatwy = w*y;
   floatxz = x*z;
   floatyz = y*z;
   floatwx = w*x;
 
   ret._11 = 1.0f-2*(yy+zz);
   ret._12 = 2*(xy-wz);
   ret._13 = 2*(wy+xz);
   ret._14 = 0.0f;
 
   ret._21 = 2*(xy+wz);
   ret._22 = 1.0f-2*(xx+zz);
   ret._23 = 2*(yz-wx);
   ret._24 = 0.0f;
 
   ret._31 = 2*(xy-wy);
   ret._32 = 2*(yz+wx);
   ret._33 = 1.0f-2*(xx+yy);
   ret._34 = 0.0f;
 
   ret._41 = 0.0f;
   ret._42 = 0.0f;
   ret._43 = 0.0f;
   ret._44 = 1.0f;
 
   returnret;
  };
  inlineCMatrix4 GetMatrixRH()
  {
   CMatrix4 ret;
   floatxx = x*x;
   floatyy = y*y;
   floatzz = z*z;
   floatxy = x*y;
   floatwz = -w*z;
   floatwy = -w*y;
   floatxz = x*z;
   floatyz = y*z;
   floatwx = -w*x;
 
   ret._11 = 1.0f-2*(yy+zz);
   ret._12 = 2*(xy-wz);
   ret._13 = 2*(wy+xz);
   ret._14 = 0.0f;
 
   ret._21 = 2*(xy+wz);
   ret._22 = 1.0f-2*(xx+zz);
   ret._23 = 2*(yz-wx);
   ret._24 = 0.0f;
 
   ret._31 = 2*(xy-wy);
   ret._32 = 2*(yz+wx);
   ret._33 = 1.0f-2*(xx+yy);
   ret._34 = 0.0f;
 
   ret._41 = 0.0f;
   ret._42 = 0.0f;
   ret._43 = 0.0f;
   ret._44 = 1.0f;
 
   returnret;
  };
 
  // 由四元数返回欧拉角（主要是这个dx api里没有提供）
  inlineCVector3 GetEulerAngle()
  {
   CVector3 ret;
 
   floattest = y*z + x*w;
   if(test > 0.4999f)
   {
    ret.z = 2.0f * atan2(y, w);
    ret.y = PIOver2;
    ret.x = 0.0f;
    returnret;
   }
   if(test < -0.4999f)
   {
    ret.z = 2.0f * atan2(y, w);
    ret.y = -PIOver2;
    ret.x = 0.0f;
    returnret;
   }
   floatsqx = x * x;
   floatsqy = y * y;
   floatsqz = z * z;
   ret.z = atan2(2.0f * z * w - 2.0f * y * x, 1.0f - 2.0f * sqz - 2.0f * sqx);
   ret.y = asin(2.0f * test);
   ret.x = atan2(2.0f * y * w - 2.0f * z * x, 1.0f - 2.0f * sqy - 2.0f * sqx);
      
   returnret;
  };