Quaternion to Euler angles conversion regarding rotation sequence

Question

This is what I have:

• I have a Vector3D class that represents a vector or a point in 3D space.
• I have a Quaternion class that performs calculus on quaternions, and I can create a rotation unit quaternion from the Angle-Axis representation using a static method Quaternion::fromAngleAxisRotation(double angle, Vector3D axis).
• I have a EulerAngles class with alpha, beta, gamma angles, and in constructor I must specify the rotation sequence, that can be one of canonical 12 (XZX, YXZ, ZYX ecc). As private member I have an array of three elements Vector3D that specifies rotation sequence axis. I.e. for ZYX rotation I have axes[0] = Vector3D(1,0,0); axes[1] = Vector3D(0,1,0); axes[2] = Vector3D(0,0,1);. For XZX rotation sequence I have axes[0] = Vector3D(1,0,0); axes[1] = Vector3D(0,0,1); axes[2] = Vector3D(1,0,0); and so on.

• The class EulerAngles has a method getQuaternion(). This allows to calculate the corresponding quaternion having three rotation angles and the rotation sequence. In order to work, the method creates three ordered Quaternion instances using the Quaternion::fromAngleAxisRotation static method to find quaternion for single rotation angles and then it multiplies them. This allows me to have a single method that calculates rotation unit quaternion in all twelve rotation sequences.

  Quaternion qa = Quaternion::fromAngleAxisRotation(alpha, axes[0]); Quaternion qb = Quaternion::fromAngleAxisRotation(beta, axes[1]); Quaternion qg = Quaternion::fromAngleAxisRotation(gamma, axes[2]); return qa * qb * qg;

And this is what I want:

I have a Quaternion instance. I want to create in EulerAngles class a method EulerAngles::setFromQuaternion(Quaternion q) that allows me to take the quaternion (normalized etc.) and create three angles from it, taking into account the same rotation sequence stored in axes array.

Using some math I can take into account every single rotation sequence combination and threat them separately in order to find angles. But I'd like to create a unique method as I do in EulerAngles::getQuaternion().

Is there a way to decompose a unit quaternion into angles giving rotation sequence as input, or I must create twelve different methods in order to find them?

Vector3D.h

#ifndef GEOMETRY_VECTOR3D_H_
#define GEOMETRY_VECTOR3D_H_

#include <vector>
#include <ostream>

namespace Geometry {

class Vector3D {
public:

    static const Vector3D Zero;
    static const Vector3D One;
    static const Vector3D UnitX;
    static const Vector3D UnitY;
    static const Vector3D UnitZ;

public:
    Vector3D();
    Vector3D(const double &x, const double &y, const double &z);
    Vector3D(const Vector3D &point);
    virtual ~Vector3D();

    void setX(const double &x);
    void setY(const double &y);
    void setZ(const double &z);
    void set(const double &x, const double &y, const double &z);
    double getX() const;
    double getY() const;
    double getZ() const;
    void get(double *x, double *y, double *z) const;
    void normalize();
    bool isZero() const;
    double getDistanceTo(const Vector3D &point) const;
    double getModule() const;
    double getSquaredModule() const;

    static Vector3D getNormal(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3);

    Vector3D& operator+=(const Vector3D &point);
    Vector3D& operator-=(const Vector3D &point);
    Vector3D& operator*=(const double &value);
    const Vector3D operator+(const Vector3D &point) const;
    const Vector3D operator-(const Vector3D &point) const;
    const Vector3D operator*(const double &value) const;

private:

    double m_x;
    double m_y;
    double m_z;
};

std::ostream& operator<<(std::ostream &os, const Geometry::Vector3D &point);

/** A useful typedef for a vector of points. */
typedef std::vector<Vector3D> Vector3DVector;

} // namespace Geometry

#endif // !GEOMETRY_VECTOR3D_H_

Vector3D.cpp

#include "Geometry/Vector3D.h"
#include <cmath>

