17 #ifndef _IGNITION_QUATERNION_HH_
18 #define _IGNITION_QUATERNION_HH_
38 : qw(1), qx(0), qy(0), qz(0)
49 public:
Quaternion(
const T &_w,
const T &_x,
const T &_y,
const T &_z)
50 : qw(_w), qx(_x), qy(_y), qz(_z)
57 public:
Quaternion(
const T &_roll,
const T &_pitch,
const T &_yaw)
67 this->
Axis(_axis, _angle);
107 this->qx = -this->qx;
108 this->qy = -this->qy;
109 this->qz = -this->qz;
120 s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
122 if (equal<T>(s, static_cast<T>(0)))
152 if (std::abs(this->qw) < 1.0)
154 T fAngle = acos(this->qw);
155 T fSin = sin(fAngle);
156 if (std::abs(fSin) >= 1e-3)
158 T fCoeff = fAngle/fSin;
159 result.qx = fCoeff*this->qx;
160 result.qy = fCoeff*this->qy;
161 result.qz = fCoeff*this->qz;
166 result.qx = this->qx;
167 result.qy = this->qy;
168 result.qz = this->qz;
181 T fAngle = sqrt(this->qx*this->qx+
182 this->qy*this->qy+this->qz*this->qz);
183 T fSin = sin(fAngle);
186 result.qw = cos(fAngle);
188 if (std::abs(fSin) >= 1e-3)
190 T fCoeff = fSin/fAngle;
191 result.qx = fCoeff*this->qx;
192 result.qy = fCoeff*this->qy;
193 result.qz = fCoeff*this->qz;
197 result.qx = this->qx;
198 result.qy = this->qy;
199 result.qz = this->qz;
210 s = sqrt(this->qw * this->qw + this->qx * this->qx +
211 this->qy * this->qy + this->qz * this->qz);
213 if (equal<T>(s, static_cast<T>(0)))
234 public:
void Axis(T _ax, T _ay, T _az, T _aa)
238 l = _ax * _ax + _ay * _ay + _az * _az;
240 if (equal<T>(l, static_cast<T>(0)))
250 l = sin(_aa) / sqrt(l);
265 this->
Axis(_axis.
X(), _axis.
Y(), _axis.
Z(), _a);
273 public:
void Set(T _w, T _x, T _y, T _z)
286 this->
Euler(_vec.
X(), _vec.
Y(), _vec.
Z());
293 public:
void Euler(T _roll, T _pitch, T _yaw)
301 this->qw = cos(phi) * cos(the) * cos(psi) +
302 sin(phi) * sin(the) * sin(psi);
303 this->qx = sin(phi) * cos(the) * cos(psi) -
304 cos(phi) * sin(the) * sin(psi);
305 this->qy = cos(phi) * sin(the) * cos(psi) +
306 sin(phi) * cos(the) * sin(psi);
307 this->qz = cos(phi) * cos(the) * sin(psi) -
308 sin(phi) * sin(the) * cos(psi);
327 squ = copy.qw * copy.qw;
328 sqx = copy.qx * copy.qx;
329 sqy = copy.qy * copy.qy;
330 sqz = copy.qz * copy.qz;
333 vec.
X(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
334 squ - sqx - sqy + sqz));
337 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
338 vec.
Y(sarg <= -1.0 ? -0.5*
IGN_PI :
339 (sarg >= 1.0 ? 0.5*
IGN_PI : asin(sarg)));
342 vec.
Z(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
343 squ + sqx - sqy - sqz));
370 return this->
Euler().X();
377 return this->
Euler().Y();
384 return this->
Euler().Z();
392 T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
393 if (equal<T>(len, static_cast<T>(0)))
400 _angle = 2.0 * acos(this->qw);
401 T invLen = 1.0 / sqrt(len);
402 _axis.
Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
415 this->
ToAxis(axis, angle);
418 this->
Axis(axis.
X(), axis.
Y(), axis.
Z(), angle);
427 this->qy + _qt.qy, this->qz + _qt.qz);
447 this->qy - _qt.qy, this->qz - _qt.qz);
466 this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
467 this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
468 this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
469 this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
478 this->qy*_f, this->qz*_f);
495 Vector3<T> qvec(this->qx, this->qy, this->qz);
497 uuv = qvec.
Cross(uv);
498 uv *= (2.0f * this->qw);
501 return _v + uv + uuv;
509 return equal(this->qx, _qt.qx, static_cast<T>(0.001)) &&
510 equal(this->qy, _qt.qy, static_cast<T>(0.001)) &&
511 equal(this->qz, _qt.qz, static_cast<T>(0.001)) &&
512 equal(this->qw, _qt.qw, static_cast<T>(0.001));
520 return !
equal(this->qx, _qt.qx, static_cast<T>(0.001)) ||
521 !
equal(this->qy, _qt.qy, static_cast<T>(0.001)) ||
522 !
equal(this->qz, _qt.qz, static_cast<T>(0.001)) ||
523 !
equal(this->qw, _qt.qw, static_cast<T>(0.001));
530 return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
539 _vec.
X(), _vec.
Y(), _vec.
Z());
540 tmp = (*this) * (tmp * this->
Inverse());
551 tmp = this->
Inverse() * (tmp * (*this));
562 return std::isfinite(static_cast<double>(this->qw)) &&
563 std::isfinite(static_cast<double>(this->qx)) &&
564 std::isfinite(static_cast<double>(this->qy)) &&
565 std::isfinite(static_cast<double>(this->qz));
573 if (!std::isfinite(static_cast<double>(this->qx)))
575 if (!std::isfinite(static_cast<double>(this->qy)))
577 if (!std::isfinite(static_cast<double>(this->qz)))
579 if (!std::isfinite(static_cast<double>(this->qw)))
582 if (
equal(this->qw, static_cast<T>(0)) &&
583 equal(this->qx, static_cast<T>(0)) &&
584 equal(this->qy, static_cast<T>(0)) &&
585 equal(this->qz, static_cast<T>(0)))
595 T fTy = 2.0f*this->qy;
596 T fTz = 2.0f*this->qz;
598 T fTwy = fTy*this->qw;
599 T fTwz = fTz*this->qw;
600 T fTxy = fTy*this->qx;
601 T fTxz = fTz*this->qx;
602 T fTyy = fTy*this->qy;
603 T fTzz = fTz*this->qz;
605 return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
612 T fTx = 2.0f*this->qx;
613 T fTy = 2.0f*this->qy;
614 T fTz = 2.0f*this->qz;
615 T fTwx = fTx*this->qw;
616 T fTwz = fTz*this->qw;
617 T fTxx = fTx*this->qx;
618 T fTxy = fTy*this->qx;
619 T fTyz = fTz*this->qy;
620 T fTzz = fTz*this->qz;
622 return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
629 T fTx = 2.0f*this->qx;
630 T fTy = 2.0f*this->qy;
631 T fTz = 2.0f*this->qz;
632 T fTwx = fTx*this->qw;
633 T fTwy = fTy*this->qw;
634 T fTxx = fTx*this->qx;
635 T fTxz = fTz*this->qx;
636 T fTyy = fTy*this->qy;
637 T fTyz = fTz*this->qy;
639 return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
646 this->qx =
precision(this->qx, _precision);
647 this->qy =
precision(this->qy, _precision);
648 this->qz =
precision(this->qz, _precision);
649 this->qw =
precision(this->qw, _precision);
657 return this->qw*_q.qw + this->qx * _q.qx +
658 this->qy*_q.qy + this->qz*_q.qz;
673 bool _shortestPath =
false)
675 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
678 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
690 bool _shortestPath =
false)
692 T fCos = _rkP.
Dot(_rkQ);
696 if (fCos < 0.0f && _shortestPath)
706 if (std::abs(fCos) < 1 - 1e-03)
709 T fSin = sqrt(1 - (fCos*fCos));
710 T fAngle = atan2(fSin, fCos);
712 T fInvSin = 1.0f / fSin;
713 T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
714 T fCoeff1 = sin(_fT * fAngle) * fInvSin;
715 return _rkP * fCoeff0 + rkT * fCoeff1;
736 public:
inline const T &
W()
const
743 public:
inline const T &
X()
const
750 public:
inline const T &
Y()
const
757 public:
inline const T &
Z()
const
765 public:
inline T &
W()
772 public:
inline T &
X()
779 public:
inline T &
Y()
786 public:
inline T &
Z()
793 public:
inline void X(T _v)
800 public:
inline void Y(T _v)
807 public:
inline void Z(T _v)
814 public:
inline void W(T _v)
839 Angle roll, pitch, yaw;
842 _in.setf(std::ios_base::skipws);
843 _in >> roll >> pitch >> yaw;
863 template<
typename T>
const Quaternion<T>
An angle and related functions.
Definition: Angle.hh:44
Quaternion< double > Quaterniond
Definition: Quaternion.hh:866
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Quaternion.hh:644
void Y(T _v)
Set the y component.
Definition: Quaternion.hh:800
Quaternion(const Vector3< T > &_axis, const T &_angle)
Constructor from axis angle.
Definition: Quaternion.hh:65
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:171
~Quaternion()
Destructor.
Definition: Quaternion.hh:88
void X(T _v)
Set the x component.
Definition: Quaternion.hh:793
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:293
Quaternion(const Vector3< T > &_rpy)
Constructor.
Definition: Quaternion.hh:72
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:188
Vector3< T > operator*(const Vector3< T > &_v) const
Vector3 multiplication operator.
Definition: Quaternion.hh:492
bool operator==(const Quaternion< T > &_qt) const
Equal to operator.
Definition: Quaternion.hh:507
void Z(T _v)
Set the z component.
Definition: Quaternion.hh:807
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:114
void Invert()
Invert the quaternion.
Definition: Quaternion.hh:103
Quaternion< T > operator-(const Quaternion< T > &_qt) const
Substraction operator.
Definition: Quaternion.hh:444
T & X()
Get a mutable x component.
Definition: Quaternion.hh:772
T Roll() const
Get the Euler roll angle in radians.
Definition: Quaternion.hh:368
Quaternion< float > Quaternionf
Definition: Quaternion.hh:867
friend std::istream & operator>>(std::istream &_in, ignition::math::Quaternion< T > &_q)
Stream extraction operator.
Definition: Quaternion.hh:836
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: Quaternion.hh:670
const T & Y() const
Get the y component.
Definition: Quaternion.hh:750
T & Y()
Get a mutable y component.
Definition: Quaternion.hh:779
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition: Quaternion.hh:315
T Pitch() const
Get the Euler pitch angle in radians.
Definition: Quaternion.hh:375
T X() const
Get the x value.
Definition: Vector3.hh:545
Quaternion< T > operator*(const T &_f) const
Multiplication operator.
Definition: Quaternion.hh:475
const T & Z() const
Get the z component.
Definition: Quaternion.hh:757
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition: Quaternion.hh:434
void Correct()
Correct any nan.
Definition: Quaternion.hh:569
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:181
Quaternion(const Quaternion< T > &_qt)
Copy constructor.
Definition: Quaternion.hh:79
Vector3< T > YAxis() const
Return the Y axis.
Definition: Quaternion.hh:610
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition: Quaternion.hh:350
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition: Quaternion.hh:655
Quaternion< T > & operator=(const Quaternion< T > &_qt)
Equal operator.
Definition: Quaternion.hh:92
T Y() const
Get the y value.
Definition: Vector3.hh:552
void Axis(const Vector3< T > &_axis, T _a)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:263
void Axis(T _ax, T _ay, T _az, T _aa)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:234
Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
Constructor.
Definition: Quaternion.hh:49
Vector3< T > RotateVectorReverse(Vector3< T > _vec) const
Do the reverse rotation of a vector by this quaternion.
Definition: Quaternion.hh:547
static const Quaternion Identity
math::Quaternion(1, 0, 0, 1)
Definition: Quaternion.hh:34
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Quaternion< T > &_q)
Stream insertion operator.
Definition: Quaternion.hh:823
Vector3< T > XAxis() const
Return the X axis.
Definition: Quaternion.hh:593
Quaternion< T > operator*(const Quaternion< T > &_q) const
Multiplication operator.
Definition: Quaternion.hh:463
T & W()
Get a mutable w component.
Definition: Quaternion.hh:765
T Z() const
Get the z value.
Definition: Vector3.hh:559
void Scale(T _scale)
Scale a Quaternion<T>ion.
Definition: Quaternion.hh:408
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: Quaternion.hh:688
void ToAxis(Vector3< T > &_axis, T &_angle) const
Return rotation as axis and angle.
Definition: Quaternion.hh:390
Quaternion< T > operator-() const
Unary minus operator.
Definition: Quaternion.hh:528
Quaternion< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition: Quaternion.hh:424
T & Z()
Get a mutable z component.
Definition: Quaternion.hh:786
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
bool IsFinite() const
See if a quatern is finite (e.g., not nan)
Definition: Quaternion.hh:558
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition: Quaternion.hh:518
void W(T _v)
Set the w component.
Definition: Quaternion.hh:814
Quaternion< T > operator*=(const Quaternion< T > &qt)
Multiplication operator.
Definition: Quaternion.hh:484
Quaternion< int > Quaternioni
Definition: Quaternion.hh:868
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:284
T Yaw() const
Get the Euler yaw angle in radians.
Definition: Quaternion.hh:382
Quaternion()
Default Constructor.
Definition: Quaternion.hh:37
const T & W() const
Get the w component.
Definition: Quaternion.hh:736
bool equal(const T &_a, const T &_b, const T &_epsilon=1e-6)
check if two values are equal, within a tolerance
Definition: Helpers.hh:177
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:206
A quaternion class.
Definition: Quaternion.hh:31
Quaternion< T > Exp() const
Return the exponent.
Definition: Quaternion.hh:175
Quaternion< T > operator-=(const Quaternion< T > &_qt)
Substraction operator.
Definition: Quaternion.hh:454
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:59
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition: Quaternion.hh:536
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition: Quaternion.hh:57
Quaternion< T > Log() const
Return the logarithm.
Definition: Quaternion.hh:143
Vector3< T > ZAxis() const
Return the Z axis.
Definition: Quaternion.hh:627
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition: Quaternion.hh:361
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: Quaternion.hh:273
const T & X() const
Get the x component.
Definition: Quaternion.hh:743