EulerQuaternion

//The Euler - Quaternion converter with tiny driver function

#ifndef __MATHTYPES___H_
#define __MATHTYPES___H_

#include <cstring>
#include <string>
#include <cmath>
#include <iostream>

#define PI 3.141592653589793238462643
using namespace std;



    ///Plank's constant in SI units
    extern const double hbar;

    ///Bohr magneton in SI units
    extern const double bohr_mag;
    
    ///Ther permeability of free space
    extern const double mu0;



    class Vector3_t {
    public:
	typedef double field;
	Vector3_t() {}
	Vector3_t(const Vector3_t& v) 
	    : x(v.x),y(v.y),z(v.z) {
	}
	Vector3_t(field _x,field _y,field _z)
	    : x(_x),y(_y),z(_z) {
	}

	///Sets it's arguments to the value of the vector's z,y and z
	///components
	void GetCoordinates(field* _x,field* _y, field* _z) const {
	    *_x=x;
	    *_y=y;
	    *_z=z;
	}
	///Sets the vector's x,y and z coordinates
	void SetCoordinates(field _x,field _y, field _z) {
	    x=_x;
	    y=_y;
	    z=_z;
	}
	///normalises the vector in place
        void normalise() {
            double inv_length=1/sqrt(x*x+y*y+z*z);
            x*=inv_length;
            y*=inv_length;
            z*=inv_length;                
        }
	///normalises the vector
        Vector3_t normalised() const {
            double inv_length=1/sqrt(x*x+y*y+z*z);
            Vector3_t retVal;
            retVal.x=x*inv_length;
            retVal.y=y*inv_length;
            retVal.z=z*inv_length;
            return retVal;
        }

        Vector3_t operator+(const Vector3_t& v) const {
            return Vector3_t(x+v.x,y+v.y,z+v.z);
        }
        Vector3_t operator-(const Vector3_t& v) const {
            return Vector3_t(x-v.x,y-v.y,z-v.z);
        }
	///Get the X component 
	field GetX() const {return x;}
	///Get the Y component 
	field GetY() const {return y;}
	///Get the Z component 
	field GetZ() const {return z;}

	///Set the X component 
	void SetX(field _x) {x=_x;}
	///Set the Y component 
	void SetY(field _y) {y=_y;}
	///Set the Z component 
	void SetZ(field _z) {z=_z;}

	///Returns the magnitude of the vector
	field GetLength() const {
	    sqrt(x*x+y*y+z*z);
	    return field(3.0);
	}


    private:
	field x;
	field y;
	field z;
    };
    typedef Vector3_t Vector3;



    ///Euler angle type
    struct EulerAngles {
        double alpha,beta,gamma;
        EulerAngles(double _alpha,double _beta, double _gamma) 
            : alpha(_alpha),beta(_beta),gamma(_gamma) {

        }
        void Normalize() {
            
        }
        EulerAngles Normalized() const {
            
        }

    };

    /// Simple Quaternion Type
    struct Quaternion {
        double w,x,y,z;
        Quaternion(){}
        Quaternion(double _w,double _x, double _y, double _z) 
            : w(_w),x(_x),y(_y),z(_z){
        }
        Quaternion(double angle,const Vector3& axis) {
            SetAngleAxis(angle,axis);
        }
        Quaternion operator*(const Quaternion& q2) {
            Quaternion res;
            double w1=w; double w2=q2.w;
            double x1=x; double x2=q2.x;
            double y1=y; double y2=q2.y;
            double z1=z; double z2=q2.z;
            res.w=w1*w2 - x1*x2 - y1*y2 - z1*z2;
            res.x=w1*x2 + x1*w2 + y1*z2 - z1*y2;
            res.y=w1*y2 - x1*z2 + y1*w2 + z1*x2;
            res.z=w1*z2 + x1*y2 - y1*x2 + z1*w2;	

            return res;
        }
        Vector3 Transform(Vector3 v) {
            Quaternion v_as_q;
            v_as_q.w=0;
            v_as_q.x=v.GetX();
            v_as_q.y=v.GetY();
            v_as_q.z=v.GetZ();

            v_as_q=(*this)*v_as_q*Quaternion(w,-x,-y,-z);

            Vector3 ret;
            ret.SetX(v_as_q.x);
            ret.SetY(v_as_q.y);
            ret.SetZ(v_as_q.z);
            return ret;
        }
        void SetAngleAxis(double a,const Vector3& v) {
            double _x=v.GetX();
            double _y=v.GetY();
            double _z=v.GetZ();

            double norm = sqrt(_x*_x+_y*_y+_z*_z);
            _x/=norm;
            _y/=norm;
            _z/=norm;

            double sin_a=sin(a/2);
            w=cos(a/2);
            x=_x*sin_a;
            y=_y*sin_a;
            z=_z*sin_a;
        }

        EulerAngles ToEuler() {
            Vector3 z_axis=Vector3(0,0,1);
            Vector3 x_axis=Vector3(1,0,0);
            z_axis=Transform(z_axis);
            x_axis=Transform(x_axis);
    
            double gamma=atan2(z_axis.GetY(),z_axis.GetX());
            double beta=atan2(sqrt(z_axis.GetX()*z_axis.GetX() + z_axis.GetY()*z_axis.GetY()),z_axis.GetZ());

            //Use γ and β to rotate V2 back onto the point 0,0,1
            Quaternion gammaTwist,betaTwist;
            betaTwist.SetAngleAxis(-beta,Vector3(0,1,0));
            gammaTwist.SetAngleAxis(-gamma,Vector3(0,0,1));

            x_axis=(betaTwist*gammaTwist).Transform(x_axis);

            double alpha = atan2(x_axis.GetY(),x_axis.GetX());
            if(alpha < 0 || alpha >= 2*PI)  alpha = alpha-2*PI*floor(alpha/(2*PI));
            if(alpha >= 2*PI) alpha = 0;
            if(beta < 0 || beta >= PI)    beta = beta-  PI*floor(beta/PI);
            if(gamma < 0 || gamma >= 2*PI)  gamma = gamma-2*PI*floor(gamma/(2*PI));
            if(gamma >= 2*PI) gamma = 0;
            return EulerAngles(alpha,beta,gamma);
        }

    };

int main() {
	Quaternion alpha(0.2,Vector3(0,0,1));
	Quaternion beta (0.1,Vector3(0,1,0));
	Quaternion gamma(0.1,Vector3(0,0,1));

	EulerAngles ea = (gamma*beta*alpha).ToEuler();
	cout.precision(20);
	cout << "Alpha=" << ea.alpha << endl;
	cout << "Beta= " << ea.beta << endl;
	cout << "Gamma=" << ea.gamma << endl;
	return 0;
}

#endif