namespace Geometry {

const Vector3D Vector3D::Zero  = Vector3D(0.0, 0.0, 0.0);
const Vector3D Vector3D::One   = Vector3D(1.0, 1.0, 1.0);
const Vector3D Vector3D::UnitX = Vector3D(1.0, 0.0, 0.0);
const Vector3D Vector3D::UnitY = Vector3D(0.0, 1.0, 0.0);
const Vector3D Vector3D::UnitZ = Vector3D(0.0, 0.0, 1.0);

Vector3D::Vector3D() {
    m_x = 0.0;
    m_y = 0.0;
    m_z = 0.0;
}

Vector3D::Vector3D(const double &x, const double &y, const double &z) {
    set(x, y, z);
}

Vector3D::Vector3D(const Vector3D &point) {
    m_x = point.m_x;
    m_y = point.m_y;
    m_z = point.m_z;
}

Vector3D::~Vector3D() {

}

void Vector3D::setX(const double &x) {
    m_x = x;
}

void Vector3D::setY(const double &y) {
    m_y = y;
}

void Vector3D::setZ(const double &z) {
    m_z = z;
}

void Vector3D::set(const double &x, const double &y, const double &z) {
    m_x = x;
    m_y = y;
    m_z = z;
}

double Vector3D::getX() const {
    return m_x;
}

double Vector3D::getY() const {
    return m_y;
}

double Vector3D::getZ() const {
    return m_z;
}

void Vector3D::get(double *x, double *y, double *z) const {
    *x = m_x;
    *y = m_y;
    *z = m_z;
}

void Vector3D::normalize() {
    const double r = sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
    if (r == 0) return;
    m_x /= r;
    m_y /= r;
    m_z /= r;
}

bool Vector3D::isZero() const {
    if (m_x == 0.0 && m_y == 0.0 && m_z == 0.0) return true;
    return false;
}

double Vector3D::getDistanceTo(const Vector3D &point) const {
    const double dx = m_x - point.m_x;
    const double dy = m_y - point.m_y;
    const double dz = m_z - point.m_z;
    const double r  = sqrt(dx * dx + dy * dy + dz * dz);
    return r;
}

double Vector3D::getModule() const {
    return sqrt(getSquaredModule());
}

double Vector3D::getSquaredModule() const {
    return m_x * m_x + m_y * m_y + m_z * m_z;
}

Vector3D& Vector3D::operator+=(const Vector3D &point) {
    m_x += point.m_x;
    m_y += point.m_y;
    m_z += point.m_z;
    return *this;
}

Vector3D& Vector3D::operator-=(const Vector3D &point) {
    m_x -= point.m_x;
    m_y -= point.m_y;
    m_z -= point.m_z;
    return *this;
}

Vector3D& Vector3D::operator*=(const double &value) {
    m_x *= value;
    m_y *= value;
    m_z *= value;
    return *this;
}

const Vector3D Vector3D::operator+(const Vector3D &point) const {
    return Vector3D(*this) += point;
}

const Vector3D Vector3D::operator-(const Vector3D &point) const {
    return Vector3D(*this) -= point;
}

const Vector3D Vector3D::operator*(const double &value) const {
    return Vector3D(*this) *= value;
}

Vector3D Vector3D::getNormal(const Vector3D &p1, const Vector3D &p2, const Vector3D &p3) {
    return Vector3D(
            (p2.getY() - p1.getY()) * (p3.getZ() - p1.getZ()) - (p3.getY() - p1.getY()) * (p2.getZ() - p1.getZ()),
            (p2.getZ() - p1.getZ()) * (p3.getX() - p1.getX()) - (p3.getZ() - p1.getZ()) * (p2.getX() - p1.getX()),
            (p2.getX() - p1.getX()) * (p3.getY() - p1.getY()) - (p3.getX() - p1.getX()) * (p2.getY() - p1.getY())
            );
}

std::ostream& operator<<(std::ostream &os, const Vector3D &point) {
    os << "(" << point.getX() << ", " << point.getY() << ", " << point.getZ() << ")";
    return os;
}

} // namespace Geometry

Quaternion.h

#ifndef GEOMETRY_QUATERION_H_
#define GEOMETRY_QUATERION_H_

#include "Geometry/Vector3D.h"

namespace Geometry {

class Quaternion {
public:

    static Quaternion fromAngleAxisRotation(const double &angle, const double &x, const double &y, const double &z);
    static Quaternion fromAngleAxisRotation(const double &angle, const Vector3D &p);
    static Quaternion slerp(const Quaternion &q1, const Quaternion &q2, const double &t, const bool &normalize = true);

public:

