Quaternion - Conversion between YawPitchRoll and EulerAngles produces incorrect result only with pitch of Pi

Question

I have spent some time implementing a couple of algorithms for converting between EulerAngles and Quaternions.

I am testing that the quaternion values are the same with this code

        Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, 0, 0);
        Vector3 rotation = orientation0.ToEulerAngles();
        Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);

        Console.WriteLine(orientation0);
        Console.WriteLine(orientation1);

I have used a previous method discussed here and have since implemented another method described here

    public static Quaternion QuaternionFromYawPitchRoll(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);

        // X = PI is giving incorrect result (pitch)

        // Heading = Yaw
        // Attitude = Pitch
        // Bank = Roll

        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;

        result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2;
        result.X = sinYawOver2 * sinPitchOver2 * cosRollOver2 + cosYawOver2 * cosPitchOver2 * sinRollOver2;
        result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
        result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;

        return result;
    }

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

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

        // 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;
        //double test = q.X * q.Z - q.W * q.Y;

        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 = PIOVER2;                           // 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 = -PIOVER2;                          // 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

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

        return pitchYawRoll;
    }

All my implementations work except for when the pitch value is ±PI.

    Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, PI, 0);
    Vector3 rotation = orientation0.ToEulerAngles();
    Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);

    Console.WriteLine(orientation0);
    Console.WriteLine(orientation1);     // Not the same quaternion values

Why will this not work for that particular value? If it is a singularity then it is not being determined as one in the algorithm and the 'test' value will instead be very close to 0.

Answer 1

Rotation space wraps onto itself. Obviously if you rotate by 2PI around any axis, you end up back where you started. Likewise, if you rotate by PI around an axis, it's the same thing as rotating by -PI around the same axis. Or if you rotate by any angle around an axis, it's the same as rotating by the negation of that angle around the negation of that axis.

All of this means that your quaternion conversion algorithms have to decide what to do when dealing with redundancy. The two orientations that you provide in the comments are the same orientation: (0,0,0,1) and (0,0,0,-1) [I prefer having 'w' in alphabetical order].

You should make sure that you always normalize your quaternions or else you'll eventually get some strange drifting. Other than that, what seems to be happening is that when you rotate by PI around the 'z' axis, floating point round-off or a 'less-than' vs. 'less-than-or-equal-to' discrepancy is pushing the representation around the circle to the point that your algorithm decides to represent the angle as a rotation by -PI around the z-axis. That's the same thing.

In a similar manner, if you rotate by 2PI around any axis, your quaternion might be (-1,0,0,0). But if you rotate by zero, it will be (1,0,0,0). The Euler angle representation coming back from either of those quaternions, however, should be (0,0,0).