关于旋转顺序的四元数到欧拉角的转换

时间：2014-11-13 11:03:40

标签： c++ rotation quaternions euler-angles

这就是我所拥有的：

  • 我有一个Vector3D类，用于表示3D空间中的矢量或点。
  • 我有一个Quaternion类，它在四元数上执行微积分，我可以使用静态方法Quaternion::fromAngleAxisRotation(double angle, Vector3D axis)从Angle-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)，允许我采用四元数（标准化等）并从中创建三个角度，同时考虑{{1}中存储的相同旋转序列} array。

使用一些数学我可以考虑每个旋转序列组合并分别威胁它们以找到角度。但我想像axes中那样创建一种独特的方法。

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

Vector3D.h 

EulerAngles::getQuaternion()

Vector3D.cpp 

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

Quaternion.h 

#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.cpp 

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

EulerAngles.h 

#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.cpp 

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

0 个答案:

没有答案
相关问题
最新问题