All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Quaternion.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef _IGNITION_QUATERNION_HH_
18 #define _IGNITION_QUATERNION_HH_
19 
20 #include <ignition/math/Helpers.hh>
21 #include <ignition/math/Angle.hh>
22 #include <ignition/math/Vector3.hh>
23 
24 namespace ignition
25 {
26  namespace math
27  {
30  template<typename T>
31  class Quaternion
32  {
34  public: static const Quaternion Identity;
35 
37  public: Quaternion()
38  : qw(1), qx(0), qy(0), qz(0)
39  {
40  // quaternion not normalized, because that breaks
41  // Pose::CoordPositionAdd(...)
42  }
43 
49  public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
50  : qw(_w), qx(_x), qy(_y), qz(_z)
51  {}
52 
57  public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
58  {
59  this->Euler(Vector3<T>(_roll, _pitch, _yaw));
60  }
61 
65  public: Quaternion(const Vector3<T> &_axis, const T &_angle)
66  {
67  this->Axis(_axis, _angle);
68  }
69 
72  public: Quaternion(const Vector3<T> &_rpy)
73  {
74  this->Euler(_rpy);
75  }
76 
79  public: Quaternion(const Quaternion<T> &_qt)
80  {
81  this->qw = _qt.qw;
82  this->qx = _qt.qx;
83  this->qy = _qt.qy;
84  this->qz = _qt.qz;
85  }
86 
88  public: ~Quaternion() {}
89 
92  public: Quaternion<T> &operator=(const Quaternion<T> &_qt)
93  {
94  this->qw = _qt.qw;
95  this->qx = _qt.qx;
96  this->qy = _qt.qy;
97  this->qz = _qt.qz;
98 
99  return *this;
100  }
101 
103  public: void Invert()
104  {
105  this->Normalize();
106  // this->qw = this->qw;
107  this->qx = -this->qx;
108  this->qy = -this->qy;
109  this->qz = -this->qz;
110  }
111 
114  public: inline Quaternion<T> Inverse() const
115  {
116  T s = 0;
117  Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
118 
119  // use s to test if quaternion is valid
120  s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
121 
122  if (equal<T>(s, static_cast<T>(0)))
123  {
124  q.qw = 1.0;
125  q.qx = 0.0;
126  q.qy = 0.0;
127  q.qz = 0.0;
128  }
129  else
130  {
131  // deal with non-normalized quaternion
132  // div by s so q * qinv = identity
133  q.qw = q.qw / s;
134  q.qx = -q.qx / s;
135  q.qy = -q.qy / s;
136  q.qz = -q.qz / s;
137  }
138  return q;
139  }
140 
143  public: Quaternion<T> Log() const
144  {
145  // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
146  // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
147  // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
148 
149  Quaternion<T> result;
150  result.qw = 0.0;
151 
152  if (std::abs(this->qw) < 1.0)
153  {
154  T fAngle = acos(this->qw);
155  T fSin = sin(fAngle);
156  if (std::abs(fSin) >= 1e-3)
157  {
158  T fCoeff = fAngle/fSin;
159  result.qx = fCoeff*this->qx;
160  result.qy = fCoeff*this->qy;
161  result.qz = fCoeff*this->qz;
162  return result;
163  }
164  }
165 
166  result.qx = this->qx;
167  result.qy = this->qy;
168  result.qz = this->qz;
169 
170  return result;
171  }
172 
175  public: Quaternion<T> Exp() const
176  {
177  // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
178  // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
179  // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
180 
181  T fAngle = sqrt(this->qx*this->qx+
182  this->qy*this->qy+this->qz*this->qz);
183  T fSin = sin(fAngle);
184 
185  Quaternion<T> result;
186  result.qw = cos(fAngle);
187 
188  if (std::abs(fSin) >= 1e-3)
189  {
190  T fCoeff = fSin/fAngle;
191  result.qx = fCoeff*this->qx;
192  result.qy = fCoeff*this->qy;
193  result.qz = fCoeff*this->qz;
194  }
195  else
196  {
197  result.qx = this->qx;
198  result.qy = this->qy;
199  result.qz = this->qz;
200  }
201 
202  return result;
203  }
204 
206  public: void Normalize()
207  {
208  T s = 0;
209 
210  s = sqrt(this->qw * this->qw + this->qx * this->qx +
211  this->qy * this->qy + this->qz * this->qz);
212 
213  if (equal<T>(s, static_cast<T>(0)))
214  {
215  this->qw = 1.0;
216  this->qx = 0.0;
217  this->qy = 0.0;
218  this->qz = 0.0;
219  }
220  else
221  {
222  this->qw /= s;
223  this->qx /= s;
224  this->qy /= s;
225  this->qz /= s;
226  }
227  }
228 
234  public: void Axis(T _ax, T _ay, T _az, T _aa)
235  {
236  T l;
237 
238  l = _ax * _ax + _ay * _ay + _az * _az;
239 
240  if (equal<T>(l, static_cast<T>(0)))
241  {
242  this->qw = 1;
243  this->qx = 0;
244  this->qy = 0;
245  this->qz = 0;
246  }
247  else
248  {
249  _aa *= 0.5;
250  l = sin(_aa) / sqrt(l);
251  this->qw = cos(_aa);
252  this->qx = _ax * l;
253  this->qy = _ay * l;
254  this->qz = _az * l;
255  }
256 
257  this->Normalize();
258  }
259 
263  public: void Axis(const Vector3<T> &_axis, T _a)
264  {
265  this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a);
266  }
267 
273  public: void Set(T _w, T _x, T _y, T _z)
274  {
275  this->qw = _w;
276  this->qx = _x;
277  this->qy = _y;
278  this->qz = _z;
279  }
280 
286  public: void Euler(const Vector3<T> &_vec)
287  {
288  this->Euler(_vec.X(), _vec.Y(), _vec.Z());
289  }
290 
295  public: void Euler(T _roll, T _pitch, T _yaw)
296  {
297  T phi, the, psi;
298 
299  phi = _roll / 2.0;
300  the = _pitch / 2.0;
301  psi = _yaw / 2.0;
302 
303  this->qw = cos(phi) * cos(the) * cos(psi) +
304  sin(phi) * sin(the) * sin(psi);
305  this->qx = sin(phi) * cos(the) * cos(psi) -
306  cos(phi) * sin(the) * sin(psi);
307  this->qy = cos(phi) * sin(the) * cos(psi) +
308  sin(phi) * cos(the) * sin(psi);
309  this->qz = cos(phi) * cos(the) * sin(psi) -
310  sin(phi) * sin(the) * cos(psi);
311 
312  this->Normalize();
313  }
314 
317  public: Vector3<T> Euler() const
318  {
319  Vector3<T> vec;
320 
321  Quaternion<T> copy = *this;
322  T squ;
323  T sqx;
324  T sqy;
325  T sqz;
326 
327  copy.Normalize();
328 
329  squ = copy.qw * copy.qw;
330  sqx = copy.qx * copy.qx;
331  sqy = copy.qy * copy.qy;
332  sqz = copy.qz * copy.qz;
333 
334  // Roll
335  vec.X(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
336  squ - sqx - sqy + sqz));
337 
338  // Pitch
339  T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
340  vec.Y(sarg <= -1.0 ? -0.5*IGN_PI :
341  (sarg >= 1.0 ? 0.5*IGN_PI : asin(sarg)));
342 
343  // Yaw
344  vec.Z(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
345  squ + sqx - sqy - sqz));
346 
347  return vec;
348  }
349 
353  public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
354  {
355  Quaternion<T> result;
356  result.Euler(_vec);
357  return result;
358  }
359 
365  public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
366  {
367  return EulerToQuaternion(Vector3<T>(_x, _y, _z));
368  }
369 
372  public: T Roll() const
373  {
374  return this->Euler().X();
375  }
376 
379  public: T Pitch() const
380  {
381  return this->Euler().Y();
382  }
383 
386  public: T Yaw() const
387  {
388  return this->Euler().Z();
389  }
390 
394  public: void ToAxis(Vector3<T> &_axis, T &_angle) const
395  {
396  T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
397  if (equal<T>(len, static_cast<T>(0)))
398  {
399  _angle = 0.0;
400  _axis.Set(1, 0, 0);
401  }
402  else
403  {
404  _angle = 2.0 * acos(this->qw);
405  T invLen = 1.0 / sqrt(len);
406  _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
407  }
408  }
409 
412  public: void Scale(T _scale)
413  {
414  Quaternion<T> b;
415  Vector3<T> axis;
416  T angle;
417 
418  // Convert to axis-and-angle
419  this->ToAxis(axis, angle);
420  angle *= _scale;
421 
422  this->Axis(axis.X(), axis.Y(), axis.Z(), angle);
423  }
424 
428  public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
429  {
430  Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
431  this->qy + _qt.qy, this->qz + _qt.qz);
432  return result;
433  }
434 
439  {
440  *this = *this + _qt;
441 
442  return *this;
443  }
444 
448  public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
449  {
450  Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
451  this->qy - _qt.qy, this->qz - _qt.qz);
452  return result;
453  }
454 
459  {
460  *this = *this - _qt;
461  return *this;
462  }
463 
467  public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
468  {
469  return Quaternion<T>(
470  this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
471  this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
472  this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
473  this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
474  }
475 
479  public: Quaternion<T> operator*(const T &_f) const
480  {
481  return Quaternion<T>(this->qw*_f, this->qx*_f,
482  this->qy*_f, this->qz*_f);
483  }
484 
489  {
490  *this = *this * qt;
491  return *this;
492  }
493 
497  public: Vector3<T> operator*(const Vector3<T> &_v) const
498  {
499  Vector3<T> uv, uuv;
500  Vector3<T> qvec(this->qx, this->qy, this->qz);
501  uv = qvec.Cross(_v);
502  uuv = qvec.Cross(uv);
503  uv *= (2.0f * this->qw);
504  uuv *= 2.0f;
505 
506  return _v + uv + uuv;
507  }
508 
512  public: bool operator==(const Quaternion<T> &_qt) const
513  {
514  return equal(this->qx, _qt.qx, static_cast<T>(0.001)) &&
515  equal(this->qy, _qt.qy, static_cast<T>(0.001)) &&
516  equal(this->qz, _qt.qz, static_cast<T>(0.001)) &&
517  equal(this->qw, _qt.qw, static_cast<T>(0.001));
518  }
519 
523  public: bool operator!=(const Quaternion<T> &_qt) const
524  {
525  return !equal(this->qx, _qt.qx, static_cast<T>(0.001)) ||
526  !equal(this->qy, _qt.qy, static_cast<T>(0.001)) ||
527  !equal(this->qz, _qt.qz, static_cast<T>(0.001)) ||
528  !equal(this->qw, _qt.qw, static_cast<T>(0.001));
529  }
530 
533  public: Quaternion<T> operator-() const
534  {
535  return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
536  }
537 
541  public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
542  {
543  Quaternion<T> tmp(static_cast<T>(0),
544  _vec.X(), _vec.Y(), _vec.Z());
545  tmp = (*this) * (tmp * this->Inverse());
546  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
547  }
548 
553  {
554  Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
555 
556  tmp = this->Inverse() * (tmp * (*this));
557 
558  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
559  }
560 
563  public: bool IsFinite() const
564  {
565  // std::isfinite works with floating point values, need to explicit
566  // cast to avoid ambiguity in vc++.
567  return std::isfinite(static_cast<double>(this->qw)) &&
568  std::isfinite(static_cast<double>(this->qx)) &&
569  std::isfinite(static_cast<double>(this->qy)) &&
570  std::isfinite(static_cast<double>(this->qz));
571  }
572 
574  public: inline void Correct()
575  {
576  // std::isfinite works with floating point values, need to explicit
577  // cast to avoid ambiguity in vc++.
578  if (!std::isfinite(static_cast<double>(this->qx)))
579  this->qx = 0;
580  if (!std::isfinite(static_cast<double>(this->qy)))
581  this->qy = 0;
582  if (!std::isfinite(static_cast<double>(this->qz)))
583  this->qz = 0;
584  if (!std::isfinite(static_cast<double>(this->qw)))
585  this->qw = 1;
586 
587  if (equal(this->qw, static_cast<T>(0)) &&
588  equal(this->qx, static_cast<T>(0)) &&
589  equal(this->qy, static_cast<T>(0)) &&
590  equal(this->qz, static_cast<T>(0)))
591  {
592  this->qw = 1;
593  }
594  }
595 
598  public: Vector3<T> XAxis() const
599  {
600  T fTy = 2.0f*this->qy;
601  T fTz = 2.0f*this->qz;
602 
603  T fTwy = fTy*this->qw;
604  T fTwz = fTz*this->qw;
605  T fTxy = fTy*this->qx;
606  T fTxz = fTz*this->qx;
607  T fTyy = fTy*this->qy;
608  T fTzz = fTz*this->qz;
609 
610  return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
611  }
612 
615  public: Vector3<T> YAxis() const
616  {
617  T fTx = 2.0f*this->qx;
618  T fTy = 2.0f*this->qy;
619  T fTz = 2.0f*this->qz;
620  T fTwx = fTx*this->qw;
621  T fTwz = fTz*this->qw;
622  T fTxx = fTx*this->qx;
623  T fTxy = fTy*this->qx;
624  T fTyz = fTz*this->qy;
625  T fTzz = fTz*this->qz;
626 
627  return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
628  }
629 
632  public: Vector3<T> ZAxis() const
633  {
634  T fTx = 2.0f*this->qx;
635  T fTy = 2.0f*this->qy;
636  T fTz = 2.0f*this->qz;
637  T fTwx = fTx*this->qw;
638  T fTwy = fTy*this->qw;
639  T fTxx = fTx*this->qx;
640  T fTxz = fTz*this->qx;
641  T fTyy = fTy*this->qy;
642  T fTyz = fTz*this->qy;
643 
644  return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
645  }
646 
649  public: void Round(int _precision)
650  {
651  this->qx = precision(this->qx, _precision);
652  this->qy = precision(this->qy, _precision);
653  this->qz = precision(this->qz, _precision);
654  this->qw = precision(this->qw, _precision);
655  }
656 
660  public: T Dot(const Quaternion<T> &_q) const
661  {
662  return this->qw*_q.qw + this->qx * _q.qx +
663  this->qy*_q.qy + this->qz*_q.qz;
664  }
665 
676  public: static Quaternion<T> Squad(T _fT,
677  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
678  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
679  bool _shortestPath = false)
680  {
681  T fSlerpT = 2.0f*_fT*(1.0f-_fT);
682  Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
683  Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
684  return Slerp(fSlerpT, kSlerpP, kSlerpQ);
685  }
686 
695  public: static Quaternion<T> Slerp(T _fT,
696  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
697  bool _shortestPath = false)
698  {
699  T fCos = _rkP.Dot(_rkQ);
700  Quaternion<T> rkT;
701 
702  // Do we need to invert rotation?
703  if (fCos < 0.0f && _shortestPath)
704  {
705  fCos = -fCos;
706  rkT = -_rkQ;
707  }
708  else
709  {
710  rkT = _rkQ;
711  }
712 
713  if (std::abs(fCos) < 1 - 1e-03)
714  {
715  // Standard case (slerp)
716  T fSin = sqrt(1 - (fCos*fCos));
717  T fAngle = atan2(fSin, fCos);
718  // FIXME: should check if (std::abs(fSin) >= 1e-3)
719  T fInvSin = 1.0f / fSin;
720  T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
721  T fCoeff1 = sin(_fT * fAngle) * fInvSin;
722  return _rkP * fCoeff0 + rkT * fCoeff1;
723  }
724  else
725  {
726  // There are two situations:
727  // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
728  // so we can do a linear interpolation safely.
729  // 2. "rkP" and "rkQ" are almost inverse of each
730  // other (fCos ~= -1), there
731  // are an infinite number of possibilities interpolation.
732  // but we haven't have method to fix this case, so just use
733  // linear interpolation here.
734  Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
735  // taking the complement requires renormalisation
736  t.Normalize();
737  return t;
738  }
739  }
740 
749  public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
750  const T _deltaT) const
751  {
752  Quaternion<T> deltaQ;
753  Vector3<T> theta = _angularVelocity * _deltaT * 0.5;
754  T thetaMagSq = theta.SquaredLength();
755  T s;
756  if (thetaMagSq * thetaMagSq / 24.0 < IGN_DBL_MIN)
757  {
758  deltaQ.W() = 1.0 - thetaMagSq / 2.0;
759  s = 1.0 - thetaMagSq / 6.0;
760  }
761  else
762  {
763  double thetaMag = sqrt(thetaMagSq);
764  deltaQ.W() = cos(thetaMag);
765  s = sin(thetaMag) / thetaMag;
766  }
767  deltaQ.X() = theta.X() * s;
768  deltaQ.Y() = theta.Y() * s;
769  deltaQ.Z() = theta.Z() * s;
770  return deltaQ * (*this);
771  }
772 
775  public: inline const T &W() const
776  {
777  return this->qw;
778  }
779 
782  public: inline const T &X() const
783  {
784  return this->qx;
785  }
786 
789  public: inline const T &Y() const
790  {
791  return this->qy;
792  }
793 
796  public: inline const T &Z() const
797  {
798  return this->qz;
799  }
800 
801 
804  public: inline T &W()
805  {
806  return this->qw;
807  }
808 
811  public: inline T &X()
812  {
813  return this->qx;
814  }
815 
818  public: inline T &Y()
819  {
820  return this->qy;
821  }
822 
825  public: inline T &Z()
826  {
827  return this->qz;
828  }
829 
832  public: inline void X(T _v)
833  {
834  this->qx = _v;
835  }
836 
839  public: inline void Y(T _v)
840  {
841  this->qy = _v;
842  }
843 
846  public: inline void Z(T _v)
847  {
848  this->qz = _v;
849  }
850 
853  public: inline void W(T _v)
854  {
855  this->qw = _v;
856  }
857 
862  public: friend std::ostream &operator<<(std::ostream &_out,
864  {
865  Vector3<T> v(_q.Euler());
866  _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " "
867  << precision(v.Z(), 6);
868  return _out;
869  }
870 
875  public: friend std::istream &operator>>(std::istream &_in,
877  {
878  Angle roll, pitch, yaw;
879 
880  // Skip white spaces
881  _in.setf(std::ios_base::skipws);
882  _in >> roll >> pitch >> yaw;
883 
884  _q.Euler(Vector3<T>(*roll, *pitch, *yaw));
885 
886  return _in;
887  }
888 
890  private: T qw;
891 
893  private: T qx;
894 
896  private: T qy;
897 
899  private: T qz;
900  };
901 
902  template<typename T> const Quaternion<T>
903  Quaternion<T>::Identity(1, 0, 0, 0);
904 
908  }
909 }
910 #endif
An angle and related functions.
Definition: Angle.hh:44
Quaternion< double > Quaterniond
Definition: Quaternion.hh:905
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Quaternion.hh:649
void Y(T _v)
Set the y component.
Definition: Quaternion.hh:839
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:832
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:295
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:250
Vector3< T > operator*(const Vector3< T > &_v) const
Vector3 multiplication operator.
Definition: Quaternion.hh:497
bool operator==(const Quaternion< T > &_qt) const
Equal to operator.
Definition: Quaternion.hh:512
void Z(T _v)
Set the z component.
Definition: Quaternion.hh:846
#define IGN_DBL_MIN
Double min value. This value will be similar to 2.22507e-308.
Definition: Helpers.hh:35
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
Subtraction operator.
Definition: Quaternion.hh:448
T & X()
Get a mutable x component.
Definition: Quaternion.hh:811
T Roll() const
Get the Euler roll angle in radians.
Definition: Quaternion.hh:372
Quaternion< float > Quaternionf
Definition: Quaternion.hh:906
friend std::istream & operator>>(std::istream &_in, ignition::math::Quaternion< T > &_q)
Stream extraction operator.
Definition: Quaternion.hh:875
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:676
const T & Y() const
Get the y component.
Definition: Quaternion.hh:789
T & Y()
Get a mutable y component.
Definition: Quaternion.hh:818
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition: Quaternion.hh:317
T Pitch() const
Get the Euler pitch angle in radians.
Definition: Quaternion.hh:379
T X() const
Get the x value.
Definition: Vector3.hh:560
Quaternion< T > operator*(const T &_f) const
Multiplication operator by a scalar.
Definition: Quaternion.hh:479
const T & Z() const
Get the z component.
Definition: Quaternion.hh:796
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition: Quaternion.hh:438
void Correct()
Correct any nan values in this quaternion.
Definition: Quaternion.hh:574
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:615
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition: Quaternion.hh:353
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition: Quaternion.hh:660
Quaternion< T > & operator=(const Quaternion< T > &_qt)
Equal operator.
Definition: Quaternion.hh:92
T Y() const
Get the y value.
Definition: Vector3.hh:567
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:552
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:862
Vector3< T > XAxis() const
Return the X axis.
Definition: Quaternion.hh:598
Quaternion< T > operator*(const Quaternion< T > &_q) const
Multiplication operator.
Definition: Quaternion.hh:467
T & W()
Get a mutable w component.
Definition: Quaternion.hh:804
T Z() const
Get the z value.
Definition: Vector3.hh:574
void Scale(T _scale)
Scale a Quaternion&lt;T&gt;ion.
Definition: Quaternion.hh:412
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:695
void ToAxis(Vector3< T > &_axis, T &_angle) const
Return rotation as axis and angle.
Definition: Quaternion.hh:394
Quaternion< T > operator-() const
Unary minus operator.
Definition: Quaternion.hh:533
Quaternion< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition: Quaternion.hh:428
T & Z()
Get a mutable z component.
Definition: Quaternion.hh:825
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: Quaternion.hh:563
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition: Quaternion.hh:523
void W(T _v)
Set the w component.
Definition: Quaternion.hh:853
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:123
Quaternion< T > operator*=(const Quaternion< T > &qt)
Multiplication operator.
Definition: Quaternion.hh:488
Quaternion< int > Quaternioni
Definition: Quaternion.hh:907
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:286
T Yaw() const
Get the Euler yaw angle in radians.
Definition: Quaternion.hh:386
Quaternion()
Default Constructor.
Definition: Quaternion.hh:37
const T & W() const
Get the w component.
Definition: Quaternion.hh:775
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:238
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)
Subtraction operator.
Definition: Quaternion.hh:458
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:79
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition: Quaternion.hh:541
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition: Quaternion.hh:57
Quaternion< T > Integrate(const Vector3< T > &_angularVelocity, const T _deltaT) const
Integrate quaternion for constant angular velocity vector along specified interval _deltaT...
Definition: Quaternion.hh:749
Quaternion< T > Log() const
Return the logarithm.
Definition: Quaternion.hh:143
Vector3< T > ZAxis() const
Return the Z axis.
Definition: Quaternion.hh:632
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition: Quaternion.hh:365
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:782