0
\$\begingroup\$

I am using NVidia PhysX in the game that I'm developing and I stumbled upon a math problem. I need to retrieve Yaw, Pitch and Roll from the orientation quaternion stored in the rigid body of some object.

The game is developed with OpenGL, using a right hand coordinate system (x right, y up, z back).

I tried many examples online but none of them gave a correct result. One of them is from Wikipedia, which I list below :

#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
         angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
         angles.pitch = std::asin(sinp);

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

As my quaternion skills are not that good, I was wondering if someone can provide some sample as to what has to be done to get the yaw, pitch and roll from the orientation quaternion?

\$\endgroup\$
0

1 Answer 1

1
\$\begingroup\$

The solution was found here

And here is the code :

///////////////////////////////
// Quaternion to Euler
///////////////////////////////
enum class RotSeq { zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy, xzx };

void twoaxisrot(float r11, float r12, float r21, float r31, float r32, float res[]) {
    res[0] = atan2(r11, r12);
    res[1] = acos(r21);
    res[2] = atan2(r31, r32);
}

void threeaxisrot(float r11, float r12, float r21, float r31, float r32, float res[]) {
    res[0] = atan2(r31, r32);
    res[1] = asin(r21);
    res[2] = atan2(r11, r12);
}

void quaternion2Euler(const physx::PxQuat& q, float res[], RotSeq rotSeq)
{
    switch (rotSeq) {
    case RotSeq::zyx:
        threeaxisrot(2 * (q.x * q.y + q.w * q.z),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            -2 * (q.x * q.z - q.w * q.y),
            2 * (q.y * q.z + q.w * q.x),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            res);
        break;

    case RotSeq::zyz:
        twoaxisrot(2 * (q.y * q.z - q.w * q.x),
            2 * (q.x * q.z + q.w * q.y),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            2 * (q.y * q.z + q.w * q.x),
            -2 * (q.x * q.z - q.w * q.y),
            res);
        break;

    case RotSeq::zxy:
        threeaxisrot(-2 * (q.x * q.y - q.w * q.z),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            2 * (q.y * q.z + q.w * q.x),
            -2 * (q.x * q.z - q.w * q.y),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            res);
        break;

    case RotSeq::zxz:
        twoaxisrot(2 * (q.x * q.z + q.w * q.y),
            -2 * (q.y * q.z - q.w * q.x),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            2 * (q.x * q.z - q.w * q.y),
            2 * (q.y * q.z + q.w * q.x),
            res);
        break;

    case RotSeq::yxz:
        threeaxisrot(2 * (q.x * q.z + q.w * q.y),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            -2 * (q.y * q.z - q.w * q.x),
            2 * (q.x * q.y + q.w * q.z),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            res);
        break;

    case RotSeq::yxy:
        twoaxisrot(2 * (q.x * q.y - q.w * q.z),
            2 * (q.y * q.z + q.w * q.x),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            2 * (q.x * q.y + q.w * q.z),
            -2 * (q.y * q.z - q.w * q.x),
            res);
        break;

    case RotSeq::yzx:
        threeaxisrot(-2 * (q.x * q.z - q.w * q.y),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            2 * (q.x * q.y + q.w * q.z),
            -2 * (q.y * q.z - q.w * q.x),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            res);
        break;

    case RotSeq::yzy:
        twoaxisrot(2 * (q.y * q.z + q.w * q.x),
            -2 * (q.x * q.y - q.w * q.z),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            2 * (q.y * q.z - q.w * q.x),
            2 * (q.x * q.y + q.w * q.z),
            res);
        break;

    case RotSeq::xyz:
        threeaxisrot(-2 * (q.y * q.z - q.w * q.x),
            q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z,
            2 * (q.x * q.z + q.w * q.y),
            -2 * (q.x * q.y - q.w * q.z),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            res);
        break;

    case RotSeq::xyx:
        twoaxisrot(2 * (q.x * q.y + q.w * q.z),
            -2 * (q.x * q.z - q.w * q.y),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            2 * (q.x * q.y - q.w * q.z),
            2 * (q.x * q.z + q.w * q.y),
            res);
        break;

    case RotSeq::xzy:
        threeaxisrot(2 * (q.y * q.z + q.w * q.x),
            q.w * q.w - q.x * q.x + q.y * q.y - q.z * q.z,
            -2 * (q.x * q.y - q.w * q.z),
            2 * (q.x * q.z + q.w * q.y),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            res);
        break;

    case RotSeq::xzx:
        twoaxisrot(2 * (q.x * q.z - q.w * q.y),
            2 * (q.x * q.y + q.w * q.z),
            q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z,
            2 * (q.x * q.z + q.w * q.y),
            -2 * (q.x * q.y - q.w * q.z),
            res);
        break;
    default:
        break;
    }
}

solve for ZYX

\$\endgroup\$

You must log in to answer this question.

Not the answer you're looking for? Browse other questions tagged .