Преобразование углов кватерниона в Эйлер относительно последовательности вращения

Вот что у меня есть:

• у меня есть Vector3D класс, который представляет вектор или точку в трехмерном пространстве.
• у меня есть Quaternion класс, который выполняет исчисление на кватернионах, и я могу создать кватернион единицы вращения из представления Угол-Ось, используя статический метод Quaternion::fromAngleAxisRotation(double angle, Vector3D axis),
• у меня есть EulerAngles класс с альфа, бета, гамма углами, и в конструкторе я должен указать последовательность вращения, которая может быть одной из канонических 12 (XZX, YXZ, ZYX ecc). Как приватный член у меня есть массив из трех элементов Vector3D это определяет ось последовательности вращения. То есть для вращения ZYX у меня есть axes[0] = Vector3D(1,0,0); axes[1] = Vector3D(0,1,0); axes[2] = Vector3D(0,0,1);, Для последовательности вращения XZX у меня есть axes[0] = Vector3D(1,0,0); axes[1] = Vector3D(0,0,1); axes[2] = Vector3D(1,0,0); и так далее.

• Класс EulerAngles есть метод getQuaternion(), Это позволяет рассчитать соответствующий кватернион, имеющий три угла поворота и последовательность вращения. Для того, чтобы работать, метод создает три упорядоченных Quaternion случаи использования Quaternion::fromAngleAxisRotation Статический метод, чтобы найти кватернион для отдельных углов поворота, а затем он умножает их. Это позволяет мне иметь единственный метод, который вычисляет кватернион единицы вращения во всех двенадцати последовательностях вращения.

  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;

И это то, что я хочу:

у меня есть Quaternion пример. Я хочу создать в EulerAngles класс метод EulerAngles::setFromQuaternion(Quaternion q) что позволяет мне взять кватернион (нормализованный и т. д.) и создать из него три угла, учитывая ту же последовательность вращения, которая хранится в axes массив.

Используя некоторую математику, я могу учесть каждую комбинацию последовательностей вращения и угрожать им отдельно, чтобы найти углы. Но я хотел бы создать уникальный метод, как я делаю в EulerAngles::getQuaternion(),

Есть ли способ разложить единичный кватернион на углы, дающие последовательность вращения в качестве входных данных, или я должен создать двенадцать различных методов, чтобы найти их?

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