Gazebo Math

API Reference

6.15.1
gz/math/Quaternion.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012 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 GZ_MATH_QUATERNION_HH_
18 #define GZ_MATH_QUATERNION_HH_
19 
20 #include <gz/math/Helpers.hh>
21 #include <gz/math/Angle.hh>
22 #include <gz/math/Vector3.hh>
23 #include <gz/math/Matrix3.hh>
24 #include <gz/math/config.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
30  // Inline bracket to help doxygen filtering.
31  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
32  //
33  template <typename T> class Matrix3;
34 
37  template<typename T>
38  class Quaternion
39  {
41  public: static const Quaternion Identity;
42 
44  public: static const Quaternion Zero;
45 
47  public: Quaternion()
48  : qw(1), qx(0), qy(0), qz(0)
49  {
50  // quaternion not normalized, because that breaks
51  // Pose::CoordPositionAdd(...)
52  }
53 
59  public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
60  : qw(_w), qx(_x), qy(_y), qz(_z)
61  {}
62 
67  public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
68  {
69  this->Euler(Vector3<T>(_roll, _pitch, _yaw));
70  }
71 
75  public: Quaternion(const Vector3<T> &_axis, const T &_angle)
76  {
77  this->Axis(_axis, _angle);
78  }
79 
82  public: explicit Quaternion(const Vector3<T> &_rpy)
83  {
84  this->Euler(_rpy);
85  }
86 
90  public: explicit Quaternion(const Matrix3<T> &_mat)
91  {
92  this->Matrix(_mat);
93  }
94 
97  public: Quaternion(const Quaternion<T> &_qt)
98  {
99  this->qw = _qt.qw;
100  this->qx = _qt.qx;
101  this->qy = _qt.qy;
102  this->qz = _qt.qz;
103  }
104 
106  public: ~Quaternion() {}
107 
110  public: Quaternion<T> &operator=(const Quaternion<T> &_qt)
111  {
112  this->qw = _qt.qw;
113  this->qx = _qt.qx;
114  this->qy = _qt.qy;
115  this->qz = _qt.qz;
116 
117  return *this;
118  }
119 
121  public: void Invert()
122  {
123  this->Normalize();
124  // this->qw = this->qw;
125  this->qx = -this->qx;
126  this->qy = -this->qy;
127  this->qz = -this->qz;
128  }
129 
132  public: inline Quaternion<T> Inverse() const
133  {
134  T s = 0;
135  Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
136 
137  // use s to test if quaternion is valid
138  s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
139 
140  if (equal<T>(s, static_cast<T>(0)))
141  {
142  q.qw = 1.0;
143  q.qx = 0.0;
144  q.qy = 0.0;
145  q.qz = 0.0;
146  }
147  else
148  {
149  // deal with non-normalized quaternion
150  // div by s so q * qinv = identity
151  q.qw = q.qw / s;
152  q.qx = -q.qx / s;
153  q.qy = -q.qy / s;
154  q.qz = -q.qz / s;
155  }
156  return q;
157  }
158 
161  public: Quaternion<T> Log() const
162  {
163  // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
164  // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
165  // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
166 
167  Quaternion<T> result;
168  result.qw = 0.0;
169 
170  if (std::abs(this->qw) < 1.0)
171  {
172  T fAngle = acos(this->qw);
173  T fSin = sin(fAngle);
174  if (std::abs(fSin) >= 1e-3)
175  {
176  T fCoeff = fAngle/fSin;
177  result.qx = fCoeff*this->qx;
178  result.qy = fCoeff*this->qy;
179  result.qz = fCoeff*this->qz;
180  return result;
181  }
182  }
183 
184  result.qx = this->qx;
185  result.qy = this->qy;
186  result.qz = this->qz;
187 
188  return result;
189  }
190 
193  public: Quaternion<T> Exp() const
194  {
195  // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
196  // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
197  // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
198 
199  T fAngle = sqrt(this->qx*this->qx+
200  this->qy*this->qy+this->qz*this->qz);
201  T fSin = sin(fAngle);
202 
203  Quaternion<T> result;
204  result.qw = cos(fAngle);
205 
206  if (std::abs(fSin) >= 1e-3)
207  {
208  T fCoeff = fSin/fAngle;
209  result.qx = fCoeff*this->qx;
210  result.qy = fCoeff*this->qy;
211  result.qz = fCoeff*this->qz;
212  }
213  else
214  {
215  result.qx = this->qx;
216  result.qy = this->qy;
217  result.qz = this->qz;
218  }
219 
220  return result;
221  }
222 
224  public: void Normalize()
225  {
226  T s = 0;
227 
228  s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
229  this->qy * this->qy + this->qz * this->qz));
230 
231  if (equal<T>(s, static_cast<T>(0)))
232  {
233  this->qw = T(1.0);
234  this->qx = T(0.0);
235  this->qy = T(0.0);
236  this->qz = T(0.0);
237  }
238  else
239  {
240  this->qw /= s;
241  this->qx /= s;
242  this->qy /= s;
243  this->qz /= s;
244  }
245  }
246 
249  public: Quaternion<T> Normalized() const
250  {
251  Quaternion<T> result = *this;
252  result.Normalize();
253  return result;
254  }
255 
261  public: void Axis(T _ax, T _ay, T _az, T _aa)
262  {
263  T l;
264 
265  l = _ax * _ax + _ay * _ay + _az * _az;
266 
267  if (equal<T>(l, static_cast<T>(0)))
268  {
269  this->qw = 1;
270  this->qx = 0;
271  this->qy = 0;
272  this->qz = 0;
273  }
274  else
275  {
276  _aa *= 0.5;
277  l = sin(_aa) / sqrt(l);
278  this->qw = cos(_aa);
279  this->qx = _ax * l;
280  this->qy = _ay * l;
281  this->qz = _az * l;
282  }
283 
284  this->Normalize();
285  }
286 
290  public: void Axis(const Vector3<T> &_axis, T _a)
291  {
292  this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a);
293  }
294 
300  public: void Set(T _w, T _x, T _y, T _z)
301  {
302  this->qw = _w;
303  this->qx = _x;
304  this->qy = _y;
305  this->qz = _z;
306  }
307 
313  public: void Euler(const Vector3<T> &_vec)
314  {
315  this->Euler(_vec.X(), _vec.Y(), _vec.Z());
316  }
317 
322  public: void Euler(T _roll, T _pitch, T _yaw)
323  {
324  T phi, the, psi;
325 
326  phi = _roll / T(2.0);
327  the = _pitch / T(2.0);
328  psi = _yaw / T(2.0);
329 
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));
338 
339  this->Normalize();
340  }
341 
344  public: Vector3<T> Euler() const
345  {
346  Vector3<T> vec;
347 
348  T tol = static_cast<T>(1e-15);
349 
350  Quaternion<T> copy = *this;
351  T squ;
352  T sqx;
353  T sqy;
354  T sqz;
355 
356  copy.Normalize();
357 
358  squ = copy.qw * copy.qw;
359  sqx = copy.qx * copy.qx;
360  sqy = copy.qy * copy.qy;
361  sqz = copy.qz * copy.qz;
362 
363  // Pitch
364  T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
365  if (sarg <= T(-1.0))
366  {
367  vec.Y(T(-0.5*IGN_PI));
368  }
369  else if (sarg >= T(1.0))
370  {
371  vec.Y(T(0.5*IGN_PI));
372  }
373  else
374  {
375  vec.Y(T(asin(sarg)));
376  }
377 
378  // If the pitch angle is PI/2 or -PI/2, we can only compute
379  // the sum roll + yaw. However, any combination that gives
380  // the right sum will produce the correct orientation, so we
381  // set yaw = 0 and compute roll.
382  // pitch angle is PI/2
383  if (std::abs(sarg - 1) < tol)
384  {
385  vec.Z(0);
386  vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
387  squ - sqx + sqy - sqz)));
388  }
389  // pitch angle is -PI/2
390  else if (std::abs(sarg + 1) < tol)
391  {
392  vec.Z(0);
393  vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
394  squ - sqx + sqy - sqz)));
395  }
396  else
397  {
398  // Roll
399  vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
400  squ - sqx - sqy + sqz)));
401 
402  // Yaw
403  vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
404  squ + sqx - sqy - sqz)));
405  }
406 
407  return vec;
408  }
409 
413  public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
414  {
415  Quaternion<T> result;
416  result.Euler(_vec);
417  return result;
418  }
419 
425  public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
426  {
427  return EulerToQuaternion(Vector3<T>(_x, _y, _z));
428  }
429 
432  public: T Roll() const
433  {
434  return this->Euler().X();
435  }
436 
439  public: T Pitch() const
440  {
441  return this->Euler().Y();
442  }
443 
446  public: T Yaw() const
447  {
448  return this->Euler().Z();
449  }
450 
454  public: void ToAxis(Vector3<T> &_axis, T &_angle) const
455  {
456  T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
457  if (equal<T>(len, static_cast<T>(0)))
458  {
459  _angle = 0.0;
460  _axis.Set(1, 0, 0);
461  }
462  else
463  {
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);
467  }
468  }
469 
477  void Matrix(const Matrix3<T> &_mat)
478  {
479  const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
480  if (trace > 0.0000001)
481  {
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;
487  }
488  else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
489  {
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;
495  }
496  else if (_mat(1, 1) > _mat(2, 2))
497  {
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;
503  }
504  else
505  {
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;
511  }
512  }
513 
523  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
524  {
525  // generally, we utilize the fact that a quat (w, x, y, z) represents
526  // rotation of angle 2*w about axis (x, y, z)
527  //
528  // so we want to take get a vector half-way between no rotation and the
529  // double rotation, which is
530  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
531  // if _v1 and _v2 are unit quaternions
532  //
533  // since we normalize the result anyway, we can omit the division,
534  // getting the result:
535  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
536  //
537  // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
538  // is multiplied by k = norm(_v1)*norm(_v2)
539 
540  const T kCosTheta = _v1.Dot(_v2);
541  const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
542 
543  if (fabs(kCosTheta/k + 1) < 1e-6)
544  {
545  // the vectors are opposite
546  // any vector orthogonal to _v1
547  Vector3<T> other;
548  {
549  const Vector3<T> _v1Abs(_v1.Abs());
550  if (_v1Abs.X() < _v1Abs.Y())
551  {
552  if (_v1Abs.X() < _v1Abs.Z())
553  {
554  other.Set(1, 0, 0);
555  }
556  else
557  {
558  other.Set(0, 0, 1);
559  }
560  }
561  else
562  {
563  if (_v1Abs.Y() < _v1Abs.Z())
564  {
565  other.Set(0, 1, 0);
566  }
567  else
568  {
569  other.Set(0, 0, 1);
570  }
571  }
572  }
573 
574  const Vector3<T> axis(_v1.Cross(other).Normalize());
575 
576  qw = 0;
577  qx = axis.X();
578  qy = axis.Y();
579  qz = axis.Z();
580  }
581  else
582  {
583  // the vectors are in general position
584  const Vector3<T> axis(_v1.Cross(_v2));
585  qw = kCosTheta + k;
586  qx = axis.X();
587  qy = axis.Y();
588  qz = axis.Z();
589  this->Normalize();
590  }
591  }
592 
595  public: void Scale(T _scale)
596  {
597  Quaternion<T> b;
598  Vector3<T> axis;
599  T angle;
600 
601  // Convert to axis-and-angle
602  this->ToAxis(axis, angle);
603  angle *= _scale;
604 
605  this->Axis(axis.X(), axis.Y(), axis.Z(), angle);
606  }
607 
611  public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
612  {
613  Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
614  this->qy + _qt.qy, this->qz + _qt.qz);
615  return result;
616  }
617 
622  {
623  *this = *this + _qt;
624 
625  return *this;
626  }
627 
631  public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
632  {
633  Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
634  this->qy - _qt.qy, this->qz - _qt.qz);
635  return result;
636  }
637 
642  {
643  *this = *this - _qt;
644  return *this;
645  }
646 
650  public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
651  {
652  return Quaternion<T>(
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);
657  }
658 
662  public: Quaternion<T> operator*(const T &_f) const
663  {
664  return Quaternion<T>(this->qw*_f, this->qx*_f,
665  this->qy*_f, this->qz*_f);
666  }
667 
672  {
673  *this = *this * _qt;
674  return *this;
675  }
676 
680  public: Vector3<T> operator*(const Vector3<T> &_v) const
681  {
682  Vector3<T> uv, uuv;
683  Vector3<T> qvec(this->qx, this->qy, this->qz);
684  uv = qvec.Cross(_v);
685  uuv = qvec.Cross(uv);
686  uv *= (2.0f * this->qw);
687  uuv *= 2.0f;
688 
689  return _v + uv + uuv;
690  }
691 
697  public: bool Equal(const Quaternion<T> &_qt, const T &_tol) const
698  {
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);
703  }
704 
708  public: bool operator==(const Quaternion<T> &_qt) const
709  {
710  return this->Equal(_qt, static_cast<T>(0.001));
711  }
712 
716  public: bool operator!=(const Quaternion<T> &_qt) const
717  {
718  return !(*this == _qt);
719  }
720 
723  public: Quaternion<T> operator-() const
724  {
725  return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
726  }
727 
731  public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
732  {
733  Quaternion<T> tmp(static_cast<T>(0),
734  _vec.X(), _vec.Y(), _vec.Z());
735  tmp = (*this) * (tmp * this->Inverse());
736  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
737  }
738 
742  public: Vector3<T> RotateVectorReverse(const Vector3<T> &_vec) const
743  {
744  Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
745 
746  tmp = this->Inverse() * (tmp * (*this));
747 
748  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
749  }
750 
753  public: bool IsFinite() const
754  {
755  // std::isfinite works with floating point values, need to explicit
756  // cast to avoid ambiguity in vc++.
757  return std::isfinite(static_cast<double>(this->qw)) &&
758  std::isfinite(static_cast<double>(this->qx)) &&
759  std::isfinite(static_cast<double>(this->qy)) &&
760  std::isfinite(static_cast<double>(this->qz));
761  }
762 
764  public: inline void Correct()
765  {
766  // std::isfinite works with floating point values, need to explicit
767  // cast to avoid ambiguity in vc++.
768  if (!std::isfinite(static_cast<double>(this->qx)))
769  this->qx = 0;
770  if (!std::isfinite(static_cast<double>(this->qy)))
771  this->qy = 0;
772  if (!std::isfinite(static_cast<double>(this->qz)))
773  this->qz = 0;
774  if (!std::isfinite(static_cast<double>(this->qw)))
775  this->qw = 1;
776 
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)))
781  {
782  this->qw = 1;
783  }
784  }
785 
788  public: Vector3<T> XAxis() const
789  {
790  T fTy = 2.0f*this->qy;
791  T fTz = 2.0f*this->qz;
792 
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;
799 
800  return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
801  }
802 
805  public: Vector3<T> YAxis() const
806  {
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;
816 
817  return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
818  }
819 
822  public: Vector3<T> ZAxis() const
823  {
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;
833 
834  return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
835  }
836 
839  public: void Round(int _precision)
840  {
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);
845  }
846 
850  public: T Dot(const Quaternion<T> &_q) const
851  {
852  return this->qw*_q.qw + this->qx * _q.qx +
853  this->qy*_q.qy + this->qz*_q.qz;
854  }
855 
866  public: static Quaternion<T> Squad(T _fT,
867  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
868  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
869  bool _shortestPath = false)
870  {
871  T fSlerpT = 2.0f*_fT*(1.0f-_fT);
872  Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
873  Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
874  return Slerp(fSlerpT, kSlerpP, kSlerpQ);
875  }
876 
885  public: static Quaternion<T> Slerp(T _fT,
886  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
887  bool _shortestPath = false)
888  {
889  T fCos = _rkP.Dot(_rkQ);
890  Quaternion<T> rkT;
891 
892  // Do we need to invert rotation?
893  if (fCos < 0.0f && _shortestPath)
894  {
895  fCos = -fCos;
896  rkT = -_rkQ;
897  }
898  else
899  {
900  rkT = _rkQ;
901  }
902 
903  if (std::abs(fCos) < 1 - 1e-03)
904  {
905  // Standard case (slerp)
906  T fSin = sqrt(1 - (fCos*fCos));
907  T fAngle = atan2(fSin, fCos);
908  // FIXME: should check if (std::abs(fSin) >= 1e-3)
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;
913  }
914  else
915  {
916  // There are two situations:
917  // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
918  // so we can do a linear interpolation safely.
919  // 2. "rkP" and "rkQ" are almost inverse of each
920  // other (fCos ~= -1), there
921  // are an infinite number of possibilities interpolation.
922  // but we haven't have method to fix this case, so just use
923  // linear interpolation here.
924  Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
925  // taking the complement requires renormalisation
926  t.Normalize();
927  return t;
928  }
929  }
930 
939  public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
940  const T _deltaT) const
941  {
942  Quaternion<T> deltaQ;
943  Vector3<T> theta = _angularVelocity * _deltaT / 2;
944  T thetaMagSq = theta.SquaredLength();
945  T s;
946  if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
947  {
948  deltaQ.W() = 1.0 - thetaMagSq / 2.0;
949  s = 1.0 - thetaMagSq / 6.0;
950  }
951  else
952  {
953  double thetaMag = sqrt(thetaMagSq);
954  deltaQ.W() = cos(thetaMag);
955  s = sin(thetaMag) / thetaMag;
956  }
957  deltaQ.X() = theta.X() * s;
958  deltaQ.Y() = theta.Y() * s;
959  deltaQ.Z() = theta.Z() * s;
960  return deltaQ * (*this);
961  }
962 
965  public: inline const T &W() const
966  {
967  return this->qw;
968  }
969 
972  public: inline const T &X() const
973  {
974  return this->qx;
975  }
976 
979  public: inline const T &Y() const
980  {
981  return this->qy;
982  }
983 
986  public: inline const T &Z() const
987  {
988  return this->qz;
989  }
990 
991 
994  public: inline T &W()
995  {
996  return this->qw;
997  }
998 
1001  public: inline T &X()
1002  {
1003  return this->qx;
1004  }
1005 
1008  public: inline T &Y()
1009  {
1010  return this->qy;
1011  }
1012 
1015  public: inline T &Z()
1016  {
1017  return this->qz;
1018  }
1019 
1022  public: inline void X(T _v)
1023  {
1024  this->qx = _v;
1025  }
1026 
1029  public: inline void Y(T _v)
1030  {
1031  this->qy = _v;
1032  }
1033 
1036  public: inline void Z(T _v)
1037  {
1038  this->qz = _v;
1039  }
1040 
1043  public: inline void W(T _v)
1044  {
1045  this->qw = _v;
1046  }
1047 
1052  public: friend std::ostream &operator<<(std::ostream &_out,
1053  const gz::math::Quaternion<T> &_q)
1054  {
1055  Vector3<T> v(_q.Euler());
1056  _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " "
1057  << precision(v.Z(), 6);
1058  return _out;
1059  }
1060 
1065  public: friend std::istream &operator>>(std::istream &_in,
1066  gz::math::Quaternion<T> &_q)
1067  {
1068  Angle roll, pitch, yaw;
1069 
1070  // Skip white spaces
1071  _in.setf(std::ios_base::skipws);
1072  _in >> roll >> pitch >> yaw;
1073 
1074  if (!_in.fail())
1075  {
1076  _q.Euler(Vector3<T>(*roll, *pitch, *yaw));
1077  }
1078 
1079  return _in;
1080  }
1081 
1083  private: T qw;
1084 
1086  private: T qx;
1087 
1089  private: T qy;
1090 
1092  private: T qz;
1093  };
1094 
1095  template<typename T> const Quaternion<T>
1096  Quaternion<T>::Identity(1, 0, 0, 0);
1097 
1098  template<typename T> const Quaternion<T>
1099  Quaternion<T>::Zero(0, 0, 0, 0);
1100 
1104  }
1105  }
1106 }
1107 #endif
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 isfinite(T... args)
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
STL class.
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
T setf(T... args)
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
T fail(T... args)
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
STL class.
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