Quaternion to Euler angles algorithm - How to convert to 'Y = Up' and between handedness?

Question

I have an algorithm for converting between a Quaternion and Euler angles.

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

This method only works for a right-handed Cartesian coordinate system with the Z axis pointing up.

What would I change in order to make the Y axis point up instead of Z? (Would swapping X and Z work?)

How can I accommodate left handed coordinate systems?

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

Quaternions and euler angles are independent of the alignment of the coordinate system and of the headedness. Yaw, pitch and roll define rotations about the z, y and x axis respecitvely. It does not matter, how the axes are oriented. — Nico Schertler
– Nico Schertler, Commented Jul 15, 2012 at 18:59
If I use this in combination with XNA's Quaternion.CreateFromYawPitchRoll then I do not get the original Quaternion though. msdn.microsoft.com/en-us/library/… — user1423893
– user1423893, Commented Jul 15, 2012 at 23:24
Then the functions probably use different associations of yaw/pitch/roll to the axes. Swap the euler angles, so the definitions match . — Nico Schertler
– Nico Schertler, Commented Jul 16, 2012 at 7:20
Could you please post an example based on the EDIT I have made which now includes the Quaternion.CreateFromYawPitchRoll? — user1423893
– user1423893, Commented Jul 16, 2012 at 11:36

Nico Schertler · Accepted Answer · 2012-07-16 13:19:58Z

Here are changed methods that use the same definition of 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;
}

For 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

I performed the following test:

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

with the following results;

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

Source: Wikipedia

Excellent answer, thanks. Are the singularities correct as originally written?
if (test > 0.4999f * unit) // 0.4999f OR 0.5f - EPSILON { // Singularity at north pole pitchYawRoll.Y = 2f * (float)Math.Atan2(q.Y, q.W); // Yaw pitchYawRoll.X = PI * 0.5f; // Pitch pitchYawRoll.Z = 0f; // Roll return pitchYawRoll; }
Just checked, PI does not produce the correct results. For example FromYawPitchRoll((float)Math.PI, 0f, 0f)
test has to be = q.X * q.Z - q.W * q.Y. However, I am not absolutely sure, about the singularity values. You're best off trying it out.
That equation for 'test' can't be correct. It produces a value of 0 for both (0, 0, 0) and (PI, 0, 0).

Collectives™ on Stack Overflow

Quaternion to Euler angles algorithm - How to convert to 'Y = Up' and between handedness?

1 Answer 1

8 Comments

Linked

Hot Network Questions

Collectives™ on Stack Overflow

1 Answer 1

8 Comments

Linked

Related