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_MATH_QUATERNION_HH_
18 #define IGNITION_MATH_QUATERNION_HH_
19 
20 #include <ignition/math/Helpers.hh>
21 #include <ignition/math/Angle.hh>
22 #include <ignition/math/Vector3.hh>
23 #include <ignition/math/Matrix3.hh>
24 
25 namespace ignition
26 {
27  namespace math
28  {
29  template <typename T> class Matrix3;
30 
33  template<typename T>
34  class Quaternion
35  {
37  public: static const Quaternion Identity;
38 
40  public: static const Quaternion Zero;
41 
43  public: Quaternion()
44  : qw(1), qx(0), qy(0), qz(0)
45  {
46  // quaternion not normalized, because that breaks
47  // Pose::CoordPositionAdd(...)
48  }
49 
55  public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
56  : qw(_w), qx(_x), qy(_y), qz(_z)
57  {}
58 
63  public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
64  {
65  this->Euler(Vector3<T>(_roll, _pitch, _yaw));
66  }
67 
71  public: Quaternion(const Vector3<T> &_axis, const T &_angle)
72  {
73  this->Axis(_axis, _angle);
74  }
75 
78  public: explicit Quaternion(const Vector3<T> &_rpy)
79  {
80  this->Euler(_rpy);
81  }
82 
86  public: explicit Quaternion(const Matrix3<T> &_mat)
87  {
88  this->Matrix(_mat);
89  }
90 
93  public: Quaternion(const Quaternion<T> &_qt)
94  {
95  this->qw = _qt.qw;
96  this->qx = _qt.qx;
97  this->qy = _qt.qy;
98  this->qz = _qt.qz;
99  }
100 
102  public: ~Quaternion() {}
103 
106  public: Quaternion<T> &operator=(const Quaternion<T> &_qt)
107  {
108  this->qw = _qt.qw;
109  this->qx = _qt.qx;
110  this->qy = _qt.qy;
111  this->qz = _qt.qz;
112 
113  return *this;
114  }
115 
117  public: void Invert()
118  {
119  this->Normalize();
120  // this->qw = this->qw;
121  this->qx = -this->qx;
122  this->qy = -this->qy;
123  this->qz = -this->qz;
124  }
125 
128  public: inline Quaternion<T> Inverse() const
129  {
130  T s = 0;
131  Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
132 
133  // use s to test if quaternion is valid
134  s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
135 
136  if (equal<T>(s, static_cast<T>(0)))
137  {
138  q.qw = 1.0;
139  q.qx = 0.0;
140  q.qy = 0.0;
141  q.qz = 0.0;
142  }
143  else
144  {
145  // deal with non-normalized quaternion
146  // div by s so q * qinv = identity
147  q.qw = q.qw / s;
148  q.qx = -q.qx / s;
149  q.qy = -q.qy / s;
150  q.qz = -q.qz / s;
151  }
152  return q;
153  }
154 
157  public: Quaternion<T> Log() const
158  {
159  // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
160  // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
161  // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
162 
163  Quaternion<T> result;
164  result.qw = 0.0;
165 
166  if (std::abs(this->qw) < 1.0)
167  {
168  T fAngle = acos(this->qw);
169  T fSin = sin(fAngle);
170  if (std::abs(fSin) >= 1e-3)
171  {
172  T fCoeff = fAngle/fSin;
173  result.qx = fCoeff*this->qx;
174  result.qy = fCoeff*this->qy;
175  result.qz = fCoeff*this->qz;
176  return result;
177  }
178  }
179 
180  result.qx = this->qx;
181  result.qy = this->qy;
182  result.qz = this->qz;
183 
184  return result;
185  }
186 
189  public: Quaternion<T> Exp() const
190  {
191  // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
192  // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
193  // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
194 
195  T fAngle = sqrt(this->qx*this->qx+
196  this->qy*this->qy+this->qz*this->qz);
197  T fSin = sin(fAngle);
198 
199  Quaternion<T> result;
200  result.qw = cos(fAngle);
201 
202  if (std::abs(fSin) >= 1e-3)
203  {
204  T fCoeff = fSin/fAngle;
205  result.qx = fCoeff*this->qx;
206  result.qy = fCoeff*this->qy;
207  result.qz = fCoeff*this->qz;
208  }
209  else
210  {
211  result.qx = this->qx;
212  result.qy = this->qy;
213  result.qz = this->qz;
214  }
215 
216  return result;
217  }
218 
220  public: void Normalize()
221  {
222  T s = 0;
223 
224  s = sqrt(this->qw * this->qw + this->qx * this->qx +
225  this->qy * this->qy + this->qz * this->qz);
226 
227  if (equal<T>(s, static_cast<T>(0)))
228  {
229  this->qw = 1.0;
230  this->qx = 0.0;
231  this->qy = 0.0;
232  this->qz = 0.0;
233  }
234  else
235  {
236  this->qw /= s;
237  this->qx /= s;
238  this->qy /= s;
239  this->qz /= s;
240  }
241  }
242 
248  public: void Axis(T _ax, T _ay, T _az, T _aa)
249  {
250  T l;
251 
252  l = _ax * _ax + _ay * _ay + _az * _az;
253 
254  if (equal<T>(l, static_cast<T>(0)))
255  {
256  this->qw = 1;
257  this->qx = 0;
258  this->qy = 0;
259  this->qz = 0;
260  }
261  else
262  {
263  _aa *= 0.5;
264  l = sin(_aa) / sqrt(l);
265  this->qw = cos(_aa);
266  this->qx = _ax * l;
267  this->qy = _ay * l;
268  this->qz = _az * l;
269  }
270 
271  this->Normalize();
272  }
273 
277  public: void Axis(const Vector3<T> &_axis, T _a)
278  {
279  this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a);
280  }
281 
287  public: void Set(T _w, T _x, T _y, T _z)
288  {
289  this->qw = _w;
290  this->qx = _x;
291  this->qy = _y;
292  this->qz = _z;
293  }
294 
300  public: void Euler(const Vector3<T> &_vec)
301  {
302  this->Euler(_vec.X(), _vec.Y(), _vec.Z());
303  }
304 
309  public: void Euler(T _roll, T _pitch, T _yaw)
310  {
311  T phi, the, psi;
312 
313  phi = _roll / 2.0;
314  the = _pitch / 2.0;
315  psi = _yaw / 2.0;
316 
317  this->qw = cos(phi) * cos(the) * cos(psi) +
318  sin(phi) * sin(the) * sin(psi);
319  this->qx = sin(phi) * cos(the) * cos(psi) -
320  cos(phi) * sin(the) * sin(psi);
321  this->qy = cos(phi) * sin(the) * cos(psi) +
322  sin(phi) * cos(the) * sin(psi);
323  this->qz = cos(phi) * cos(the) * sin(psi) -
324  sin(phi) * sin(the) * cos(psi);
325 
326  this->Normalize();
327  }
328 
331  public: Vector3<T> Euler() const
332  {
333  Vector3<T> vec;
334 
335  T tol = static_cast<T>(1e-15);
336 
337  Quaternion<T> copy = *this;
338  T squ;
339  T sqx;
340  T sqy;
341  T sqz;
342 
343  copy.Normalize();
344 
345  squ = copy.qw * copy.qw;
346  sqx = copy.qx * copy.qx;
347  sqy = copy.qy * copy.qy;
348  sqz = copy.qz * copy.qz;
349 
350  // Pitch
351  T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
352  vec.Y(sarg <= -1.0 ? -0.5*IGN_PI :
353  (sarg >= 1.0 ? 0.5*IGN_PI : asin(sarg)));
354 
355  // If the pitch angle is PI/2 or -PI/2, we can only compute
356  // the sum roll + yaw. However, any combination that gives
357  // the right sum will produce the correct orientation, so we
358  // set yaw = 0 and compute roll.
359  // pitch angle is PI/2
360  if (std::abs(sarg - 1) < tol)
361  {
362  vec.Z(0);
363  vec.X(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
364  squ - sqx + sqy - sqz));
365  }
366  // pitch angle is -PI/2
367  else if (std::abs(sarg + 1) < tol)
368  {
369  vec.Z(0);
370  vec.X(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
371  squ - sqx + sqy - sqz));
372  }
373  else
374  {
375  // Roll
376  vec.X(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
377  squ - sqx - sqy + sqz));
378 
379  // Yaw
380  vec.Z(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
381  squ + sqx - sqy - sqz));
382  }
383 
384  return vec;
385  }
386 
390  public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
391  {
392  Quaternion<T> result;
393  result.Euler(_vec);
394  return result;
395  }
396 
402  public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
403  {
404  return EulerToQuaternion(Vector3<T>(_x, _y, _z));
405  }
406 
409  public: T Roll() const
410  {
411  return this->Euler().X();
412  }
413 
416  public: T Pitch() const
417  {
418  return this->Euler().Y();
419  }
420 
423  public: T Yaw() const
424  {
425  return this->Euler().Z();
426  }
427 
431  public: void ToAxis(Vector3<T> &_axis, T &_angle) const
432  {
433  T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
434  if (equal<T>(len, static_cast<T>(0)))
435  {
436  _angle = 0.0;
437  _axis.Set(1, 0, 0);
438  }
439  else
440  {
441  _angle = 2.0 * acos(this->qw);
442  T invLen = 1.0 / sqrt(len);
443  _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
444  }
445  }
446 
454  void Matrix(const Matrix3<T> &_mat)
455  {
456  const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
457  if (trace > 0.0000001)
458  {
459  qw = sqrt(1 + trace) / 2;
460  const T s = 1.0 / (4 * qw);
461  qx = (_mat(2, 1) - _mat(1, 2)) * s;
462  qy = (_mat(0, 2) - _mat(2, 0)) * s;
463  qz = (_mat(1, 0) - _mat(0, 1)) * s;
464  }
465  else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
466  {
467  qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
468  const T s = 1.0 / (4 * qx);
469  qw = (_mat(2, 1) - _mat(1, 2)) * s;
470  qy = (_mat(1, 0) + _mat(0, 1)) * s;
471  qz = (_mat(0, 2) + _mat(2, 0)) * s;
472  }
473  else if (_mat(1, 1) > _mat(2, 2))
474  {
475  qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
476  const T s = 1.0 / (4 * qy);
477  qw = (_mat(0, 2) - _mat(2, 0)) * s;
478  qx = (_mat(0, 1) + _mat(1, 0)) * s;
479  qz = (_mat(1, 2) + _mat(2, 1)) * s;
480  }
481  else
482  {
483  qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
484  const T s = 1.0 / (4 * qz);
485  qw = (_mat(1, 0) - _mat(0, 1)) * s;
486  qx = (_mat(0, 2) + _mat(2, 0)) * s;
487  qy = (_mat(1, 2) + _mat(2, 1)) * s;
488  }
489  }
490 
500  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
501  {
502  // generally, we utilize the fact that a quat (w, x, y, z) represents
503  // rotation of angle 2*w about axis (x, y, z)
504  //
505  // so we want to take get a vector half-way between no rotation and the
506  // double rotation, which is
507  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
508  // if _v1 and _v2 are unit quaternions
509  //
510  // since we normalize the result anyway, we can omit the division,
511  // getting the result:
512  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
513  //
514  // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
515  // is multiplied by k = norm(_v1)*norm(_v2)
516 
517  const T kCosTheta = _v1.Dot(_v2);
518  const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
519 
520  if (fabs(kCosTheta/k + 1) < 1e-6)
521  {
522  // the vectors are opposite
523  // any vector orthogonal to _v1
524  Vector3<T> other;
525  {
526  const Vector3<T> _v1Abs(_v1.Abs());
527  if (_v1Abs.X() < _v1Abs.Y())
528  {
529  if (_v1Abs.X() < _v1Abs.Z())
530  {
531  other.Set(1, 0, 0);
532  }
533  else
534  {
535  other.Set(0, 0, 1);
536  }
537  }
538  else
539  {
540  if (_v1Abs.Y() < _v1Abs.Z())
541  {
542  other.Set(0, 1, 0);
543  }
544  else
545  {
546  other.Set(0, 0, 1);
547  }
548  }
549  }
550 
551  const Vector3<T> axis(_v1.Cross(other).Normalize());
552 
553  qw = 0;
554  qx = axis.X();
555  qy = axis.Y();
556  qz = axis.Z();
557  }
558  else
559  {
560  // the vectors are in general position
561  const Vector3<T> axis(_v1.Cross(_v2));
562  qw = kCosTheta + k;
563  qx = axis.X();
564  qy = axis.Y();
565  qz = axis.Z();
566  this->Normalize();
567  }
568  }
569 
572  public: void Scale(T _scale)
573  {
574  Quaternion<T> b;
575  Vector3<T> axis;
576  T angle;
577 
578  // Convert to axis-and-angle
579  this->ToAxis(axis, angle);
580  angle *= _scale;
581 
582  this->Axis(axis.X(), axis.Y(), axis.Z(), angle);
583  }
584 
588  public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
589  {
590  Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
591  this->qy + _qt.qy, this->qz + _qt.qz);
592  return result;
593  }
594 
599  {
600  *this = *this + _qt;
601 
602  return *this;
603  }
604 
608  public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
609  {
610  Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
611  this->qy - _qt.qy, this->qz - _qt.qz);
612  return result;
613  }
614 
619  {
620  *this = *this - _qt;
621  return *this;
622  }
623 
627  public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
628  {
629  return Quaternion<T>(
630  this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
631  this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
632  this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
633  this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
634  }
635 
639  public: Quaternion<T> operator*(const T &_f) const
640  {
641  return Quaternion<T>(this->qw*_f, this->qx*_f,
642  this->qy*_f, this->qz*_f);
643  }
644 
649  {
650  *this = *this * qt;
651  return *this;
652  }
653 
657  public: Vector3<T> operator*(const Vector3<T> &_v) const
658  {
659  Vector3<T> uv, uuv;
660  Vector3<T> qvec(this->qx, this->qy, this->qz);
661  uv = qvec.Cross(_v);
662  uuv = qvec.Cross(uv);
663  uv *= (2.0f * this->qw);
664  uuv *= 2.0f;
665 
666  return _v + uv + uuv;
667  }
668 
672  public: bool operator==(const Quaternion<T> &_qt) const
673  {
674  return equal(this->qx, _qt.qx, static_cast<T>(0.001)) &&
675  equal(this->qy, _qt.qy, static_cast<T>(0.001)) &&
676  equal(this->qz, _qt.qz, static_cast<T>(0.001)) &&
677  equal(this->qw, _qt.qw, static_cast<T>(0.001));
678  }
679 
683  public: bool operator!=(const Quaternion<T> &_qt) const
684  {
685  return !equal(this->qx, _qt.qx, static_cast<T>(0.001)) ||
686  !equal(this->qy, _qt.qy, static_cast<T>(0.001)) ||
687  !equal(this->qz, _qt.qz, static_cast<T>(0.001)) ||
688  !equal(this->qw, _qt.qw, static_cast<T>(0.001));
689  }
690 
693  public: Quaternion<T> operator-() const
694  {
695  return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
696  }
697 
701  public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
702  {
703  Quaternion<T> tmp(static_cast<T>(0),
704  _vec.X(), _vec.Y(), _vec.Z());
705  tmp = (*this) * (tmp * this->Inverse());
706  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
707  }
708 
713  {
714  Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
715 
716  tmp = this->Inverse() * (tmp * (*this));
717 
718  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
719  }
720 
723  public: bool IsFinite() const
724  {
725  // std::isfinite works with floating point values, need to explicit
726  // cast to avoid ambiguity in vc++.
727  return std::isfinite(static_cast<double>(this->qw)) &&
728  std::isfinite(static_cast<double>(this->qx)) &&
729  std::isfinite(static_cast<double>(this->qy)) &&
730  std::isfinite(static_cast<double>(this->qz));
731  }
732 
734  public: inline void Correct()
735  {
736  // std::isfinite works with floating point values, need to explicit
737  // cast to avoid ambiguity in vc++.
738  if (!std::isfinite(static_cast<double>(this->qx)))
739  this->qx = 0;
740  if (!std::isfinite(static_cast<double>(this->qy)))
741  this->qy = 0;
742  if (!std::isfinite(static_cast<double>(this->qz)))
743  this->qz = 0;
744  if (!std::isfinite(static_cast<double>(this->qw)))
745  this->qw = 1;
746 
747  if (equal(this->qw, static_cast<T>(0)) &&
748  equal(this->qx, static_cast<T>(0)) &&
749  equal(this->qy, static_cast<T>(0)) &&
750  equal(this->qz, static_cast<T>(0)))
751  {
752  this->qw = 1;
753  }
754  }
755 
758  public: Vector3<T> XAxis() const
759  {
760  T fTy = 2.0f*this->qy;
761  T fTz = 2.0f*this->qz;
762 
763  T fTwy = fTy*this->qw;
764  T fTwz = fTz*this->qw;
765  T fTxy = fTy*this->qx;
766  T fTxz = fTz*this->qx;
767  T fTyy = fTy*this->qy;
768  T fTzz = fTz*this->qz;
769 
770  return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
771  }
772 
775  public: Vector3<T> YAxis() const
776  {
777  T fTx = 2.0f*this->qx;
778  T fTy = 2.0f*this->qy;
779  T fTz = 2.0f*this->qz;
780  T fTwx = fTx*this->qw;
781  T fTwz = fTz*this->qw;
782  T fTxx = fTx*this->qx;
783  T fTxy = fTy*this->qx;
784  T fTyz = fTz*this->qy;
785  T fTzz = fTz*this->qz;
786 
787  return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
788  }
789 
792  public: Vector3<T> ZAxis() const
793  {
794  T fTx = 2.0f*this->qx;
795  T fTy = 2.0f*this->qy;
796  T fTz = 2.0f*this->qz;
797  T fTwx = fTx*this->qw;
798  T fTwy = fTy*this->qw;
799  T fTxx = fTx*this->qx;
800  T fTxz = fTz*this->qx;
801  T fTyy = fTy*this->qy;
802  T fTyz = fTz*this->qy;
803 
804  return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
805  }
806 
809  public: void Round(int _precision)
810  {
811  this->qx = precision(this->qx, _precision);
812  this->qy = precision(this->qy, _precision);
813  this->qz = precision(this->qz, _precision);
814  this->qw = precision(this->qw, _precision);
815  }
816 
820  public: T Dot(const Quaternion<T> &_q) const
821  {
822  return this->qw*_q.qw + this->qx * _q.qx +
823  this->qy*_q.qy + this->qz*_q.qz;
824  }
825 
836  public: static Quaternion<T> Squad(T _fT,
837  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
838  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
839  bool _shortestPath = false)
840  {
841  T fSlerpT = 2.0f*_fT*(1.0f-_fT);
842  Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
843  Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
844  return Slerp(fSlerpT, kSlerpP, kSlerpQ);
845  }
846 
855  public: static Quaternion<T> Slerp(T _fT,
856  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
857  bool _shortestPath = false)
858  {
859  T fCos = _rkP.Dot(_rkQ);
860  Quaternion<T> rkT;
861 
862  // Do we need to invert rotation?
863  if (fCos < 0.0f && _shortestPath)
864  {
865  fCos = -fCos;
866  rkT = -_rkQ;
867  }
868  else
869  {
870  rkT = _rkQ;
871  }
872 
873  if (std::abs(fCos) < 1 - 1e-03)
874  {
875  // Standard case (slerp)
876  T fSin = sqrt(1 - (fCos*fCos));
877  T fAngle = atan2(fSin, fCos);
878  // FIXME: should check if (std::abs(fSin) >= 1e-3)
879  T fInvSin = 1.0f / fSin;
880  T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
881  T fCoeff1 = sin(_fT * fAngle) * fInvSin;
882  return _rkP * fCoeff0 + rkT * fCoeff1;
883  }
884  else
885  {
886  // There are two situations:
887  // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
888  // so we can do a linear interpolation safely.
889  // 2. "rkP" and "rkQ" are almost inverse of each
890  // other (fCos ~= -1), there
891  // are an infinite number of possibilities interpolation.
892  // but we haven't have method to fix this case, so just use
893  // linear interpolation here.
894  Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
895  // taking the complement requires renormalisation
896  t.Normalize();
897  return t;
898  }
899  }
900 
909  public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
910  const T _deltaT) const
911  {
912  Quaternion<T> deltaQ;
913  Vector3<T> theta = _angularVelocity * _deltaT * 0.5;
914  T thetaMagSq = theta.SquaredLength();
915  T s;
916  if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
917  {
918  deltaQ.W() = 1.0 - thetaMagSq / 2.0;
919  s = 1.0 - thetaMagSq / 6.0;
920  }
921  else
922  {
923  double thetaMag = sqrt(thetaMagSq);
924  deltaQ.W() = cos(thetaMag);
925  s = sin(thetaMag) / thetaMag;
926  }
927  deltaQ.X() = theta.X() * s;
928  deltaQ.Y() = theta.Y() * s;
929  deltaQ.Z() = theta.Z() * s;
930  return deltaQ * (*this);
931  }
932 
935  public: inline const T &W() const
936  {
937  return this->qw;
938  }
939 
942  public: inline const T &X() const
943  {
944  return this->qx;
945  }
946 
949  public: inline const T &Y() const
950  {
951  return this->qy;
952  }
953 
956  public: inline const T &Z() const
957  {
958  return this->qz;
959  }
960 
961 
964  public: inline T &W()
965  {
966  return this->qw;
967  }
968 
971  public: inline T &X()
972  {
973  return this->qx;
974  }
975 
978  public: inline T &Y()
979  {
980  return this->qy;
981  }
982 
985  public: inline T &Z()
986  {
987  return this->qz;
988  }
989 
992  public: inline void X(T _v)
993  {
994  this->qx = _v;
995  }
996 
999  public: inline void Y(T _v)
1000  {
1001  this->qy = _v;
1002  }
1003 
1006  public: inline void Z(T _v)
1007  {
1008  this->qz = _v;
1009  }
1010 
1013  public: inline void W(T _v)
1014  {
1015  this->qw = _v;
1016  }
1017 
1022  public: friend std::ostream &operator<<(std::ostream &_out,
1024  {
1025  Vector3<T> v(_q.Euler());
1026  _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " "
1027  << precision(v.Z(), 6);
1028  return _out;
1029  }
1030 
1035  public: friend std::istream &operator>>(std::istream &_in,
1037  {
1038  Angle roll, pitch, yaw;
1039 
1040  // Skip white spaces
1041  _in.setf(std::ios_base::skipws);
1042  _in >> roll >> pitch >> yaw;
1043 
1044  _q.Euler(Vector3<T>(*roll, *pitch, *yaw));
1045 
1046  return _in;
1047  }
1048 
1050  private: T qw;
1051 
1053  private: T qx;
1054 
1056  private: T qy;
1057 
1059  private: T qz;
1060  };
1061 
1062  template<typename T> const Quaternion<T>
1063  Quaternion<T>::Identity(1, 0, 0, 0);
1064 
1065  template<typename T> const Quaternion<T>
1066  Quaternion<T>::Zero(0, 0, 0, 0);
1067 
1071  }
1072 }
1073 #endif
An angle and related functions.
Definition: Angle.hh:44
Quaternion< double > Quaterniond
Definition: Quaternion.hh:1068
const T & W() const
Get the w component.
Definition: Quaternion.hh:935
T X() const
Get the x value.
Definition: Vector3.hh:635
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Quaternion.hh:809
void Y(T _v)
Set the y component.
Definition: Quaternion.hh:999
const T & Z() const
Get the z component.
Definition: Quaternion.hh:956
T Roll() const
Get the Euler roll angle in radians.
Definition: Quaternion.hh:409
Quaternion(const Vector3< T > &_axis, const T &_angle)
Constructor from axis angle.
Definition: Quaternion.hh:71
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:175
~Quaternion()
Destructor.
Definition: Quaternion.hh:102
void X(T _v)
Set the x component.
Definition: Quaternion.hh:992
Quaternion< T > Exp() const
Return the exponent.
Definition: Quaternion.hh:189
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:309
static const Quaternion Zero
math::Quaternion(0, 0, 0, 0)
Definition: Quaternion.hh:40
Quaternion(const Vector3< T > &_rpy)
Constructor.
Definition: Quaternion.hh:78
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:552
Quaternion< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition: Quaternion.hh:588
void Z(T _v)
Set the z component.
Definition: Quaternion.hh:1006
void Invert()
Invert the quaternion.
Definition: Quaternion.hh:117
T & X()
Get a mutable x component.
Definition: Quaternion.hh:971
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition: Quaternion.hh:701
Quaternion< float > Quaternionf
Definition: Quaternion.hh:1069
friend std::istream & operator>>(std::istream &_in, ignition::math::Quaternion< T > &_q)
Stream extraction operator.
Definition: Quaternion.hh:1035
Quaternion< T > operator-() const
Unary minus operator.
Definition: Quaternion.hh:693
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:836
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:219
T & Y()
Get a mutable y component.
Definition: Quaternion.hh:978
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition: Quaternion.hh:683
Vector3< T > ZAxis() const
Return the Z axis.
Definition: Quaternion.hh:792
void ToAxis(Vector3< T > &_axis, T &_angle) const
Return rotation as axis and angle.
Definition: Quaternion.hh:431
Quaternion< T > Log() const
Return the logarithm.
Definition: Quaternion.hh:157
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition: Quaternion.hh:598
void Correct()
Correct any nan values in this quaternion.
Definition: Quaternion.hh:734
Quaternion(const Quaternion< T > &_qt)
Copy constructor.
Definition: Quaternion.hh:93
static const double MIN_D
Double min value. This value will be similar to 2.22507e-308.
Definition: Helpers.hh:246
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition: Quaternion.hh:390
T Pitch() const
Get the Euler pitch angle in radians.
Definition: Quaternion.hh:416
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:195
Quaternion< T > & operator=(const Quaternion< T > &_qt)
Equal operator.
Definition: Quaternion.hh:106
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:120
void Axis(const Vector3< T > &_axis, T _a)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:277
T Yaw() const
Get the Euler yaw angle in radians.
Definition: Quaternion.hh:423
void Axis(T _ax, T _ay, T _az, T _aa)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:248
A 3x3 matrix class.
Definition: Matrix3.hh:35
Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
Constructor.
Definition: Quaternion.hh:55
Quaternion< T > operator*(const Quaternion< T > &_q) const
Multiplication operator.
Definition: Quaternion.hh:627
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: Quaternion.hh:37
const T & Y() const
Get the y component.
Definition: Quaternion.hh:949
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Quaternion< T > &_q)
Stream insertion operator.
Definition: Quaternion.hh:1022
bool operator==(const Quaternion< T > &_qt) const
Equal to operator.
Definition: Quaternion.hh:672
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: Quaternion.hh:723
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition: Quaternion.hh:820
T Y() const
Get the y value.
Definition: Vector3.hh:642
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.Normalize() == this * _v1.Normalize() holds.
Definition: Quaternion.hh:500
T & W()
Get a mutable w component.
Definition: Quaternion.hh:964
void Scale(T _scale)
Scale a Quaternion<T>ion.
Definition: Quaternion.hh:572
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:855
Quaternion(const Matrix3< T > &_mat)
Construct from rotation matrix.
Definition: Quaternion.hh:86
T & Z()
Get a mutable z component.
Definition: Quaternion.hh:985
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:128
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition: Quaternion.hh:331
Vector3< T > XAxis() const
Return the X axis.
Definition: Quaternion.hh:758
const T & X() const
Get the x component.
Definition: Quaternion.hh:942
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
Vector3< T > YAxis() const
Return the Y axis.
Definition: Quaternion.hh:775
Vector3< T > operator*(const Vector3< T > &_v) const
Vector3 multiplication operator.
Definition: Quaternion.hh:657
T Z() const
Get the z value.
Definition: Vector3.hh:649
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:185
void W(T _v)
Set the w component.
Definition: Quaternion.hh:1013
Quaternion< T > operator*=(const Quaternion< T > &qt)
Multiplication operator.
Definition: Quaternion.hh:648
Quaternion< T > operator-(const Quaternion< T > &_qt) const
Subtraction operator.
Definition: Quaternion.hh:608
Quaternion< int > Quaternioni
Definition: Quaternion.hh:1070
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:300
Quaternion()
Default Constructor.
Definition: Quaternion.hh:43
void Matrix(const Matrix3< T > &_mat)
Set from a rotation matrix.
Definition: Quaternion.hh:454
Definition: Angle.hh:38
Vector3< T > RotateVectorReverse(Vector3< T > _vec) const
Do the reverse rotation of a vector by this quaternion.
Definition: Quaternion.hh:712
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:518
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:220
A quaternion class.
Definition: Matrix3.hh:30
Quaternion< T > operator-=(const Quaternion< T > &_qt)
Subtraction operator.
Definition: Quaternion.hh:618
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:173
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition: Quaternion.hh:63
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:909
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition: Quaternion.hh:402
Quaternion< T > operator*(const T &_f) const
Multiplication operator by a scalar.
Definition: Quaternion.hh:639
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: Quaternion.hh:287