1

这就是我所拥有的：

  • 我有一个Vector3D表示 3D 空间中的向量或点的类。
  • 我有一个Quaternion对四元数执行微积分的类，我可以使用静态方法从角度轴表示创建一个旋转单位四元数Quaternion::fromAngleAxisRotation(double angle, Vector3D axis)。
  • 我有一个EulerAngles具有 alpha、beta、gamma 角度的类，并且在构造函数中我必须指定旋转顺序，它可以是规范 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().

有没有办法将一个单位四元数分解成角度，将旋转序列作为输入，或者我必须创建十二种不同的方法才能找到它们？

矢量3D.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_

矢量3D.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

四元数

#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_

四元数.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
4

0 回答 0