在矩阵中查找旋转轴和角度

我试图从3x3旋转矩阵提取旋转轴和旋转角度。我目前正在使用这种方法，但我不确定自己是否正在跟踪。我在我的程序的其他地方遇到了问题，并认为这可能是一个原因，但一直无法亲自验证。在矩阵中查找旋转轴和角度

我正在使用XNA框架，如果有帮助的话。我看着使用Matrix.Decompose（），它返回的比例（vec3）翻译（vec3）和旋转（四元数），但我不知道我可以从这些值中提取我需要的数据数据。

public static void MatrixRotationAxisAngle(Matrix m, ref float theta, ref Vector3 rot_axis) { 
     float trace = m.M11 + m.M22 + m.M33; 
     float cos_theta = 0.5f * (trace - 1.0f); 
     if (cos_theta > 0.999999875f) { 
      //allow some epsilon value 
      theta = 0.0f; 
      rot_axis = new Vector3(1.0f, 0.0f, 0.0f); 
     } else if (cos_theta > -0.999999875f) { 

      theta = (float)Math.Acos(cos_theta); 
      rot_axis.X = (m.Up.Z - m.Forward.Y); 
      rot_axis.Y = (m.Forward.X - m.Right.Z); 
      rot_axis.Z = (m.Right.Y - m.Up.X); 
      rot_axis.Normalize(); 

     } else { //angle within PI limits 
      theta = (float)Math.PI; 
      //find index of largest diag term in matrix 
      int index = 0; 

      if (m.M11 > m.M22 && m.M11 > m.M33) { 
       index = 0; 
      } 
      if (m.M22 > m.M11 && m.M22 > m.M33) { 
       index = 1; 
      } 
      if (m.M33 > m.M11 && m.M33 > m.M22) { 
       index = 2; 
      } 

      switch (index) { 
       case 0: 
        float ix = 1.0f/rot_axis.X; 
        rot_axis.X = (float)Math.Sqrt(m.M11 + 1.0f); 
        rot_axis.Y = m.M12 * ix; 
        rot_axis.X = m.M13 * ix; 
        break; 
       case 1: 
        float iy = 1.0f/rot_axis.Y; 
        rot_axis.Y = (float)Math.Sqrt(m.M22 + 1.0f); 
        rot_axis.X = m.M21 * iy; 
        rot_axis.Z = m.M23 * iy; 
        break; 
       case 2: 
        float iz = 1.0f/rot_axis.Y; 
        rot_axis.Z = (float)Math.Sqrt(m.M33 + 1.0f); 
        rot_axis.X = m.M31 * iz; 
        rot_axis.Y = m.M32 * iz; 
        break; 
      } 
      rot_axis.Normalize(); 
     } 
    }

来源

2012-02-08 Matt Weichselbaum