-1
\$\begingroup\$

I have the code below that takes a set of yaw, pitch, and roll rotation angles (in degrees), and populates forward, right, and up basis vectors to express this rotated orientation as a Cartesian coordinate basis.

Now I'd like to do the opposite operation: given a forward, right, and up vector, how do I determine the yaw, pitch, and roll angles of this orientation?

Here is the code I'm using to convert from angles to vectors (run this code online):

// Example program
#include <iostream>
#include <string>
#include <math.h> 

#define PITCH 0 // up/down
#define YAW   1 // left/right
#define ROLL  2 // fall over

void AngleVectors(const float *angles, float *forward, float *right, float *up)
{
    float sr, sp, sy, cr, cp;

    float cy;
    float angle;

    angle = float(angles[YAW] * (M_PI * 2 / 360));
    sy = sin(angle);
    cy = cos(angle);

    angle = float(angles[PITCH] * (M_PI * 2 / 360));
    sp = sin(angle);
    cp = cos(angle);

    angle = float(angles[ROLL] * (M_PI * 2 / 360));
    sr = sin(angle);
    cr = cos(angle);

    if (forward)
    {
        forward[0] = cp * cy;
        forward[1] = cp * sy;
        forward[2] = -sp;
    }
    if (right)
    {
        right[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
        right[1] = (-1 * sr * sp * sy + -1 * cr * cy);
        right[2] = -1 * sr * cp;
    }
    if (up)
    {
        up[0] = (cr * sp * cy + -sr * -sy);
        up[1] = (cr * sp * sy + -sr * cy);
        up[2] = cr * cp;
    }
}

int main()
{
  float forward, right, up;
  float angles[3];
  angles[PITCH] = 0;
  angles[YAW] = 270;
  angles[ROLL] = 0;
  AngleVectors(angles,&forward,&right,&up);
  char buffer[256];
  sprintf(buffer,"%f %f %f",forward,right,up);

  std::cout << buffer;
}
\$\endgroup\$
0
\$\begingroup\$

This is related to the problem of converting from Cartesian Coordinates to Spherical Coordinates. Note that the reverse operation is not unique: there are many possible angle triplets that produce the same rotation transformation, so any function we choose will necessarily have to standardize on one option. This means an angle triplet might not necessarily round-trip convert to vectors and back as you expect, even though the result will be equivalent in effect when rotating vectors.

The details will vary a little based on the conventions you choose (eg. in what order are the angles in an angle triplet applied?) In your case it looks like your conventions are:

  • In a neutral rotation (0 degrees on all axes)...

    • forward = (1, 0 0)
    • right = (0, -1, 0)
    • up = (0, 0, 1)

      (ie. this is a right-handed coordinate system, with x+ forward, y+ left, z+ up)

  • From this neutral rotation...

    • increasing YAW rotates the forward vector to the left
    • increasing PITCH rotates the forward vector downward
    • increasing ROLL rotates the up vector to the right
  • Rotations are applied in the order (from most local to most global)

    • Roll
    • Pitch
    • Yaw

So, given two unit vectors (length = 1), we can compute the angles like so:

void AnglesFromVectors(float *angles, const float *forward, const float *up)
{
    // Yaw is the bearing of the forward vector's shadow in the xy plane.
    float yaw = atan2(forward[1], forward[0]);

    // Pitch is the altitude of the forward vector off the xy plane, toward the down direction.
    float pitch = -asin(*forward[2]);

    // 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.
    float roll = asin(up[0]*planeRightX + up[1]*planeRightY);
    // If we're twisted upside-down, return a roll in the range +-(pi/2, pi)
    if(up[z] < 0)
        roll = sign(roll) * M_PI - roll;

    // Convert radians to degrees.
    angles[YAW]   =   yaw * 180 / M_PI;
    angles[PITCH] = pitch * 180 / M_PI;
    angles[ROLL]  =  roll * 180 / M_PI;
}
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.