Calculate Yaw, Pitch, Roll from Up, Right, Forward

Question

I have a 3D object with a forward, right, and up vector.

I'm trying to calculate its Yaw, Pitch and Roll.

My last attempt was:

void GetYawPitchRoll(OvMaths::FVector3 forward, OvMaths::FVector3 up, float& yaw, float& pitch, float& roll)
{
    // Yaw is the bearing of the forward vector's shadow in the xy plane.
    yaw = atan2(forward.y, forward.x);

    // Pitch is the altitude of the forward vector off the xy plane, toward the down direction.
    pitch = -asin(forward.z);

    // Find the vector in the xy plane 90 degrees to the right of our bearing.
    float planeRightX = sin(yaw);
    float planeRightY = -cos(yaw);

    // Roll is the rightward lean of our up vector, computed here using a dot product.
    roll = asin(up.x * planeRightX + up.y * planeRightY);
    // If we're twisted upside-down, return a roll in the range +-(pi/2, pi)
    if (up.z < 0)
        roll = (0.0f < roll) - (roll < 0.0f) * 3.14f - roll;

    // Convert radians to degrees.
    yaw     *= 180 / 3.14f;
    pitch   *= 180 / 3.14f;
    roll    *= 180 / 3.14f;
}

Howver, return values aren't correct.

Any idea how to improve this code or another approach?

Answer 1

It looks like you missed some parentheses here:

        roll = (0.0f < roll) - (roll < 0.0f) * 3.14f - roll;

In the case where (0.0f < roll) is true, it adds $1$ to the result but does not get multiplied by $3.14.$

This line of code also does not correctly handle the case where you are rolled exactly $180$ degrees (so that roll is zero before this line executes). Then you neither add nor subtract $3.14$ and you end up with roll still equal to zero.

This seems to fix it:

        roll = ((0.0f <= roll) - (roll < 0.0f)) * 3.14f - roll;

One other thing you might want to change is the approximation for $\pi$, which could be a lot more accurate.

I tried this with a forward vector with some pitch and yaw and five different "up" vectors representing roll angles in all four quadrants plus a $180$-degree roll, and the function seems to come up with reasonable answers (considering the accuracy of the approximation of $\pi$).

Other things: the way you compute yaw, pitch, and roll, if your $z$ axis points up in the world frame then your roll is what we would usually call rightward but the yaw is leftward and the pitch is downward. I don't know if that was intentional, or perhaps your $z$ axis points downward.