Thuật toán góc độ Quaternion thành Euler - Cách chuyển đổi thành 'Y = Up' và giữa tính thuận tay?

Question

Tôi có một thuật toán để chuyển đổi giữa các góc Quaternion và Euler.

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; 
    } 

Phương pháp này chỉ hoạt động cho hệ tọa độ Descartes thuận tay phải với trục Z hướng lên trên.

Tôi sẽ thay đổi điều gì để làm cho trục Y trỏ lên thay vì Z? (Liệu việc hoán đổi X và Z có hoạt động không?)

Làm thế nào tôi có thể chứa các hệ tọa độ tay trái?

EDIT:

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

Dưới đây là thay đổi phương pháp sử dụng cùng một định nghĩa của yaw, pitch, roll:

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; 
} 

Đối ToEulerAngles (singularities 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 

Tôi đã thực hiện thử nghiệm sau:

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

với các kết quả sau;

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

Nguồn: Wikipedia