Gazebo Math

API Reference

7.5.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 gz::math
27 {
28  // Inline bracket to help doxygen filtering.
29  inline namespace GZ_MATH_VERSION_NAMESPACE {
30  //
31  template <typename T> class Matrix3;
32 
77  template<typename T>
78  class Quaternion
79  {
82  public: static const Quaternion &Identity;
83 
86  public: static const Quaternion &Zero;
87 
89  public: constexpr Quaternion()
90  : qw(1), qx(0), qy(0), qz(0)
91  {
92  // quaternion not normalized, because that breaks
93  // Pose::CoordPositionAdd(...)
94  }
95 
103  public: constexpr Quaternion(const T &_w, const T &_x, const T &_y,
104  const T &_z)
105  : qw(_w), qx(_x), qy(_y), qz(_z)
106  {}
107 
114  public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
115  {
116  this->SetFromEuler(Vector3<T>(_roll, _pitch, _yaw));
117  }
118 
123  public: Quaternion(const Vector3<T> &_axis, const T &_angle)
124  {
125  this->SetFromAxisAngle(_axis, _angle);
126  }
127 
131  public: explicit Quaternion(const Vector3<T> &_rpy)
132  {
133  this->SetFromEuler(_rpy);
134  }
135 
140  public: explicit Quaternion(const Matrix3<T> &_mat)
141  {
142  this->SetFromMatrix(_mat);
143  }
144 
148  public: Quaternion(const Quaternion<T> &_qt) = default;
149 
151  public: ~Quaternion() = default;
152 
155  public: Quaternion<T> &operator=(const Quaternion<T> &_qt) = default;
156 
159  public: void Invert()
160  {
161  this->Normalize();
162  // this->qw = this->qw;
163  this->qx = -this->qx;
164  this->qy = -this->qy;
165  this->qz = -this->qz;
166  }
167 
170  public: inline Quaternion<T> Inverse() const
171  {
172  T s = 0;
173  Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
174 
175  // use s to test if quaternion is valid
176  s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
177 
178  if (equal<T>(s, static_cast<T>(0)))
179  {
180  q.qw = 1.0;
181  q.qx = 0.0;
182  q.qy = 0.0;
183  q.qz = 0.0;
184  }
185  else
186  {
187  // deal with non-normalized quaternion
188  // div by s so q * qinv = identity
189  q.qw = q.qw / s;
190  q.qx = -q.qx / s;
191  q.qy = -q.qy / s;
192  q.qz = -q.qz / s;
193  }
194  return q;
195  }
196 
204  public: Quaternion<T> Log() const
205  {
206  Quaternion<T> result;
207  result.qw = 0.0;
208 
209  if (std::abs(this->qw) < 1.0)
210  {
211  T fAngle = acos(this->qw);
212  T fSin = sin(fAngle);
213  if (std::abs(fSin) >= 1e-3)
214  {
215  T fCoeff = fAngle/fSin;
216  result.qx = fCoeff*this->qx;
217  result.qy = fCoeff*this->qy;
218  result.qz = fCoeff*this->qz;
219  return result;
220  }
221  }
222 
223  result.qx = this->qx;
224  result.qy = this->qy;
225  result.qz = this->qz;
226 
227  return result;
228  }
229 
237  public: Quaternion<T> Exp() const
238  {
239  T fAngle = sqrt(this->qx*this->qx+
240  this->qy*this->qy+this->qz*this->qz);
241  T fSin = sin(fAngle);
242 
243  Quaternion<T> result;
244  result.qw = cos(fAngle);
245 
246  if (std::abs(fSin) >= 1e-3)
247  {
248  T fCoeff = fSin/fAngle;
249  result.qx = fCoeff*this->qx;
250  result.qy = fCoeff*this->qy;
251  result.qz = fCoeff*this->qz;
252  }
253  else
254  {
255  result.qx = this->qx;
256  result.qy = this->qy;
257  result.qz = this->qz;
258  }
259 
260  return result;
261  }
262 
264  public: void Normalize()
265  {
266  T s = 0;
267 
268  s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
269  this->qy * this->qy + this->qz * this->qz));
270 
271  if (equal<T>(s, static_cast<T>(0)))
272  {
273  this->qw = T(1.0);
274  this->qx = T(0.0);
275  this->qy = T(0.0);
276  this->qz = T(0.0);
277  }
278  else
279  {
280  this->qw /= s;
281  this->qx /= s;
282  this->qy /= s;
283  this->qz /= s;
284  }
285  }
286 
289  public: Quaternion<T> Normalized() const
290  {
291  Quaternion<T> result = *this;
292  result.Normalize();
293  return result;
294  }
295 
302  public: void GZ_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa)
303  {
304  this->SetFromAxisAngle(_ax, _ay, _az, _aa);
305  }
306 
312  public: void SetFromAxisAngle(T _ax, T _ay, T _az, T _aa)
313  {
314  T l;
315 
316  l = _ax * _ax + _ay * _ay + _az * _az;
317 
318  if (equal<T>(l, static_cast<T>(0)))
319  {
320  this->qw = 1;
321  this->qx = 0;
322  this->qy = 0;
323  this->qz = 0;
324  }
325  else
326  {
327  _aa *= 0.5;
328  l = sin(_aa) / sqrt(l);
329  this->qw = cos(_aa);
330  this->qx = _ax * l;
331  this->qy = _ay * l;
332  this->qz = _az * l;
333  }
334 
335  this->Normalize();
336  }
337 
342  public: void GZ_DEPRECATED(7) Axis(const Vector3<T> &_axis, T _a)
343  {
344  this->SetFromAxisAngle(_axis, _a);
345  }
346 
350  public: void SetFromAxisAngle(const Vector3<T> &_axis, T _a)
351  {
352  this->SetFromAxisAngle(_axis.X(), _axis.Y(), _axis.Z(), _a);
353  }
354 
360  public: void Set(T _w, T _x, T _y, T _z)
361  {
362  this->qw = _w;
363  this->qx = _x;
364  this->qy = _y;
365  this->qz = _z;
366  }
367 
374  public: void GZ_DEPRECATED(7) Euler(const Vector3<T> &_vec)
375  {
376  this->SetFromEuler(_vec);
377  }
378 
384  public: void SetFromEuler(const Vector3<T> &_vec)
385  {
386  this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z());
387  }
388 
394  public: void GZ_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw)
395  {
396  this->SetFromEuler(_roll, _pitch, _yaw);
397  }
398 
403  public: void SetFromEuler(T _roll, T _pitch, T _yaw)
404  {
405  T phi, the, psi;
406 
407  phi = _roll / T(2.0);
408  the = _pitch / T(2.0);
409  psi = _yaw / T(2.0);
410 
411  this->qw = T(cos(phi) * cos(the) * cos(psi) +
412  sin(phi) * sin(the) * sin(psi));
413  this->qx = T(sin(phi) * cos(the) * cos(psi) -
414  cos(phi) * sin(the) * sin(psi));
415  this->qy = T(cos(phi) * sin(the) * cos(psi) +
416  sin(phi) * cos(the) * sin(psi));
417  this->qz = T(cos(phi) * cos(the) * sin(psi) -
418  sin(phi) * sin(the) * cos(psi));
419 
420  this->Normalize();
421  }
422 
425  public: Vector3<T> Euler() const
426  {
427  Vector3<T> vec;
428 
429  T tol = static_cast<T>(1e-15);
430 
431  Quaternion<T> copy = *this;
432  T squ;
433  T sqx;
434  T sqy;
435  T sqz;
436 
437  copy.Normalize();
438 
439  squ = copy.qw * copy.qw;
440  sqx = copy.qx * copy.qx;
441  sqy = copy.qy * copy.qy;
442  sqz = copy.qz * copy.qz;
443 
444  // Pitch
445  T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
446  if (sarg <= T(-1.0))
447  {
448  vec.Y(T(-0.5*GZ_PI));
449  }
450  else if (sarg >= T(1.0))
451  {
452  vec.Y(T(0.5*GZ_PI));
453  }
454  else
455  {
456  vec.Y(T(asin(sarg)));
457  }
458 
459  // If the pitch angle is PI/2 or -PI/2, we can only compute
460  // the sum roll + yaw. However, any combination that gives
461  // the right sum will produce the correct orientation, so we
462  // set yaw = 0 and compute roll.
463  // pitch angle is PI/2
464  if (std::abs(sarg - 1) < tol)
465  {
466  vec.Z(0);
467  vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
468  squ - sqx + sqy - sqz)));
469  }
470  // pitch angle is -PI/2
471  else if (std::abs(sarg + 1) < tol)
472  {
473  vec.Z(0);
474  vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
475  squ - sqx + sqy - sqz)));
476  }
477  else
478  {
479  // Roll
480  vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
481  squ - sqx - sqy + sqz)));
482 
483  // Yaw
484  vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
485  squ + sqx - sqy - sqz)));
486  }
487 
488  return vec;
489  }
490 
494  public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
495  {
496  Quaternion<T> result;
497  result.SetFromEuler(_vec);
498  return result;
499  }
500 
506  public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
507  {
508  return EulerToQuaternion(Vector3<T>(_x, _y, _z));
509  }
510 
513  public: T Roll() const
514  {
515  return this->Euler().X();
516  }
517 
520  public: T Pitch() const
521  {
522  return this->Euler().Y();
523  }
524 
527  public: T Yaw() const
528  {
529  return this->Euler().Z();
530  }
531 
536  public: void GZ_DEPRECATED(7) ToAxis(Vector3<T> &_axis, T &_angle) const
537  {
538  this->AxisAngle(_axis, _angle);
539  }
540 
544  public: void AxisAngle(Vector3<T> &_axis, T &_angle) const
545  {
546  T sqLen = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
547  if (equal<T>(sqLen, static_cast<T>(0), static_cast<T>(1e-12)))
548  {
549  _angle = 0.0;
550  _axis.Set(1, 0, 0);
551  }
552  else
553  {
554  _angle = 2.0 * acos(this->qw);
555  T invLen = 1.0 / sqrt(sqLen);
556  _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
557  }
558  }
559 
568  public: void GZ_DEPRECATED(7) Matrix(const Matrix3<T> &_mat)
569  {
570  this->SetFromMatrix(_mat);
571  }
572 
580  public: void SetFromMatrix(const Matrix3<T> &_mat)
581  {
582  const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
583  if (trace > 0.0000001)
584  {
585  qw = sqrt(1 + trace) / 2;
586  const T s = 1.0 / (4 * qw);
587  qx = (_mat(2, 1) - _mat(1, 2)) * s;
588  qy = (_mat(0, 2) - _mat(2, 0)) * s;
589  qz = (_mat(1, 0) - _mat(0, 1)) * s;
590  }
591  else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
592  {
593  qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
594  const T s = 1.0 / (4 * qx);
595  qw = (_mat(2, 1) - _mat(1, 2)) * s;
596  qy = (_mat(1, 0) + _mat(0, 1)) * s;
597  qz = (_mat(0, 2) + _mat(2, 0)) * s;
598  }
599  else if (_mat(1, 1) > _mat(2, 2))
600  {
601  qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
602  const T s = 1.0 / (4 * qy);
603  qw = (_mat(0, 2) - _mat(2, 0)) * s;
604  qx = (_mat(0, 1) + _mat(1, 0)) * s;
605  qz = (_mat(1, 2) + _mat(2, 1)) * s;
606  }
607  else
608  {
609  qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
610  const T s = 1.0 / (4 * qz);
611  qw = (_mat(1, 0) - _mat(0, 1)) * s;
612  qx = (_mat(0, 2) + _mat(2, 0)) * s;
613  qy = (_mat(1, 2) + _mat(2, 1)) * s;
614  }
615  }
616 
627  public: void GZ_DEPRECATED(7) From2Axes(
628  const Vector3<T> &_v1, const Vector3<T> &_v2)
629  {
630  this->SetFrom2Axes(_v1, _v2);
631  }
632 
642  public: void SetFrom2Axes(const Vector3<T> &_v1,
643  const Vector3<T> &_v2)
644  {
645  // generally, we utilize the fact that a quat (w, x, y, z) represents
646  // rotation of angle 2*w about axis (x, y, z)
647  //
648  // so we want to take get a vector half-way between no rotation and the
649  // double rotation, which is
650  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
651  // if _v1 and _v2 are unit quaternions
652  //
653  // since we normalize the result anyway, we can omit the division,
654  // getting the result:
655  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
656  //
657  // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
658  // is multiplied by k = norm(_v1)*norm(_v2)
659 
660  const T kCosTheta = _v1.Dot(_v2);
661  const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
662 
663  if (fabs(kCosTheta/k + 1) < 1e-6)
664  {
665  // the vectors are opposite
666  // any vector orthogonal to _v1
667  Vector3<T> other;
668  {
669  const Vector3<T> _v1Abs(_v1.Abs());
670  if (_v1Abs.X() < _v1Abs.Y())
671  {
672  if (_v1Abs.X() < _v1Abs.Z())
673  {
674  other.Set(1, 0, 0);
675  }
676  else
677  {
678  other.Set(0, 0, 1);
679  }
680  }
681  else
682  {
683  if (_v1Abs.Y() < _v1Abs.Z())
684  {
685  other.Set(0, 1, 0);
686  }
687  else
688  {
689  other.Set(0, 0, 1);
690  }
691  }
692  }
693 
694  const Vector3<T> axis(_v1.Cross(other).Normalize());
695 
696  qw = 0;
697  qx = axis.X();
698  qy = axis.Y();
699  qz = axis.Z();
700  }
701  else
702  {
703  // the vectors are in general position
704  const Vector3<T> axis(_v1.Cross(_v2));
705  qw = kCosTheta + k;
706  qx = axis.X();
707  qy = axis.Y();
708  qz = axis.Z();
709  this->Normalize();
710  }
711  }
712 
715  public: void Scale(T _scale)
716  {
717  Vector3<T> axis;
718  T angle;
719 
720  // Convert to axis-and-angle
721  this->AxisAngle(axis, angle);
722  angle *= _scale;
723 
724  this->SetFromAxisAngle(axis.X(), axis.Y(), axis.Z(), angle);
725  }
726 
730  public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
731  {
732  Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
733  this->qy + _qt.qy, this->qz + _qt.qz);
734  return result;
735  }
736 
741  {
742  *this = *this + _qt;
743 
744  return *this;
745  }
746 
750  public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
751  {
752  Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
753  this->qy - _qt.qy, this->qz - _qt.qz);
754  return result;
755  }
756 
761  {
762  *this = *this - _qt;
763  return *this;
764  }
765 
769  public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
770  {
771  return Quaternion<T>(
772  this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
773  this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
774  this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
775  this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
776  }
777 
781  public: Quaternion<T> operator*(const T &_f) const
782  {
783  return Quaternion<T>(this->qw*_f, this->qx*_f,
784  this->qy*_f, this->qz*_f);
785  }
786 
791  {
792  *this = *this * _qt;
793  return *this;
794  }
795 
799  public: Vector3<T> operator*(const Vector3<T> &_v) const
800  {
801  Vector3<T> uv, uuv;
802  Vector3<T> qvec(this->qx, this->qy, this->qz);
803  uv = qvec.Cross(_v);
804  uuv = qvec.Cross(uv);
805  uv *= (2.0f * this->qw);
806  uuv *= 2.0f;
807 
808  return _v + uv + uuv;
809  }
810 
817  public: bool operator==(const Quaternion<T> &_qt) const
818  {
819  return this->Equal(_qt, static_cast<T>(0.001));
820  }
821 
828  public: bool operator!=(const Quaternion<T> &_qt) const
829  {
830  return !(*this == _qt);
831  }
832 
835  public: Quaternion<T> operator-() const
836  {
837  return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
838  }
839 
843  public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
844  {
845  Quaternion<T> tmp(static_cast<T>(0),
846  _vec.X(), _vec.Y(), _vec.Z());
847  tmp = (*this) * (tmp * this->Inverse());
848  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
849  }
850 
854  public: Vector3<T> RotateVectorReverse(const Vector3<T> &_vec) const
855  {
856  Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
857 
858  tmp = this->Inverse() * (tmp * (*this));
859 
860  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
861  }
862 
865  public: bool IsFinite() const
866  {
867  // std::isfinite works with floating point values, need to explicit
868  // cast to avoid ambiguity in vc++.
869  return std::isfinite(static_cast<double>(this->qw)) &&
870  std::isfinite(static_cast<double>(this->qx)) &&
871  std::isfinite(static_cast<double>(this->qy)) &&
872  std::isfinite(static_cast<double>(this->qz));
873  }
874 
876  public: inline void Correct()
877  {
878  // std::isfinite works with floating point values, need to explicit
879  // cast to avoid ambiguity in vc++.
880  if (!std::isfinite(static_cast<double>(this->qx)))
881  this->qx = 0;
882  if (!std::isfinite(static_cast<double>(this->qy)))
883  this->qy = 0;
884  if (!std::isfinite(static_cast<double>(this->qz)))
885  this->qz = 0;
886  if (!std::isfinite(static_cast<double>(this->qw)))
887  this->qw = 1;
888 
889  if (equal(this->qw, static_cast<T>(0)) &&
890  equal(this->qx, static_cast<T>(0)) &&
891  equal(this->qy, static_cast<T>(0)) &&
892  equal(this->qz, static_cast<T>(0)))
893  {
894  this->qw = 1;
895  }
896  }
897 
900  public: Vector3<T> XAxis() const
901  {
902  T fTy = 2.0f*this->qy;
903  T fTz = 2.0f*this->qz;
904 
905  T fTwy = fTy*this->qw;
906  T fTwz = fTz*this->qw;
907  T fTxy = fTy*this->qx;
908  T fTxz = fTz*this->qx;
909  T fTyy = fTy*this->qy;
910  T fTzz = fTz*this->qz;
911 
912  return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
913  }
914 
917  public: Vector3<T> YAxis() const
918  {
919  T fTx = 2.0f*this->qx;
920  T fTy = 2.0f*this->qy;
921  T fTz = 2.0f*this->qz;
922  T fTwx = fTx*this->qw;
923  T fTwz = fTz*this->qw;
924  T fTxx = fTx*this->qx;
925  T fTxy = fTy*this->qx;
926  T fTyz = fTz*this->qy;
927  T fTzz = fTz*this->qz;
928 
929  return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
930  }
931 
934  public: Vector3<T> ZAxis() const
935  {
936  T fTx = 2.0f*this->qx;
937  T fTy = 2.0f*this->qy;
938  T fTz = 2.0f*this->qz;
939  T fTwx = fTx*this->qw;
940  T fTwy = fTy*this->qw;
941  T fTxx = fTx*this->qx;
942  T fTxz = fTz*this->qx;
943  T fTyy = fTy*this->qy;
944  T fTyz = fTz*this->qy;
945 
946  return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
947  }
948 
951  public: void Round(int _precision)
952  {
953  this->qx = precision(this->qx, _precision);
954  this->qy = precision(this->qy, _precision);
955  this->qz = precision(this->qz, _precision);
956  this->qw = precision(this->qw, _precision);
957  }
958 
963  public: T Dot(const Quaternion<T> &_q) const
964  {
965  return this->qw*_q.qw + this->qx * _q.qx +
966  this->qy*_q.qy + this->qz*_q.qz;
967  }
968 
979  public: static Quaternion<T> Squad(T _fT,
980  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
981  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
982  bool _shortestPath = false)
983  {
984  T fSlerpT = 2.0f*_fT*(1.0f-_fT);
985  Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
986  Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
987  return Slerp(fSlerpT, kSlerpP, kSlerpQ);
988  }
989 
998  public: static Quaternion<T> Slerp(T _fT,
999  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
1000  bool _shortestPath = false)
1001  {
1002  T fCos = _rkP.Dot(_rkQ);
1003  Quaternion<T> rkT;
1004 
1005  // Do we need to invert rotation?
1006  if (fCos < 0.0f && _shortestPath)
1007  {
1008  fCos = -fCos;
1009  rkT = -_rkQ;
1010  }
1011  else
1012  {
1013  rkT = _rkQ;
1014  }
1015 
1016  if (std::abs(fCos) < 1 - 1e-03)
1017  {
1018  // Standard case (slerp)
1019  T fSin = sqrt(1 - (fCos*fCos));
1020  T fAngle = atan2(fSin, fCos);
1021  // FIXME: should check if (std::abs(fSin) >= 1e-3)
1022  T fInvSin = 1.0f / fSin;
1023  T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
1024  T fCoeff1 = sin(_fT * fAngle) * fInvSin;
1025  return _rkP * fCoeff0 + rkT * fCoeff1;
1026  }
1027  else
1028  {
1029  // There are two situations:
1030  // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
1031  // so we can do a linear interpolation safely.
1032  // 2. "rkP" and "rkQ" are almost inverse of each
1033  // other (fCos ~= -1), there
1034  // are an infinite number of possibilities interpolation.
1035  // but we haven't have method to fix this case, so just use
1036  // linear interpolation here.
1037  Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
1038  // taking the complement requires renormalisation
1039  t.Normalize();
1040  return t;
1041  }
1042  }
1043 
1052  public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
1053  const T _deltaT) const
1054  {
1055  Quaternion<T> deltaQ;
1056  Vector3<T> theta = _angularVelocity * _deltaT / 2;
1057  T thetaMagSq = theta.SquaredLength();
1058  T s;
1059  if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
1060  {
1061  deltaQ.W() = 1.0 - thetaMagSq / 2.0;
1062  s = 1.0 - thetaMagSq / 6.0;
1063  }
1064  else
1065  {
1066  double thetaMag = sqrt(thetaMagSq);
1067  deltaQ.W() = cos(thetaMag);
1068  s = sin(thetaMag) / thetaMag;
1069  }
1070  deltaQ.X() = theta.X() * s;
1071  deltaQ.Y() = theta.Y() * s;
1072  deltaQ.Z() = theta.Z() * s;
1073  return deltaQ * (*this);
1074  }
1075 
1078  public: inline T W() const
1079  {
1080  return this->qw;
1081  }
1082 
1085  public: inline T X() const
1086  {
1087  return this->qx;
1088  }
1089 
1092  public: inline T Y() const
1093  {
1094  return this->qy;
1095  }
1096 
1099  public: inline T Z() const
1100  {
1101  return this->qz;
1102  }
1103 
1106  public: inline T &W()
1107  {
1108  return this->qw;
1109  }
1110 
1113  public: inline T &X()
1114  {
1115  return this->qx;
1116  }
1117 
1120  public: inline T &Y()
1121  {
1122  return this->qy;
1123  }
1124 
1127  public: inline T &Z()
1128  {
1129  return this->qz;
1130  }
1131 
1135  public: inline void GZ_DEPRECATED(7) X(T _v)
1136  {
1137  this->SetX(_v);
1138  }
1139 
1142  public: inline void SetX(T _v)
1143  {
1144  this->qx = _v;
1145  }
1146 
1150  public: inline void GZ_DEPRECATED(7) Y(T _v)
1151  {
1152  this->SetY(_v);
1153  }
1154 
1157  public: inline void SetY(T _v)
1158  {
1159  this->qy = _v;
1160  }
1161 
1162 
1166  public: inline void GZ_DEPRECATED(7) Z(T _v)
1167  {
1168  this->SetZ(_v);
1169  }
1170 
1173  public: inline void SetZ(T _v)
1174  {
1175  this->qz = _v;
1176  }
1177 
1181  public: inline void GZ_DEPRECATED(7) W(T _v)
1182  {
1183  this->SetW(_v);
1184  }
1185 
1188  public: inline void SetW(T _v)
1189  {
1190  this->qw = _v;
1191  }
1192 
1197  public: friend std::ostream &operator<<(std::ostream &_out,
1198  const gz::math::Quaternion<T> &_q)
1199  {
1200  Vector3<T> v(_q.Euler());
1201  _out << v;
1202  return _out;
1203  }
1204 
1209  public: friend std::istream &operator>>(std::istream &_in,
1211  {
1212  Angle roll, pitch, yaw;
1213 
1214  // Skip white spaces
1215  _in.setf(std::ios_base::skipws);
1216  _in >> roll >> pitch >> yaw;
1217 
1218  if (!_in.fail())
1219  {
1220  _q.SetFromEuler(Vector3<T>(*roll, *pitch, *yaw));
1221  }
1222 
1223  return _in;
1224  }
1225 
1233  public: bool Equal(const Quaternion &_q, const T &_tol) const
1234  {
1235  return equal(this->qx, _q.qx, _tol) &&
1236  equal(this->qy, _q.qy, _tol) &&
1237  equal(this->qz, _q.qz, _tol) &&
1238  equal(this->qw, _q.qw, _tol);
1239  }
1240 
1242  private: T qw;
1243 
1245  private: T qx;
1246 
1248  private: T qy;
1249 
1251  private: T qz;
1252  };
1253 
1254  namespace detail {
1255 
1256  template<typename T> constexpr Quaternion<T>
1257  gQuaternionIdentity(1, 0, 0, 0);
1258 
1259  template<typename T> constexpr Quaternion<T>
1260  gQuaternionZero(0, 0, 0, 0);
1261 
1262  } // namespace detail
1263 
1264  template<typename T> const Quaternion<T>
1265  &Quaternion<T>::Identity = detail::gQuaternionIdentity<T>;
1266 
1267  template<typename T> const Quaternion<T>
1268  &Quaternion<T>::Zero = detail::gQuaternionZero<T>;
1269 
1272 
1275  } // namespace GZ_MATH_VERSION_NAMESPACE
1276 } // namespace gz::math
1277 #endif // GZ_MATH_QUATERNION_HH_