3D 旋轉中 旋轉矩陣 歐拉角 四元數的相互轉換

1. 背景

最近被 旋轉矩陣、 歐拉角、 四元數 的轉換搞的頭大，所以梳理一下。轉換程序主要參考某個神奇的網頁[1]。這個神奇的網頁有所有的相互轉換，在這里只記錄我用到的。

2. 旋轉矩陣和四元數

旋轉矩陣和四元數都能唯一確定一次旋轉，所以旋轉矩陣和四元數直接的轉換是唯一的，不需要考慮多種情況。

旋轉矩陣轉四元數：

float tr = m00 + m11 + m22

if (tr > 0) { 
  float S = sqrt(tr+1.0) * 2; // S=4*qw 
  qw = 0.25 * S;
  qx = (m21 - m12) / S;
  qy = (m02 - m20) / S; 
  qz = (m10 - m01) / S; 
} else if ((m00 > m11)&(m00 > m22)) { 
  float S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx 
  qw = (m21 - m12) / S;
  qx = 0.25 * S;
  qy = (m01 + m10) / S; 
  qz = (m02 + m20) / S; 
} else if (m11 > m22) { 
  float S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
  qw = (m02 - m20) / S;
  qx = (m01 + m10) / S; 
  qy = 0.25 * S;
  qz = (m12 + m21) / S; 
} else { 
  float S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
  qw = (m10 - m01) / S;
  qx = (m02 + m20) / S;
  qy = (m12 + m21) / S;
  qz = 0.25 * S;
}

3. 四元數歐拉角

這里就需要考慮順序了，可以自己計算，也可以參考某些現成的方案[2]。一般常采用的順序是 Z-Y-X， 我用在Unity，所以也用這個順序。下圖是歐拉角計算四元數的公式，其中第一步為四元數乘法，參考wiki[3]。

 

 

 

4. 參考

[1] 四元數與歐拉角（RPY角）的相互轉換

[2] Maths - Rotation conversions

[3] Conversion between quaternions and Euler angles