    Quaternion();
    explicit Quaternion(const double &q0);
    Quaternion(const double &q0, const double &q1, const double &q2, const double &q3);
    Quaternion(const Quaternion &rhs);
    virtual ~Quaternion();
    void setQ0(const double &q0);
    void setQ1(const double &q1);
    void setQ2(const double &q2);
    void setQ3(const double &q3);
    void setQ(const size_t &position, const double &q);
    void set(const double &q0, const double &q1, const double &q2, const double &q3);
    double getQ0() const;
    double getQ1() const;
    double getQ2() const;
    double getQ3() const;
    double getQ(const size_t &position) const;
    Quaternion getReal() const;
    Quaternion getUnreal() const;
    Quaternion getConjugate() const;
    Quaternion getInverse() const;
    Quaternion getNormalized() const;
    double dot(const Quaternion &rhs) const;
    double getAbs() const;
    double getNorm() const;
    void conjugate();
    void invert();
    void normalize();

public:

    Quaternion& operator  = (const Quaternion           &rhs);
    Quaternion& operator  = (const double               &rhs);
    Quaternion& operator += (const Quaternion           &rhs);
    Quaternion& operator += (const double               &rhs);
    Quaternion& operator -= (const Quaternion           &rhs);
    Quaternion& operator -= (const double               &rhs);
    Quaternion& operator *= (const Quaternion           &rhs);
    Quaternion& operator *= (const double               &rhs);
    Quaternion& operator /= (const Quaternion           &rhs);
    Quaternion& operator /= (const double               &rhs);

private:

    void checkIndex(const size_t &index) const;

private:

    double q[4];

};

} // namespace Geometry

