c++ - Quaternion to Euler angles conversion regarding rotation sequence -
this have:
- i have
vector3dclass represents vector or point in 3d space. - i have
quaternionclass performs calculus on quaternions, , can create rotation unit quaternion angle-axis representation using static methodquaternion::fromangleaxisrotation(double angle, vector3d axis). - i have
euleranglesclass alpha, beta, gamma angles, , in constructor must specify rotation sequence, can 1 of canonical 12 (xzx, yxz, zyx ecc). private member have array of 3 elementsvector3dspecifies rotation sequence axis. i.e. zyx rotation haveaxes[0] = vector3d(1,0,0); axes[1] = vector3d(0,1,0); axes[2] = vector3d(0,0,1);. xzx rotation sequence haveaxes[0] = vector3d(1,0,0); axes[1] = vector3d(0,0,1); axes[2] = vector3d(1,0,0);, on. the class
eulerangleshas methodgetquaternion(). allows calculate corresponding quaternion having 3 rotation angles , rotation sequence. in order work, method creates 3 orderedquaternioninstances usingquaternion::fromangleaxisrotationstatic method find quaternion single rotation angles , multiplies them. allows me have single method calculates rotation unit quaternion in 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 want:
i have quaternion instance. want create in eulerangles class method eulerangles::setfromquaternion(quaternion q) allows me take quaternion (normalized etc.) , create 3 angles it, taking account same rotation sequence stored in axes array.
using math can take account every single rotation sequence combination , threat them separately in order find angles. i'd create unique method in eulerangles::getquaternion().
is there way decompose unit quaternion angles giving rotation sequence input, or must create twelve different methods in order 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); /** useful typedef 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)); } /** * central rotation in space can defined rotation of specifid amount * @f$\theta@f$ , rotation axis, defined vector @f$\left(x, y, z * \right)@f$. method allows build quaternion relative * rotation. * * @param[in] angle rotation angle (radians). * @param[in] p rotation axis. * * @return quaternion related rotation. * * @throw std::underflow_error if vector 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); /* method want 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 quaternion, calculate rotation angles using internally defined axis * rotation sequence. * * @param[in] quat quaternion. */ void eulerangles::setfromquaternion(const quaternion &quat) { const quaternion q = quat.getnormalized(); /************************************************************* * want calculate euler angles having * * quaternion input , rotation sequence. of * * instance of class * *************************************************************/ #error want set angles quat , 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 rotation body in 3d space. method * possibile rotate body specified amount. in case * orientation rotated using rotation sequence specified * 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
Comments
Post a Comment