Go to the documentation of this file.
17 #ifndef GZ_MATH_QUATERNION_HH_
18 #define GZ_MATH_QUATERNION_HH_
24 #include <gz/math/config.hh>
31 inline namespace IGNITION_MATH_VERSION_NAMESPACE {
33 template <
typename T>
class Matrix3;
48 : qw(1), qx(0), qy(0), qz(0)
59 public:
Quaternion(
const T &_w,
const T &_x,
const T &_y,
const T &_z)
60 : qw(_w), qx(_x), qy(_y), qz(_z)
67 public:
Quaternion(
const T &_roll,
const T &_pitch,
const T &_yaw)
77 this->Axis(_axis, _angle);
125 this->qx = -this->qx;
126 this->qy = -this->qy;
127 this->qz = -this->qz;
138 s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
140 if (equal<T>(s,
static_cast<T
>(0)))
170 if (std::abs(this->qw) < 1.0)
172 T fAngle = acos(this->qw);
173 T fSin = sin(fAngle);
174 if (std::abs(fSin) >= 1e-3)
176 T fCoeff = fAngle/fSin;
177 result.qx = fCoeff*this->qx;
178 result.qy = fCoeff*this->qy;
179 result.qz = fCoeff*this->qz;
184 result.qx = this->qx;
185 result.qy = this->qy;
186 result.qz = this->qz;
199 T fAngle = sqrt(this->qx*this->qx+
200 this->qy*this->qy+this->qz*this->qz);
201 T fSin = sin(fAngle);
204 result.qw = cos(fAngle);
206 if (std::abs(fSin) >= 1e-3)
208 T fCoeff = fSin/fAngle;
209 result.qx = fCoeff*this->qx;
210 result.qy = fCoeff*this->qy;
211 result.qz = fCoeff*this->qz;
215 result.qx = this->qx;
216 result.qy = this->qy;
217 result.qz = this->qz;
228 s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
229 this->qy * this->qy + this->qz * this->qz));
231 if (equal<T>(s,
static_cast<T
>(0)))
261 public:
void Axis(T _ax, T _ay, T _az, T _aa)
265 l = _ax * _ax + _ay * _ay + _az * _az;
267 if (equal<T>(l,
static_cast<T
>(0)))
277 l = sin(_aa) / sqrt(l);
292 this->Axis(_axis.
X(), _axis.
Y(), _axis.
Z(), _a);
300 public:
void Set(T _w, T _x, T _y, T _z)
315 this->Euler(_vec.
X(), _vec.
Y(), _vec.
Z());
322 public:
void Euler(T _roll, T _pitch, T _yaw)
326 phi = _roll / T(2.0);
327 the = _pitch / T(2.0);
330 this->qw = T(cos(phi) * cos(the) * cos(psi) +
331 sin(phi) * sin(the) * sin(psi));
332 this->qx = T(sin(phi) * cos(the) * cos(psi) -
333 cos(phi) * sin(the) * sin(psi));
334 this->qy = T(cos(phi) * sin(the) * cos(psi) +
335 sin(phi) * cos(the) * sin(psi));
336 this->qz = T(cos(phi) * cos(the) * sin(psi) -
337 sin(phi) * sin(the) * cos(psi));
348 T tol =
static_cast<T
>(1e-15);
358 squ = copy.qw * copy.qw;
359 sqx = copy.qx * copy.qx;
360 sqy = copy.qy * copy.qy;
361 sqz = copy.qz * copy.qz;
364 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
369 else if (sarg >= T(1.0))
375 vec.
Y(T(asin(sarg)));
383 if (std::abs(sarg - 1) < tol)
386 vec.
X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
387 squ - sqx + sqy - sqz)));
390 else if (std::abs(sarg + 1) < tol)
393 vec.
X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
394 squ - sqx + sqy - sqz)));
399 vec.
X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
400 squ - sqx - sqy + sqz)));
403 vec.
Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
404 squ + sqx - sqy - sqz)));
427 return EulerToQuaternion(
Vector3<T>(_x, _y, _z));
434 return this->Euler().X();
441 return this->Euler().Y();
448 return this->Euler().Z();
456 T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
457 if (equal<T>(len,
static_cast<T
>(0)))
464 _angle = 2.0 * acos(this->qw);
465 T invLen = 1.0 / sqrt(len);
466 _axis.
Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
479 const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
480 if (trace > 0.0000001)
482 qw = sqrt(1 + trace) / 2;
483 const T s = 1.0 / (4 * qw);
484 qx = (_mat(2, 1) - _mat(1, 2)) * s;
485 qy = (_mat(0, 2) - _mat(2, 0)) * s;
486 qz = (_mat(1, 0) - _mat(0, 1)) * s;
488 else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
490 qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
491 const T s = 1.0 / (4 * qx);
492 qw = (_mat(2, 1) - _mat(1, 2)) * s;
493 qy = (_mat(1, 0) + _mat(0, 1)) * s;
494 qz = (_mat(0, 2) + _mat(2, 0)) * s;
496 else if (_mat(1, 1) > _mat(2, 2))
498 qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
499 const T s = 1.0 / (4 * qy);
500 qw = (_mat(0, 2) - _mat(2, 0)) * s;
501 qx = (_mat(0, 1) + _mat(1, 0)) * s;
502 qz = (_mat(1, 2) + _mat(2, 1)) * s;
506 qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
507 const T s = 1.0 / (4 * qz);
508 qw = (_mat(1, 0) - _mat(0, 1)) * s;
509 qx = (_mat(0, 2) + _mat(2, 0)) * s;
510 qy = (_mat(1, 2) + _mat(2, 1)) * s;
540 const T kCosTheta = _v1.
Dot(_v2);
543 if (fabs(kCosTheta/k + 1) < 1e-6)
550 if (_v1Abs.
X() < _v1Abs.
Y())
552 if (_v1Abs.
X() < _v1Abs.
Z())
563 if (_v1Abs.
Y() < _v1Abs.
Z())
602 this->ToAxis(axis, angle);
605 this->Axis(axis.
X(), axis.
Y(), axis.
Z(), angle);
614 this->qy + _qt.qy, this->qz + _qt.qz);
634 this->qy - _qt.qy, this->qz - _qt.qz);
653 this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
654 this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
655 this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
656 this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
665 this->qy*_f, this->qz*_f);
683 Vector3<T> qvec(this->qx, this->qy, this->qz);
685 uuv = qvec.
Cross(uv);
686 uv *= (2.0f * this->qw);
689 return _v + uv + uuv;
699 return equal(this->qx, _qt.qx, _tol) &&
700 equal(this->qy, _qt.qy, _tol) &&
701 equal(this->qz, _qt.qz, _tol) &&
702 equal(this->qw, _qt.qw, _tol);
710 return this->Equal(_qt,
static_cast<T
>(0.001));
718 return !(*
this == _qt);
725 return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
734 _vec.
X(), _vec.
Y(), _vec.
Z());
735 tmp = (*this) * (tmp * this->Inverse());
746 tmp = this->Inverse() * (tmp * (*this));
777 if (
equal(this->qw,
static_cast<T
>(0)) &&
778 equal(this->qx,
static_cast<T
>(0)) &&
779 equal(this->qy,
static_cast<T
>(0)) &&
780 equal(this->qz,
static_cast<T
>(0)))
790 T fTy = 2.0f*this->qy;
791 T fTz = 2.0f*this->qz;
793 T fTwy = fTy*this->qw;
794 T fTwz = fTz*this->qw;
795 T fTxy = fTy*this->qx;
796 T fTxz = fTz*this->qx;
797 T fTyy = fTy*this->qy;
798 T fTzz = fTz*this->qz;
800 return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
807 T fTx = 2.0f*this->qx;
808 T fTy = 2.0f*this->qy;
809 T fTz = 2.0f*this->qz;
810 T fTwx = fTx*this->qw;
811 T fTwz = fTz*this->qw;
812 T fTxx = fTx*this->qx;
813 T fTxy = fTy*this->qx;
814 T fTyz = fTz*this->qy;
815 T fTzz = fTz*this->qz;
817 return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
824 T fTx = 2.0f*this->qx;
825 T fTy = 2.0f*this->qy;
826 T fTz = 2.0f*this->qz;
827 T fTwx = fTx*this->qw;
828 T fTwy = fTy*this->qw;
829 T fTxx = fTx*this->qx;
830 T fTxz = fTz*this->qx;
831 T fTyy = fTy*this->qy;
832 T fTyz = fTz*this->qy;
834 return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
841 this->qx =
precision(this->qx, _precision);
842 this->qy =
precision(this->qy, _precision);
843 this->qz =
precision(this->qz, _precision);
844 this->qw =
precision(this->qw, _precision);
852 return this->qw*_q.qw + this->qx * _q.qx +
853 this->qy*_q.qy + this->qz*_q.qz;
869 bool _shortestPath =
false)
871 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
872 Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
874 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
887 bool _shortestPath =
false)
889 T fCos = _rkP.
Dot(_rkQ);
893 if (fCos < 0.0f && _shortestPath)
903 if (std::abs(fCos) < 1 - 1e-03)
906 T fSin = sqrt(1 - (fCos*fCos));
907 T fAngle = atan2(fSin, fCos);
909 T fInvSin = 1.0f / fSin;
910 T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
911 T fCoeff1 = sin(_fT * fAngle) * fInvSin;
912 return _rkP * fCoeff0 + rkT * fCoeff1;
940 const T _deltaT)
const
943 Vector3<T> theta = _angularVelocity * _deltaT / 2;
946 if (thetaMagSq * thetaMagSq / 24.0 <
MIN_D)
948 deltaQ.
W() = 1.0 - thetaMagSq / 2.0;
949 s = 1.0 - thetaMagSq / 6.0;
953 double thetaMag = sqrt(thetaMagSq);
954 deltaQ.
W() = cos(thetaMag);
955 s = sin(thetaMag) / thetaMag;
957 deltaQ.
X() = theta.
X() * s;
958 deltaQ.
Y() = theta.
Y() * s;
959 deltaQ.
Z() = theta.
Z() * s;
960 return deltaQ * (*this);
965 public:
inline const T &
W()
const
972 public:
inline const T &
X()
const
979 public:
inline const T &
Y()
const
986 public:
inline const T &
Z()
const
994 public:
inline T &
W()
1001 public:
inline T &
X()
1008 public:
inline T &
Y()
1015 public:
inline T &
Z()
1022 public:
inline void X(T _v)
1029 public:
inline void Y(T _v)
1036 public:
inline void Z(T _v)
1043 public:
inline void W(T _v)
1053 const gz::math::Quaternion<T> &_q)
1066 gz::math::Quaternion<T> &_q)
1068 Angle roll, pitch, yaw;
1071 _in.
setf(std::ios_base::skipws);
1072 _in >> roll >> pitch >> yaw;
1095 template<
typename T>
const Quaternion<T>
1098 template<
typename T>
const Quaternion<T>
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition: gz/math/Quaternion.hh:322
Quaternion< T > Exp() const
Return the exponent.
Definition: gz/math/Quaternion.hh:193
void ToAxis(Vector3< T > &_axis, T &_angle) const
Return rotation as axis and angle.
Definition: gz/math/Quaternion.hh:454
Quaternion< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition: gz/math/Quaternion.hh:611
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
~Quaternion()
Destructor.
Definition: gz/math/Quaternion.hh:106
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4. This was put here for Windows support.
Definition: gz/math/Helpers.hh:184
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: gz/math/Helpers.hh:556
T Roll() const
Get the Euler roll angle in radians.
Definition: gz/math/Quaternion.hh:432
Quaternion< T > operator*(const Quaternion< T > &_q) const
Multiplication operator.
Definition: gz/math/Quaternion.hh:650
friend std::ostream & operator<<(std::ostream &_out, const Quaternion< T > &_q)
Stream insertion operator.
Definition: gz/math/Quaternion.hh:1052
Quaternion< T > operator-(const Quaternion< T > &_qt) const
Subtraction operator.
Definition: gz/math/Quaternion.hh:631
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: gz/math/Quaternion.hh:300
Quaternion< float > Quaternionf
Definition: gz/math/Quaternion.hh:1102
Quaternion(const Vector3< T > &_rpy)
Constructor.
Definition: gz/math/Quaternion.hh:82
void Axis(const Vector3< T > &_axis, T _a)
Set the quaternion from an axis and angle.
Definition: gz/math/Quaternion.hh:290
Vector3< T > ZAxis() const
Return the Z axis.
Definition: gz/math/Quaternion.hh:822
void Invert()
Invert the quaternion.
Definition: gz/math/Quaternion.hh:121
Quaternion(const Vector3< T > &_axis, const T &_angle)
Constructor from axis angle.
Definition: gz/math/Quaternion.hh:75
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: gz/math/Vector3.hh:129
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition: gz/math/Quaternion.hh:413
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: gz/math/Quaternion.hh:753
A 3x3 matrix class.
Definition: gz/math/Matrix3.hh:40
Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
Constructor.
Definition: gz/math/Quaternion.hh:59
A quaternion class.
Definition: gz/math/Matrix3.hh:35
void Correct()
Correct any nan values in this quaternion.
Definition: gz/math/Quaternion.hh:764
T & W()
Get a mutable w component.
Definition: gz/math/Quaternion.hh:994
void Round(int _precision)
Round all values to _precision decimal places.
Definition: gz/math/Quaternion.hh:839
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: gz/math/Quaternion.hh:41
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition: gz/math/Quaternion.hh:850
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition: gz/math/Quaternion.hh:716
static Quaternion< T > Slerp(T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
Definition: gz/math/Quaternion.hh:885
void Normalize()
Normalize the quaternion.
Definition: gz/math/Quaternion.hh:224
Quaternion< T > Integrate(const Vector3< T > &_angularVelocity, const T _deltaT) const
Integrate quaternion for constant angular velocity vector along specified interval _deltaT....
Definition: gz/math/Quaternion.hh:939
Quaternion< T > Log() const
Return the logarithm.
Definition: gz/math/Quaternion.hh:161
Quaternion< T > operator-() const
Unary minus operator.
Definition: gz/math/Quaternion.hh:723
Quaternion< double > Quaterniond
Definition: gz/math/Quaternion.hh:1101
Quaternion< T > operator*(const T &_f) const
Multiplication operator by a scalar.
Definition: gz/math/Quaternion.hh:662
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: gz/math/Helpers.hh:590
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
const T & Y() const
Get the y component.
Definition: gz/math/Quaternion.hh:979
void From2Axes(const Vector3< T > &_v1, const Vector3< T > &_v2)
Set this quaternion to represent rotation from vector _v1 to vector _v2, so that _v2....
Definition: gz/math/Quaternion.hh:523
Vector3< T > YAxis() const
Return the Y axis.
Definition: gz/math/Quaternion.hh:805
void Scale(T _scale)
Scale a Quaternion<T>
Definition: gz/math/Quaternion.hh:595
Quaternion< T > operator*=(const Quaternion< T > &_qt)
Multiplication operator.
Definition: gz/math/Quaternion.hh:671
Quaternion< T > Normalized() const
Gets a normalized version of this quaternion.
Definition: gz/math/Quaternion.hh:249
static Quaternion< T > Squad(T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkA, const Quaternion< T > &_rkB, const Quaternion< T > &_rkQ, bool _shortestPath=false)
Spherical quadratic interpolation given the ends and an interpolation parameter between 0 and 1.
Definition: gz/math/Quaternion.hh:866
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition: gz/math/Quaternion.hh:425
T Yaw() const
Get the Euler yaw angle in radians.
Definition: gz/math/Quaternion.hh:446
bool operator==(const Quaternion< T > &_qt) const
Equal to operator.
Definition: gz/math/Quaternion.hh:708
T & Z()
Get a mutable z component.
Definition: gz/math/Quaternion.hh:1015
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
Quaternion< int > Quaternioni
Definition: gz/math/Quaternion.hh:1103
bool Equal(const Quaternion< T > &_qt, const T &_tol) const
Equality test with tolerance.
Definition: gz/math/Quaternion.hh:697
Vector3< T > operator*(const Vector3< T > &_v) const
Vector3 multiplication operator.
Definition: gz/math/Quaternion.hh:680
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: gz/math/Vector3.hh:185
const T & Z() const
Get the z component.
Definition: gz/math/Quaternion.hh:986
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition: gz/math/Quaternion.hh:731
static const double MIN_D
Double min value. This value will be similar to 2.22507e-308.
Definition: gz/math/Helpers.hh:260
Quaternion< T > & operator=(const Quaternion< T > &_qt)
Assignment operator.
Definition: gz/math/Quaternion.hh:110
void Y(T _v)
Set the y component.
Definition: gz/math/Quaternion.hh:1029
T & X()
Get a mutable x component.
Definition: gz/math/Quaternion.hh:1001
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition: gz/math/Quaternion.hh:621
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles. The order of operations is roll, pitch, yaw around a fixed body...
Definition: gz/math/Quaternion.hh:313
T Pitch() const
Get the Euler pitch angle in radians.
Definition: gz/math/Quaternion.hh:439
void Axis(T _ax, T _ay, T _az, T _aa)
Set the quaternion from an axis and angle.
Definition: gz/math/Quaternion.hh:261
The Angle class is used to simplify and clarify the use of radians and degrees measurements....
Definition: gz/math/Angle.hh:61
T & Y()
Get a mutable y component.
Definition: gz/math/Quaternion.hh:1008
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: gz/math/Quaternion.hh:132
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: gz/math/Vector3.hh:205
const T & X() const
Get the x component.
Definition: gz/math/Quaternion.hh:972
Quaternion< T > operator-=(const Quaternion< T > &_qt)
Subtraction operator.
Definition: gz/math/Quaternion.hh:641
static const Quaternion Zero
math::Quaternion(0, 0, 0, 0)
Definition: gz/math/Quaternion.hh:44
Quaternion(const Quaternion< T > &_qt)
Copy constructor.
Definition: gz/math/Quaternion.hh:97
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition: gz/math/Quaternion.hh:67
Vector3 Abs() const
Get the absolute value of the vector.
Definition: gz/math/Vector3.hh:229
Quaternion()
Default Constructor.
Definition: gz/math/Quaternion.hh:47
Quaternion(const Matrix3< T > &_mat)
Construct from rotation matrix.
Definition: gz/math/Quaternion.hh:90
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
void Matrix(const Matrix3< T > &_mat)
Set from a rotation matrix.
Definition: gz/math/Quaternion.hh:477
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition: gz/math/Quaternion.hh:344
Vector3< T > XAxis() const
Return the X axis.
Definition: gz/math/Quaternion.hh:788
void W(T _v)
Set the w component.
Definition: gz/math/Quaternion.hh:1043
void Z(T _v)
Set the z component.
Definition: gz/math/Quaternion.hh:1036
const T & W() const
Get the w component.
Definition: gz/math/Quaternion.hh:965
Vector3< T > RotateVectorReverse(const Vector3< T > &_vec) const
Do the reverse rotation of a vector by this quaternion.
Definition: gz/math/Quaternion.hh:742
void X(T _v)
Set the x component.
Definition: gz/math/Quaternion.hh:1022
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: gz/math/Vector3.hh:195
friend std::istream & operator>>(std::istream &_in, Quaternion< T > &_q)
Stream extraction operator.
Definition: gz/math/Quaternion.hh:1065