四元数欧拉角算法 - 如何转换为'Y =向上'和手型之间？

Question

我有一个在四元数和欧拉角之间转换的算法。

public static Vector3 ToEulerAngles(this Quaternion q) 
    { 
     // Store the Euler angles in radians 
     Vector3 pitchYawRoll = new Vector3(); 

     double sqw = q.W * q.W; 
     double sqx = q.X * q.X; 
     double sqy = q.Y * q.Y; 
     double sqz = q.Z * q.Z; 

     // If quaternion is normalised the unit is one, otherwise it is the correction factor 
     double unit = sqx + sqy + sqz + sqw; 
     double test = q.X * q.Y + q.Z * q.W; 

     if (test > 0.4999f * unit)        // 0.4999f OR 0.5f - EPSILON 
     { 
      // Singularity at north pole 
      pitchYawRoll.Y = 2f * (float)Math.Atan2(q.X, q.W); // Yaw 
      pitchYawRoll.X = PI * 0.5f;       // Pitch 
      pitchYawRoll.Z = 0f;        // Roll 
      return pitchYawRoll; 
     } 
     else if (test < -0.4999f * unit)      // -0.4999f OR -0.5f + EPSILON 
     { 
      // Singularity at south pole 
      pitchYawRoll.Y = -2f * (float)Math.Atan2(q.X, q.W); // Yaw 
      pitchYawRoll.X = -PI * 0.5f;      // Pitch 
      pitchYawRoll.Z = 0f;        // Roll 
      return pitchYawRoll; 
     } 
     else 
     { 
      pitchYawRoll.Y = (float)Math.Atan2(2f * q.Y * q.W - 2f * q.X * q.Z, sqx - sqy - sqz + sqw);  // Yaw 
      pitchYawRoll.X = (float)Math.Asin(2f * test/unit);            // Pitch 
      pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.W - 2f * q.Y * q.Z, -sqx + sqy - sqz + sqw);  // Roll 
     } 

     return pitchYawRoll; 
    } 

该方法仅适用于Z轴向上的右手笛卡尔坐标系。

为了使Y轴指向上而不是Z，我会改变什么？ （交换X和Z工作吗？）

如何容纳左手坐标系？

编辑：

public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) 
{ 
float num = roll * 0.5f; 
float num2 = (float)Math.Sin((double)num); 
float num3 = (float)Math.Cos((double)num); 
float num4 = pitch * 0.5f; 
float num5 = (float)Math.Sin((double)num4); 
float num6 = (float)Math.Cos((double)num4); 
float num7 = yaw * 0.5f; 
float num8 = (float)Math.Sin((double)num7); 
float num9 = (float)Math.Cos((double)num7); 
Quaternion result; 
result.X = num9 * num5 * num3 + num8 * num6 * num2; 
result.Y = num8 * num6 * num3 - num9 * num5 * num2; 
result.Z = num9 * num6 * num2 - num8 * num5 * num3; 
result.W = num9 * num6 * num3 + num8 * num5 * num2; 
return result; 
} 

Answer 1

在这里被改变使用偏航，俯仰，滚动的相同的定义的方法：

public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) 
{ 
    float rollOver2 = roll * 0.5f; 
    float sinRollOver2 = (float)Math.Sin((double)rollOver2); 
    float cosRollOver2 = (float)Math.Cos((double)rollOver2); 
    float pitchOver2 = pitch * 0.5f; 
    float sinPitchOver2 = (float)Math.Sin((double)pitchOver2); 
    float cosPitchOver2 = (float)Math.Cos((double)pitchOver2); 
    float yawOver2 = yaw * 0.5f; 
    float sinYawOver2 = (float)Math.Sin((double)yawOver2); 
    float cosYawOver2 = (float)Math.Cos((double)yawOver2); 
    Quaternion result; 
    result.X = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; 
    result.Y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; 
    result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; 
    result.W = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; 
    return result; 
} 

对于ToEulerAngles（ommitted奇点）：

pitchYawRoll.Y = (float)Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (sqz + sqw));  // Yaw 
pitchYawRoll.X = (float)Math.Asin(2f * (q.X * q.Z - q.W * q.Y));        // Pitch 
pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (sqy + sqz));  // Roll 

我进行了以下测试：

var q = CreateFromYawPitchRoll(0.2f, 0.3f, 0.7f); 
var e = ToEulerAngles(q); 
var q2 = CreateFromYawPitchRoll(e.Y, e.X, e.Z); 

具有以下结果;

e = (0.3, 0.2, 0.7) //pitch, yaw, roll 
q2 = q 

来源：Wikipedia