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