Geometry::Quaternion operator + (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator + (const Geometry::Quaternion &lhs, const double                    &rhs);
Geometry::Quaternion operator + (const double                    &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator - (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator - (const Geometry::Quaternion &lhs, const double                    &rhs);
Geometry::Quaternion operator - (const double                    &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator * (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator * (const Geometry::Quaternion &lhs, const double                    &rhs);
Geometry::Quaternion operator * (const double                    &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator / (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs);
Geometry::Quaternion operator / (const Geometry::Quaternion &lhs, const double                    &rhs);
Geometry::Quaternion operator / (const double                    &lhs, const Geometry::Quaternion &rhs);


#endif // !GEOMETRY_QUATERION_H_

Quaternion.cpp

#include "Geometry/Quaternion.h"
#include <stdexcept>
#include <sstream>
#include <cmath>

namespace Geometry {

using std::out_of_range;
using std::underflow_error;
using std::stringstream;

Quaternion Quaternion::fromAngleAxisRotation(const double &angle, const double &x, const double &y, const double &z) {
    return fromAngleAxisRotation(angle, Vector3D(x, y, z));
}

/**
 * A central rotation in space can be defined as a rotation of specifid amount
 * @f$\theta@f$ and a rotation axis, defined as vector @f$\left(x, y, z
 * \right)@f$. This method allows to build the quaternion relative to that
 * rotation.
 *
 * @param[in] angle Rotation angle (radians).
 * @param[in] p     Rotation axis.
 *
 * @return Quaternion related to this rotation.
 *
 * @throw std::underflow_error if vector is null.
 */
Quaternion Quaternion::fromAngleAxisRotation(const double &angle, const Vector3D &p) {
    const double halfAngleSin = sin(angle * 0.5);
    Vector3D np = p;
    np.normalize();
    return Quaternion(cos(angle * 0.5), np.getX() * halfAngleSin, np.getY() * halfAngleSin, np.getZ() * halfAngleSin);
}

Quaternion Quaternion::slerp(const Quaternion &q1, const Quaternion &q2, const double &t, const bool &normalize) {
    const double dotProduct = q1.dot(q2);
    const double ht = t * 0.5;
    double       theta = acos(dotProduct);
    if (theta < 0.0) theta = -theta;
    const double st = 1.0 / sin(theta);
    const double sut = sin(ht * theta);
    const double sout = sin((1.0 - ht) * theta);
    const double w1 = sout * st;
    const double w2 = sut * st;
    Quaternion res = ((w1 * q1) + (w2 * q2));
    if (true == normalize) {
        res.normalize();
    }
    return res;
}

Quaternion::Quaternion() {
    q[0] = 0.0;
    q[1] = 0.0;
    q[2] = 0.0;
    q[3] = 0.0;
}

Quaternion::Quaternion(const double &q0) {
    q[0] = q0;
    q[1] = 0.0;
    q[2] = 0.0;
    q[3] = 0.0;
}

Quaternion::Quaternion(const double &q0, const double &q1, const double &q2, const double &q3) {
    q[0] = q0;
    q[1] = q1;
    q[2] = q2;
    q[3] = q3;
}

Quaternion::Quaternion(const Quaternion &rhs) {
    q[0] = rhs.q[0];
    q[1] = rhs.q[1];
    q[2] = rhs.q[2];
    q[3] = rhs.q[3];
}

Quaternion::~Quaternion() {

}

void Quaternion::setQ0(const double &q0) {
    q[0] = q0;
}

void Quaternion::setQ1(const double &q1) {
    q[1] = q1;
}

void Quaternion::setQ2(const double &q2) {
    q[2] = q2;
}

void Quaternion::setQ3(const double &q3) {
    q[3] = q3;
}

void Quaternion::setQ(const size_t &index, const double &q) {
    checkIndex(index);
    this->q[index] = q;
}

void Quaternion::set(const double &q0, const double &q1, const double &q2, const double &q3) {
    q[0] = q0;
    q[1] = q1;
    q[2] = q2;
    q[3] = q3;
}

double Quaternion::getQ0() const {
    return q[0];
}

double Quaternion::getQ1() const {
    return q[1];
}

double Quaternion::getQ2() const {
    return q[2];
}

double Quaternion::getQ3() const {
    return q[3];
}

double Quaternion::getQ(const size_t &index) const {
    checkIndex(index);
    return q[index];
}

Quaternion Quaternion::getReal() const {
    return Quaternion(q[0], 0.0, 0.0, 0.0);
}

Quaternion Quaternion::getUnreal() const {
    return Quaternion(0.0, q[1], q[2], q[3]);
}

Quaternion Quaternion::getConjugate() const {
    return Quaternion(q[0], -q[1], -q[2], -q[3]);
}

Quaternion Quaternion::getInverse() const {
    Quaternion quat(*this);
    quat.invert();
    return quat;
}

Quaternion Quaternion::getNormalized() const {
    Quaternion quat(*this);
    quat.normalize();
    return quat;
}

double Quaternion::dot(const Quaternion &rhs) const {
    return q[0] * rhs.q[0] + q[1] * rhs.q[1] + q[2] * rhs.q[2] + q[3] * rhs.q[3];
}

double Quaternion::getAbs() const {
    return sqrt(getNorm());
}

double Quaternion::getNorm() const {
    return q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
}

void Quaternion::conjugate() {
    q[1] = -q[1];
    q[2] = -q[2];
    q[3] = -q[3];
}

void Quaternion::invert() {
    const double denominator = getNorm();
    if (denominator == 0.0) throw underflow_error("QUATERNION_ZERO_DENOMINATOR");
    const double invDen = 1.0 / denominator;
    q[0] *= invDen;
    q[1] *= invDen;
    q[2] *= invDen;
    q[3] *= invDen;;
    q[1]  = -q[1];
    q[2]  = -q[2];
    q[3]  = -q[3];
}

void Quaternion::normalize() {
    const double denominator = getAbs();
    if (denominator == 0.0) throw underflow_error("QUATERNION_ZERO_DENOMINATOR");
    const double invDen = 1.0 / denominator;
    q[0] *= invDen;
    q[1] *= invDen;
    q[2] *= invDen;
    q[3] *= invDen;
}

Quaternion& Quaternion::operator = (const Quaternion &rhs) {
    if (this != &rhs) {
        q[0] = rhs.q[0];
        q[1] = rhs.q[1];
        q[2] = rhs.q[2];
        q[3] = rhs.q[3];
    }
    return *this;
}

Quaternion& Quaternion::operator = (const double &rhs) {
    q[0] = rhs;
    q[1] = 0.0;
    q[2] = 0.0;
    q[3] = 0.0;
    return *this;
}

Quaternion& Quaternion::operator += (const Quaternion &rhs) {
    q[0] += rhs.q[0];
    q[1] += rhs.q[1];
    q[2] += rhs.q[2];
    q[3] += rhs.q[3];
    return *this;
}

Quaternion& Quaternion::operator += (const double &rhs) {
    q[0] += rhs;
    return *this;
}

Quaternion& Quaternion::operator -= (const Quaternion &rhs) {
    q[0] -= rhs.q[0];
    q[1] -= rhs.q[1];
    q[2] -= rhs.q[2];
    q[3] -= rhs.q[3];
    return *this;
}

Quaternion& Quaternion::operator -= (const double &rhs) {
    q[0] -= rhs;
    return *this;
}

Quaternion& Quaternion::operator *= (const Quaternion &rhs) {
    const double q0 = q[0] * rhs.q[0] - q[1] * rhs.q[1] - q[2] * rhs.q[2] - q[3] * rhs.q[3];
    const double q1 = q[1] * rhs.q[0] + q[0] * rhs.q[1] - q[3] * rhs.q[2] + q[2] * rhs.q[3];
    const double q2 = q[2] * rhs.q[0] + q[3] * rhs.q[1] + q[0] * rhs.q[2] - q[1] * rhs.q[3];
    const double q3 = q[3] * rhs.q[0] - q[2] * rhs.q[1] + q[1] * rhs.q[2] + q[0] * rhs.q[3];
    set(q0, q1, q2, q3);
    return *this;
}

Quaternion& Quaternion::operator *= (const double &rhs) {
    q[0] *= rhs;
    q[1] *= rhs;
    q[2] *= rhs;
    q[3] *= rhs;
    return *this;
}

Quaternion& Quaternion::operator /= (const Quaternion &rhs) {
    const double denominator = rhs.getNorm();
    if (denominator == 0.0) throw underflow_error("QUATERNION_ZERO_DENOMINATOR");
    const double invDen = 1.0 / denominator;
    const double q0 = (q[0] * rhs.q[0] + q[1] * rhs.q[1] + q[2] * rhs.q[2] + q[3] * rhs.q[3]) * invDen;
    const double q1 = (q[1] * rhs.q[0] - q[0] * rhs.q[1] - q[3] * rhs.q[2] + q[2] * rhs.q[3]) * invDen;
    const double q2 = (q[2] * rhs.q[0] + q[3] * rhs.q[1] - q[0] * rhs.q[2] - q[1] * rhs.q[3]) * invDen;
    const double q3 = (q[3] * rhs.q[0] - q[2] * rhs.q[1] + q[1] * rhs.q[2] - q[0] * rhs.q[3]) * invDen;
    set(q0, q1, q2, q3);
    return *this;
}

Quaternion& Quaternion::operator /= (const double &rhs) {
    if (rhs == 0.0) throw underflow_error("QUATERNION_ZERO_DENOMINATOR");
    const double invRhs = 1.0 / rhs;
    q[0] *= invRhs;
    q[1] *= invRhs;
    q[2] *= invRhs;
    q[3] *= invRhs;
    return *this;
}

void Quaternion::checkIndex(const size_t &index) const {
    if (index > 3) {
        stringstream ss;
        ss << QUATERNION_OUT_OF_RANGE << " " << index;
        throw out_of_range(ss.str());
    }
}

} // namespace Geometry

Geometry::Quaternion operator + (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs) {
    Geometry::Quaternion res(lhs);
    res += rhs;
    return res;
}

Geometry::Quaternion operator + (const Geometry::Quaternion &lhs, const double &rhs) {
    Geometry::Quaternion res(lhs);
    res += rhs;
    return res;
}

Geometry::Quaternion operator + (const double &lhs, const Geometry::Quaternion &rhs) {
    return rhs + lhs;
}

Geometry::Quaternion operator - (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs) {
    Geometry::Quaternion res(lhs);
    res -= rhs;
    return res;
}

Geometry::Quaternion operator - (const Geometry::Quaternion &lhs, const double &rhs) {
    Geometry::Quaternion res(lhs);
    res -= rhs;
    return res;
}

Geometry::Quaternion operator - (const double &lhs, const Geometry::Quaternion &rhs) {
    return rhs - lhs;
}

Geometry::Quaternion operator * (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs) {
    Geometry::Quaternion res(lhs);
    res *= rhs;
    return res;
}

Geometry::Quaternion operator * (const Geometry::Quaternion &lhs, const double &rhs) {
    Geometry::Quaternion res(lhs);
    res *= rhs;
    return res;
}

Geometry::Quaternion operator * (const double &lhs, const Geometry::Quaternion &rhs) {
    return rhs * lhs;
}

Geometry::Quaternion operator / (const Geometry::Quaternion &lhs, const Geometry::Quaternion &rhs) {
    Geometry::Quaternion res(lhs);
    res /= rhs;
    return res;
}

Geometry::Quaternion operator / (const Geometry::Quaternion &lhs, const double &rhs) {
    Geometry::Quaternion res(lhs);
    res /= rhs;
    return res;
}

Geometry::Quaternion operator / (const double &lhs, const Geometry::Quaternion &rhs) {
    return rhs / lhs;
}

EulerAngles.h

#ifndef GEOMETRY_EULERANGLES_H_
#define GEOMETRY_EULERANGLES_H_

#include "Geometry/Quaternion.h"
#include "Geometry/Vector3D.h"

namespace Geometry {

class EulerAngles {
public:

    enum RotationSequence {
        XZX,
        XYX,
        YXY,
        YZY,
        ZYZ,
        ZXZ,
        XZY,
        XYZ,
        YXZ,
        YZX,
        ZYX,
        ZXY
    };

public:
    EulerAngles(const RotationSequence &sequence = ZYX);
    EulerAngles(const double &alpha, const double &beta, const double &gamma, const RotationSequence &sequence = ZYX);
    EulerAngles(const EulerAngles &rhs);
    virtual ~EulerAngles();
    void setAlpha(const double &alpha);
    void setBeta(const double &beta);
    void setGamma(const double &gamma);
    void set(const double &alpha, const double &beta, const double &gamma);
    /* This is the method that I want to implement */
    void setFromQuaternion(const Quaternion &quat);
    void rotate(const EulerAngles &angles);
    void rotate(const double &alpha, const double &beta, const double &gamma);
    double getAlpha() const;
    double getBeta() const;
    double getGamma() const;
    Quaternion getQuaternion() const;
    RotationSequence getRotationSequence() const;
    EulerAngles getRotatedAngles(const double &alpha, const double &beta, const double &gamma) const;
    EulerAngles getRotatedAngles(const EulerAngles &angles) const;

private:

    void setRotationSequenceAxis(const RotationSequence &sequence);

private:

    RotationSequence rotationSequence;
    Vector3D axes[3];
    double alpha;
    double beta;
    double gamma;
};

} // namespace Geometry

#endif // !GEOMETRY_EULERANGLES_H_

EulerAngles.cpp

#include "Geometry/EulerAngles.h"

namespace Geometry {

EulerAngles::EulerAngles(const RotationSequence &sequence) {
    rotationSequence = sequence;
    alpha = 0.0;
    beta = 0.0;
    gamma = 0.0;
    setRotationSequenceAxis(sequence);
}

EulerAngles::EulerAngles(const double &alpha, const double &beta, const double &gamma, const RotationSequence &sequence) {
    rotationSequence = sequence;
    this->alpha = alpha;
    this->beta = beta;
    this->gamma = gamma;
    setRotationSequenceAxis(sequence);
}

EulerAngles::EulerAngles(const EulerAngles &rhs) {
    rotationSequence = rhs.rotationSequence;
    alpha = rhs.alpha;
    beta = rhs.beta;
    gamma = rhs.gamma;
    setRotationSequenceAxis(rotationSequence);
}

EulerAngles::~EulerAngles() {

}

void EulerAngles::setAlpha(const double &alpha) {
    this->alpha = alpha;
}

void EulerAngles::setBeta(const double &beta) {
    this->beta = beta;
}

void EulerAngles::setGamma(const double &gamma) {
    this->gamma = gamma;
}

void EulerAngles::set(const double &alpha, const double &beta, const double &gamma) {
    this->alpha = alpha;
    this->beta = beta;
    this->gamma = gamma;
}

/**
 * Given a quaternion, calculate rotation angles using the internally defined axis
 * rotation sequence.
 *
 * @param[in] quat Quaternion.
 */
void EulerAngles::setFromQuaternion(const Quaternion &quat) {
    const Quaternion q = quat.getNormalized();
    /*************************************************************
     * This is where I want to calculate Euler angles having the *
     * quaternion as input and the rotation sequence. of the     *
     * instance of this class                                    *
     *************************************************************/
     #error This is where I want to set angles from quat and stored rotation sequence.
}

void EulerAngles::rotate(const double &alpha, const double &beta, const double &gamma) {
    EulerAngles angles(alpha, beta, gamma, rotationSequence);
    rotate(angles);
}

/**
 * Euler angles represent a rotation body in 3D space. With this method
 * is possibile to rotate the body by specified amount. In this case
 * this orientation is rotated using the rotation sequence specified into
 * the argument.
 *
 * @param[in] angles Rotation amount.
 */
void EulerAngles::rotate(const EulerAngles &angles) {
    const Quaternion q1 = getQuaternion();
    const Quaternion q2 = angles.getQuaternion();
    const Quaternion q3(q1 * q2);
    setFromQuaternion(q3);
}

double EulerAngles::getAlpha() const {
    return alpha;
}

double EulerAngles::getBeta() const {
    return beta;
}

double EulerAngles::getGamma() const {
    return gamma;
}

Quaternion EulerAngles::getQuaternion() const {
    const Quaternion qAlpha = Quaternion::fromAngleAxisRotation(alpha, axes[0]);
    const Quaternion qBeta  = Quaternion::fromAngleAxisRotation(beta , axes[1]);
    const Quaternion qGamma = Quaternion::fromAngleAxisRotation(gamma, axes[2]);
    return (qAlpha * qBeta) * qGamma;
}

EulerAngles::RotationSequence EulerAngles::getRotationSequence() const {
    return rotationSequence;
}

void EulerAngles::setRotationSequenceAxis(const RotationSequence &sequence) {
    switch (sequence) {
    case XZX : {
        axes[0] = Vector3D::UnitX;
        axes[1] = Vector3D::UnitZ;
        axes[2] = Vector3D::UnitX;
    } break;
    case XYX : {
        axes[0] = Vector3D::UnitX;
        axes[1] = Vector3D::UnitY;
        axes[2] = Vector3D::UnitX;
    } break;
    case YXY : {
        axes[0] = Vector3D::UnitY;
        axes[1] = Vector3D::UnitX;
        axes[2] = Vector3D::UnitY;
    } break;
    case YZY : {
        axes[0] = Vector3D::UnitY;
        axes[1] = Vector3D::UnitZ;
        axes[2] = Vector3D::UnitY;
    } break;
    case ZYZ : {
        axes[0] = Vector3D::UnitZ;
        axes[1] = Vector3D::UnitY;
        axes[2] = Vector3D::UnitZ;
    } break;
    case ZXZ : {
        axes[0] = Vector3D::UnitZ;
        axes[1] = Vector3D::UnitX;
        axes[2] = Vector3D::UnitZ;
    } break;
    case XZY : {
        axes[0] = Vector3D::UnitX;
        axes[1] = Vector3D::UnitZ;
        axes[2] = Vector3D::UnitY;
    } break;
    case XYZ : {
        axes[0] = Vector3D::UnitX;
        axes[1] = Vector3D::UnitY;
        axes[2] = Vector3D::UnitZ;
    } break;
    case YXZ : {
        axes[0] = Vector3D::UnitY;
        axes[1] = Vector3D::UnitX;
        axes[2] = Vector3D::UnitZ;
    } break;
    case YZX : {
        axes[0] = Vector3D::UnitY;
        axes[1] = Vector3D::UnitZ;
        axes[2] = Vector3D::UnitX;
    } break;
    case ZYX : {
        axes[0] = Vector3D::UnitZ;
        axes[1] = Vector3D::UnitY;
        axes[2] = Vector3D::UnitX;
    } break;
    case ZXY : {
        axes[0] = Vector3D::UnitZ;
        axes[1] = Vector3D::UnitX;
        axes[2] = Vector3D::UnitY;
    } break;
    }
}

} // namespace Geometry