Simple Quaternions

raw_quat.cpp

#include <iostream>
#include <cmath>

using std::cout;

typedef struct quat_t {
    double w, x, y, z;
} quat, quaternion;

typedef struct euler_t {
    double x, y, z;
} euler;

typedef struct angle_axis_t {
    double angle, x, y, z;
} angle_axis, angleaxis;

quat FromAngleAxis(const angle_axis& a);
void ToAngleAxis(const quat& q, angle_axis& a);
quat FromEuler(const euler& e);
void ToEuler(const quat& q, euler& e);

quat Normalize(const quat& q);
quat Invert(const quat& q);
angle_axis Normalize(const angle_axis& a);

void Print(const quat& q);
quat operator*(const quat& left, const quat& right);

//////////////////////////////////////////////////////////////////////////////////////////////////////
// USE THIS FUNCTION TO CREATE QUATERNIONS FROM EULER ANGLES!
quat MakeFromAngles(double x, double y, double z) {
    return FromAngleAxis({y, 0, 1, 0})  * FromAngleAxis({x, 1, 0, 0})  * FromAngleAxis({z, 0, 0, 1});
}
//////////////////////////////////////////////////////////////////////////////////////////////////////


quat FromAngleAxis(const angle_axis& a) {
    angle_axis aa = Normalize(a);
    aa.angle *= 0.0174532925;
    
    double s = sin(aa.angle / 2.0);
    quat q;
    
    q.x = aa.x * s;
    q.y = aa.y * s;
    q.z = aa.z * s;
    q.w = cos(aa.angle / 2.0);
    
    return q;
}

void ToAngleAxis(const quat& q, angle_axis& a) {
    quat q1 = Normalize(q);
    a.angle = 2.0 * acos(q1.w);
    
    double s = sqrt(1.0-q1.w*q1.w);
    if (s < 0.001) { 
        a.x = q1.x;
        a.y = q1.y;
        a.z = q1.z;
    } else {
        a.x = q1.x / s;
        a.y = q1.y / s;
        a.z = q1.z / s;
    }
    
    a.angle *= 57.2957795;
}

void Print(const quat& q) {
    euler e;
    ToEuler(*const_cast<quat*>(&q), e);
    std::cout << "Quaternion: " << q.w << ", " << q.x << ", " << q.y << ", " << q.z << "\n";
    std::cout << "\tEulers: " << e.x << ", " << e.y << ", " << e.z << "\n\n";
}

quat Invert(const quat& q) {
    quat q1 = { q.w, -q.x, -q.y, -q.z };
    return q1;
}

quat Normalize(const quat& q) {
    double mag = sqrt(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
    quat q2 = { q.w / mag, q.x / mag, q.y / mag, q.z / mag };
    return q2;
}

angle_axis Normalize(const angle_axis& a) {
    double mag = sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
    angle_axis aa = { a.angle, a.x / mag, a.y / mag, a.z / mag };
    return aa;
}

void ToEuler(const quat& q, euler& e) {
    quat q1 = Normalize(q);
    
    double heading;     // Y
    double attitude;    // Z
    double bank;        // X
    
    double test = q1.x*q1.y + q1.z*q1.w;
    
    if (test > 0.499) { // singularity at north pole
  	heading = 2.0 * atan2(q1.x,q1.w);
		attitude = M_PI / 2.0;
		bank = 0.0;
		return;
	}
	if (test < -0.499) { // singularity at south pole
		heading = -2.0 * atan2(q1.x,q1.w);
		attitude = -M_PI / 2.0;
		bank = 0;
		return;
	}
    
  double sqx = q1.x*q1.x;
  double sqy = q1.y*q1.y;
  double sqz = q1.z*q1.z;
  heading = atan2(2.0 * q1.y * q1.w - 2.0 * q1.x * q1.z , 1.0 - 2.0 * sqy - 2.0 * sqz);
	attitude = asin(2.0 * test);
	bank = atan2(2.0 * q1.x * q1.w - 2.0 * q1.y * q1.z , 1.0 - 2.0 * sqx - 2.0 * sqz);
    
    
  e.x = bank * 57.2957795;
  e.y = heading * 57.2957795;
  e.z = attitude * 57.2957795;
}

quat FromEuler(const euler& e) {   
    double heading  = e.y * 0.0174532925;
    double attitude = e.z * 0.0174532925;
    double bank = e.x * 0.0174532925;
    
    double c1 = cos(heading/2.0);
    double s1 = sin(heading/2.0);
    double c2 = cos(attitude/2.0);
    double s2 = sin(attitude/2.0);
    double c3 = cos(bank/2.0);
    double s3 = sin(bank/2.0);
    double c1c2 = c1*c2;
    double s1s2 = s1*s2;
    
    quat q;
    q.w =c1c2*c3 - s1s2*s3;
    q.x =c1c2*s3 + s1s2*c3;
	  q.y =s1*c2*c3 + c1*s2*s3;
	  q.z =c1*s2*c3 - s1*c2*s3;
   
    return q;
}

quat operator*(const quat& left, const quat& right) {
    quat q;
    
    q.w = left.w * right.w - left.x * right.x - left.y * right.y - left.z * right.z;
	  q.x = left.w * right.x + left.x * right.w + left.y * right.z - left.z * right.y;
	  q.y = left.w * right.y - left.x * right.z + left.y * right.w + left.z * right.x;
	  q.z = left.w * right.z + left.x * right.y - left.y * right.x + left.z * right.w;
      
    return q;
}