# NVidia Physx get yaw pitch roll from rigid body orientation

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?

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