Convert quaternion to euler rotations

Oct 24 2013 Published under Programming

Here’s a short, self contained c++ program for demonstrating conversion of quaternion rotations to euler rotations based on various rotation sequences:

// COMPILE: g++ -o quat2EulerTest quat2EulerTest.cpp 
#include <iostream>
#include <cmath> 

using namespace std;

///////////////////////////////
// Quaternion struct
// Simple incomplete quaternion struct for demo purpose
///////////////////////////////
struct Quaternion{
  Quaternion():x(0), y(0), z(0), w(1){};
  Quaternion(double x, double y, double z, double w):x(x), y(y), z(z), w(w){};
 
  void normalize(){
    double norm = std::sqrt(x*x + y*y + z*z + w*w);
    x /= norm;
    y /= norm;
    z /= norm;
    w /= norm;
  }
  
  double norm(){
    return std::sqrt(x*x + y*y + z*z + w*w);
  }

  double x;
  double y;
  double z;
  double w;  
  
};

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

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

// note: 
// return values of res[] depends on rotSeq.
// i.e.
// for rotSeq zyx, 
// x = res[0], y = res[1], z = res[2]
// for rotSeq xyz
// z = res[0], y = res[1], x = res[2]
// ...
void quaternion2Euler(const Quaternion& q, double res[], RotSeq rotSeq)
{
    switch(rotSeq){
    case 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 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 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 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 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 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 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 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 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 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 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 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:
      std::cout << "Unknown rotation sequence" << std::endl;
      break;
   }
}

///////////////////////////////
// Helper functions
///////////////////////////////
Quaternion operator*(Quaternion& q1, Quaternion& q2){
  Quaternion q;
  q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
  q.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
  q.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
  q.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
  return q;
}

ostream& operator <<(std::ostream& stream, const Quaternion& q) {
  cout << q.w << " "<< showpos << q.x << "i " << q.y << "j " << q.z << "k"; 
  cout << noshowpos;
}

double rad2deg(double rad){
  return rad*180.0/M_PI;
}

///////////////////////////////
// Main
///////////////////////////////
int main(){

  Quaternion q; // x,y,z,w
  Quaternion qx45(sin(M_PI/8), 0,0, cos(M_PI/8) );
  Quaternion qy45(0, sin(M_PI/8), 0, cos(M_PI/8));
  Quaternion qz45(0, 0, sin(M_PI/8), cos(M_PI/8));
  Quaternion qx90(sin(M_PI/4), 0,0, cos(M_PI/4) );
  Quaternion qy90(0, sin(M_PI/4), 0, cos(M_PI/4));
  Quaternion qz90(0, 0, sin(M_PI/4), cos(M_PI/4));

  double res[3];

  q = qz45*qx45;
  q.normalize();
  quaternion2Euler(q, res, zyx);
  cout << "Rotation sequence: X->Y->Z" << endl;
  cout << "x45 -> z45" << endl;
  cout << "q: " << q << endl;
  cout << "z: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " x: " << rad2deg(res[2]) << endl << endl;

  q = qz90*qx90;
  q.normalize();
  quaternion2Euler(q, res, zyx);
  cout << "Rotation sequence: X->Y->Z" << endl;
  cout << "x90 -> z90" << endl;
  cout << "q: " << q << endl;
  cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl << endl;

  q = qx90*qz90;
  q.normalize();
  quaternion2Euler(q, res, xyz);
  cout << "Rotation sequence: Z->Y->X" << endl;
  cout << "z90 -> x90" << endl;
  cout << "q: " << q << endl;
  cout << "z: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " x: " << rad2deg(res[2]) << endl;

  q = qx90*qz45;
  q.normalize();
  quaternion2Euler(q, res, xyz);
  cout << "Rotation sequence: Z->Y->X" << endl;
  cout << "z45 -> x90" << endl;
  cout << "q: " << q << endl;
  cout << "z: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " x: " << rad2deg(res[2]) << endl << endl;

  q = qx90*qz45;
  q.normalize();
  quaternion2Euler(q, res, zyx);
  cout << "Rotation sequence: X->Y->Z" << endl;
  cout << "z45 -> x90" << endl;
  cout << "q: " << q << endl;
  cout << "x: " << rad2deg(res[0]) << " y: " << rad2deg(res[1]) << " z: " << rad2deg(res[2]) << endl;
}

Comments Off

Comments are closed at this time.

Login