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

前端 未结 1 781
半阙折子戏
半阙折子戏 2021-02-04 14:55

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

    public static Vector3 ToEulerAngles(this Quaternion q)
    {
        // Store the E         


        
1条回答
  •  [愿得一人]
    2021-02-04 15:45

    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

    0 讨论(0)
提交回复
热议问题