Gazebo Math

API Reference

7.4.0
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
26namespace gz
27{
28 namespace math
29 {
30 // Inline bracket to help doxygen filtering.
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
32 //
33 template <typename T> class Matrix3;
34
79 template<typename T>
81 {
84 public: static const Quaternion &Identity;
85
88 public: static const Quaternion &Zero;
89
91 public: constexpr Quaternion()
92 : qw(1), qx(0), qy(0), qz(0)
93 {
94 // quaternion not normalized, because that breaks
95 // Pose::CoordPositionAdd(...)
96 }
97
105 public: constexpr Quaternion(const T &_w, const T &_x, const T &_y,
106 const T &_z)
107 : qw(_w), qx(_x), qy(_y), qz(_z)
108 {}
109
116 public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
117 {
118 this->SetFromEuler(Vector3<T>(_roll, _pitch, _yaw));
119 }
120
125 public: Quaternion(const Vector3<T> &_axis, const T &_angle)
126 {
127 this->SetFromAxisAngle(_axis, _angle);
128 }
129
133 public: explicit Quaternion(const Vector3<T> &_rpy)
134 {
135 this->SetFromEuler(_rpy);
136 }
137
142 public: explicit Quaternion(const Matrix3<T> &_mat)
143 {
144 this->SetFromMatrix(_mat);
145 }
146
150 public: Quaternion(const Quaternion<T> &_qt) = default;
151
153 public: ~Quaternion() = default;
154
157 public: Quaternion<T> &operator=(const Quaternion<T> &_qt) = default;
158
161 public: void Invert()
162 {
163 this->Normalize();
164 // this->qw = this->qw;
165 this->qx = -this->qx;
166 this->qy = -this->qy;
167 this->qz = -this->qz;
168 }
169
172 public: inline Quaternion<T> Inverse() const
173 {
174 T s = 0;
175 Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
176
177 // use s to test if quaternion is valid
178 s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
179
180 if (equal<T>(s, static_cast<T>(0)))
181 {
182 q.qw = 1.0;
183 q.qx = 0.0;
184 q.qy = 0.0;
185 q.qz = 0.0;
186 }
187 else
188 {
189 // deal with non-normalized quaternion
190 // div by s so q * qinv = identity
191 q.qw = q.qw / s;
192 q.qx = -q.qx / s;
193 q.qy = -q.qy / s;
194 q.qz = -q.qz / s;
195 }
196 return q;
197 }
198
206 public: Quaternion<T> Log() const
207 {
208 Quaternion<T> result;
209 result.qw = 0.0;
210
211 if (std::abs(this->qw) < 1.0)
212 {
213 T fAngle = acos(this->qw);
214 T fSin = sin(fAngle);
215 if (std::abs(fSin) >= 1e-3)
216 {
217 T fCoeff = fAngle/fSin;
218 result.qx = fCoeff*this->qx;
219 result.qy = fCoeff*this->qy;
220 result.qz = fCoeff*this->qz;
221 return result;
222 }
223 }
224
225 result.qx = this->qx;
226 result.qy = this->qy;
227 result.qz = this->qz;
228
229 return result;
230 }
231
239 public: Quaternion<T> Exp() const
240 {
241 T fAngle = sqrt(this->qx*this->qx+
242 this->qy*this->qy+this->qz*this->qz);
243 T fSin = sin(fAngle);
244
245 Quaternion<T> result;
246 result.qw = cos(fAngle);
247
248 if (std::abs(fSin) >= 1e-3)
249 {
250 T fCoeff = fSin/fAngle;
251 result.qx = fCoeff*this->qx;
252 result.qy = fCoeff*this->qy;
253 result.qz = fCoeff*this->qz;
254 }
255 else
256 {
257 result.qx = this->qx;
258 result.qy = this->qy;
259 result.qz = this->qz;
260 }
261
262 return result;
263 }
264
266 public: void Normalize()
267 {
268 T s = 0;
269
270 s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
271 this->qy * this->qy + this->qz * this->qz));
272
273 if (equal<T>(s, static_cast<T>(0)))
274 {
275 this->qw = T(1.0);
276 this->qx = T(0.0);
277 this->qy = T(0.0);
278 this->qz = T(0.0);
279 }
280 else
281 {
282 this->qw /= s;
283 this->qx /= s;
284 this->qy /= s;
285 this->qz /= s;
286 }
287 }
288
291 public: Quaternion<T> Normalized() const
292 {
293 Quaternion<T> result = *this;
294 result.Normalize();
295 return result;
296 }
297
304 public: void GZ_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa)
305 {
306 this->SetFromAxisAngle(_ax, _ay, _az, _aa);
307 }
308
314 public: void SetFromAxisAngle(T _ax, T _ay, T _az, T _aa)
315 {
316 T l;
317
318 l = _ax * _ax + _ay * _ay + _az * _az;
319
320 if (equal<T>(l, static_cast<T>(0)))
321 {
322 this->qw = 1;
323 this->qx = 0;
324 this->qy = 0;
325 this->qz = 0;
326 }
327 else
328 {
329 _aa *= 0.5;
330 l = sin(_aa) / sqrt(l);
331 this->qw = cos(_aa);
332 this->qx = _ax * l;
333 this->qy = _ay * l;
334 this->qz = _az * l;
335 }
336
337 this->Normalize();
338 }
339
344 public: void GZ_DEPRECATED(7) Axis(const Vector3<T> &_axis, T _a)
345 {
346 this->SetFromAxisAngle(_axis, _a);
347 }
348
352 public: void SetFromAxisAngle(const Vector3<T> &_axis, T _a)
353 {
354 this->SetFromAxisAngle(_axis.X(), _axis.Y(), _axis.Z(), _a);
355 }
356
362 public: void Set(T _w, T _x, T _y, T _z)
363 {
364 this->qw = _w;
365 this->qx = _x;
366 this->qy = _y;
367 this->qz = _z;
368 }
369
376 public: void GZ_DEPRECATED(7) Euler(const Vector3<T> &_vec)
377 {
378 this->SetFromEuler(_vec);
379 }
380
386 public: void SetFromEuler(const Vector3<T> &_vec)
387 {
388 this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z());
389 }
390
396 public: void GZ_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw)
397 {
398 this->SetFromEuler(_roll, _pitch, _yaw);
399 }
400
405 public: void SetFromEuler(T _roll, T _pitch, T _yaw)
406 {
407 T phi, the, psi;
408
409 phi = _roll / T(2.0);
410 the = _pitch / T(2.0);
411 psi = _yaw / T(2.0);
412
413 this->qw = T(cos(phi) * cos(the) * cos(psi) +
414 sin(phi) * sin(the) * sin(psi));
415 this->qx = T(sin(phi) * cos(the) * cos(psi) -
416 cos(phi) * sin(the) * sin(psi));
417 this->qy = T(cos(phi) * sin(the) * cos(psi) +
418 sin(phi) * cos(the) * sin(psi));
419 this->qz = T(cos(phi) * cos(the) * sin(psi) -
420 sin(phi) * sin(the) * cos(psi));
421
422 this->Normalize();
423 }
424
427 public: Vector3<T> Euler() const
428 {
429 Vector3<T> vec;
430
431 T tol = static_cast<T>(1e-15);
432
433 Quaternion<T> copy = *this;
434 T squ;
435 T sqx;
436 T sqy;
437 T sqz;
438
439 copy.Normalize();
440
441 squ = copy.qw * copy.qw;
442 sqx = copy.qx * copy.qx;
443 sqy = copy.qy * copy.qy;
444 sqz = copy.qz * copy.qz;
445
446 // Pitch
447 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
448 if (sarg <= T(-1.0))
449 {
450 vec.Y(T(-0.5*GZ_PI));
451 }
452 else if (sarg >= T(1.0))
453 {
454 vec.Y(T(0.5*GZ_PI));
455 }
456 else
457 {
458 vec.Y(T(asin(sarg)));
459 }
460
461 // If the pitch angle is PI/2 or -PI/2, we can only compute
462 // the sum roll + yaw. However, any combination that gives
463 // the right sum will produce the correct orientation, so we
464 // set yaw = 0 and compute roll.
465 // pitch angle is PI/2
466 if (std::abs(sarg - 1) < tol)
467 {
468 vec.Z(0);
469 vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
470 squ - sqx + sqy - sqz)));
471 }
472 // pitch angle is -PI/2
473 else if (std::abs(sarg + 1) < tol)
474 {
475 vec.Z(0);
476 vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
477 squ - sqx + sqy - sqz)));
478 }
479 else
480 {
481 // Roll
482 vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
483 squ - sqx - sqy + sqz)));
484
485 // Yaw
486 vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
487 squ + sqx - sqy - sqz)));
488 }
489
490 return vec;
491 }
492
496 public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
497 {
498 Quaternion<T> result;
499 result.SetFromEuler(_vec);
500 return result;
501 }
502
508 public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
509 {
510 return EulerToQuaternion(Vector3<T>(_x, _y, _z));
511 }
512
515 public: T Roll() const
516 {
517 return this->Euler().X();
518 }
519
522 public: T Pitch() const
523 {
524 return this->Euler().Y();
525 }
526
529 public: T Yaw() const
530 {
531 return this->Euler().Z();
532 }
533
538 public: void GZ_DEPRECATED(7) ToAxis(Vector3<T> &_axis, T &_angle) const
539 {
540 this->AxisAngle(_axis, _angle);
541 }
542
546 public: void AxisAngle(Vector3<T> &_axis, T &_angle) const
547 {
548 T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
549 if (equal<T>(len, static_cast<T>(0)))
550 {
551 _angle = 0.0;
552 _axis.Set(1, 0, 0);
553 }
554 else
555 {
556 _angle = 2.0 * acos(this->qw);
557 T invLen = 1.0 / sqrt(len);
558 _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
559 }
560 }
561
570 public: void GZ_DEPRECATED(7) Matrix(const Matrix3<T> &_mat)
571 {
572 this->SetFromMatrix(_mat);
573 }
574
582 public: void SetFromMatrix(const Matrix3<T> &_mat)
583 {
584 const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
585 if (trace > 0.0000001)
586 {
587 qw = sqrt(1 + trace) / 2;
588 const T s = 1.0 / (4 * qw);
589 qx = (_mat(2, 1) - _mat(1, 2)) * s;
590 qy = (_mat(0, 2) - _mat(2, 0)) * s;
591 qz = (_mat(1, 0) - _mat(0, 1)) * s;
592 }
593 else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
594 {
595 qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
596 const T s = 1.0 / (4 * qx);
597 qw = (_mat(2, 1) - _mat(1, 2)) * s;
598 qy = (_mat(1, 0) + _mat(0, 1)) * s;
599 qz = (_mat(0, 2) + _mat(2, 0)) * s;
600 }
601 else if (_mat(1, 1) > _mat(2, 2))
602 {
603 qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
604 const T s = 1.0 / (4 * qy);
605 qw = (_mat(0, 2) - _mat(2, 0)) * s;
606 qx = (_mat(0, 1) + _mat(1, 0)) * s;
607 qz = (_mat(1, 2) + _mat(2, 1)) * s;
608 }
609 else
610 {
611 qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
612 const T s = 1.0 / (4 * qz);
613 qw = (_mat(1, 0) - _mat(0, 1)) * s;
614 qx = (_mat(0, 2) + _mat(2, 0)) * s;
615 qy = (_mat(1, 2) + _mat(2, 1)) * s;
616 }
617 }
618
629 public: void GZ_DEPRECATED(7) From2Axes(
630 const Vector3<T> &_v1, const Vector3<T> &_v2)
631 {
632 this->SetFrom2Axes(_v1, _v2);
633 }
634
644 public: void SetFrom2Axes(const Vector3<T> &_v1,
645 const Vector3<T> &_v2)
646 {
647 // generally, we utilize the fact that a quat (w, x, y, z) represents
648 // rotation of angle 2*w about axis (x, y, z)
649 //
650 // so we want to take get a vector half-way between no rotation and the
651 // double rotation, which is
652 // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
653 // if _v1 and _v2 are unit quaternions
654 //
655 // since we normalize the result anyway, we can omit the division,
656 // getting the result:
657 // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
658 //
659 // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
660 // is multiplied by k = norm(_v1)*norm(_v2)
661
662 const T kCosTheta = _v1.Dot(_v2);
663 const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
664
665 if (fabs(kCosTheta/k + 1) < 1e-6)
666 {
667 // the vectors are opposite
668 // any vector orthogonal to _v1
669 Vector3<T> other;
670 {
671 const Vector3<T> _v1Abs(_v1.Abs());
672 if (_v1Abs.X() < _v1Abs.Y())
673 {
674 if (_v1Abs.X() < _v1Abs.Z())
675 {
676 other.Set(1, 0, 0);
677 }
678 else
679 {
680 other.Set(0, 0, 1);
681 }
682 }
683 else
684 {
685 if (_v1Abs.Y() < _v1Abs.Z())
686 {
687 other.Set(0, 1, 0);
688 }
689 else
690 {
691 other.Set(0, 0, 1);
692 }
693 }
694 }
695
696 const Vector3<T> axis(_v1.Cross(other).Normalize());
697
698 qw = 0;
699 qx = axis.X();
700 qy = axis.Y();
701 qz = axis.Z();
702 }
703 else
704 {
705 // the vectors are in general position
706 const Vector3<T> axis(_v1.Cross(_v2));
707 qw = kCosTheta + k;
708 qx = axis.X();
709 qy = axis.Y();
710 qz = axis.Z();
711 this->Normalize();
712 }
713 }
714
717 public: void Scale(T _scale)
718 {
719 Vector3<T> axis;
720 T angle;
721
722 // Convert to axis-and-angle
723 this->AxisAngle(axis, angle);
724 angle *= _scale;
725
726 this->SetFromAxisAngle(axis.X(), axis.Y(), axis.Z(), angle);
727 }
728
732 public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
733 {
734 Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
735 this->qy + _qt.qy, this->qz + _qt.qz);
736 return result;
737 }
738
743 {
744 *this = *this + _qt;
745
746 return *this;
747 }
748
752 public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
753 {
754 Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
755 this->qy - _qt.qy, this->qz - _qt.qz);
756 return result;
757 }
758
763 {
764 *this = *this - _qt;
765 return *this;
766 }
767
771 public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
772 {
773 return Quaternion<T>(
774 this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
775 this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
776 this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
777 this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
778 }
779
783 public: Quaternion<T> operator*(const T &_f) const
784 {
785 return Quaternion<T>(this->qw*_f, this->qx*_f,
786 this->qy*_f, this->qz*_f);
787 }
788
793 {
794 *this = *this * _qt;
795 return *this;
796 }
797
801 public: Vector3<T> operator*(const Vector3<T> &_v) const
802 {
803 Vector3<T> uv, uuv;
804 Vector3<T> qvec(this->qx, this->qy, this->qz);
805 uv = qvec.Cross(_v);
806 uuv = qvec.Cross(uv);
807 uv *= (2.0f * this->qw);
808 uuv *= 2.0f;
809
810 return _v + uv + uuv;
811 }
812
819 public: bool operator==(const Quaternion<T> &_qt) const
820 {
821 return this->Equal(_qt, static_cast<T>(0.001));
822 }
823
830 public: bool operator!=(const Quaternion<T> &_qt) const
831 {
832 return !(*this == _qt);
833 }
834
837 public: Quaternion<T> operator-() const
838 {
839 return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
840 }
841
845 public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
846 {
847 Quaternion<T> tmp(static_cast<T>(0),
848 _vec.X(), _vec.Y(), _vec.Z());
849 tmp = (*this) * (tmp * this->Inverse());
850 return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
851 }
852
856 public: Vector3<T> RotateVectorReverse(const Vector3<T> &_vec) const
857 {
858 Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
859
860 tmp = this->Inverse() * (tmp * (*this));
861
862 return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
863 }
864
867 public: bool IsFinite() const
868 {
869 // std::isfinite works with floating point values, need to explicit
870 // cast to avoid ambiguity in vc++.
871 return std::isfinite(static_cast<double>(this->qw)) &&
872 std::isfinite(static_cast<double>(this->qx)) &&
873 std::isfinite(static_cast<double>(this->qy)) &&
874 std::isfinite(static_cast<double>(this->qz));
875 }
876
878 public: inline void Correct()
879 {
880 // std::isfinite works with floating point values, need to explicit
881 // cast to avoid ambiguity in vc++.
882 if (!std::isfinite(static_cast<double>(this->qx)))
883 this->qx = 0;
884 if (!std::isfinite(static_cast<double>(this->qy)))
885 this->qy = 0;
886 if (!std::isfinite(static_cast<double>(this->qz)))
887 this->qz = 0;
888 if (!std::isfinite(static_cast<double>(this->qw)))
889 this->qw = 1;
890
891 if (equal(this->qw, static_cast<T>(0)) &&
892 equal(this->qx, static_cast<T>(0)) &&
893 equal(this->qy, static_cast<T>(0)) &&
894 equal(this->qz, static_cast<T>(0)))
895 {
896 this->qw = 1;
897 }
898 }
899
902 public: Vector3<T> XAxis() const
903 {
904 T fTy = 2.0f*this->qy;
905 T fTz = 2.0f*this->qz;
906
907 T fTwy = fTy*this->qw;
908 T fTwz = fTz*this->qw;
909 T fTxy = fTy*this->qx;
910 T fTxz = fTz*this->qx;
911 T fTyy = fTy*this->qy;
912 T fTzz = fTz*this->qz;
913
914 return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
915 }
916
919 public: Vector3<T> YAxis() const
920 {
921 T fTx = 2.0f*this->qx;
922 T fTy = 2.0f*this->qy;
923 T fTz = 2.0f*this->qz;
924 T fTwx = fTx*this->qw;
925 T fTwz = fTz*this->qw;
926 T fTxx = fTx*this->qx;
927 T fTxy = fTy*this->qx;
928 T fTyz = fTz*this->qy;
929 T fTzz = fTz*this->qz;
930
931 return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
932 }
933
936 public: Vector3<T> ZAxis() const
937 {
938 T fTx = 2.0f*this->qx;
939 T fTy = 2.0f*this->qy;
940 T fTz = 2.0f*this->qz;
941 T fTwx = fTx*this->qw;
942 T fTwy = fTy*this->qw;
943 T fTxx = fTx*this->qx;
944 T fTxz = fTz*this->qx;
945 T fTyy = fTy*this->qy;
946 T fTyz = fTz*this->qy;
947
948 return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
949 }
950
953 public: void Round(int _precision)
954 {
955 this->qx = precision(this->qx, _precision);
956 this->qy = precision(this->qy, _precision);
957 this->qz = precision(this->qz, _precision);
958 this->qw = precision(this->qw, _precision);
959 }
960
965 public: T Dot(const Quaternion<T> &_q) const
966 {
967 return this->qw*_q.qw + this->qx * _q.qx +
968 this->qy*_q.qy + this->qz*_q.qz;
969 }
970
981 public: static Quaternion<T> Squad(T _fT,
982 const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
983 const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
984 bool _shortestPath = false)
985 {
986 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
987 Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
988 Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
989 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
990 }
991
1000 public: static Quaternion<T> Slerp(T _fT,
1001 const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
1002 bool _shortestPath = false)
1003 {
1004 T fCos = _rkP.Dot(_rkQ);
1005 Quaternion<T> rkT;
1006
1007 // Do we need to invert rotation?
1008 if (fCos < 0.0f && _shortestPath)
1009 {
1010 fCos = -fCos;
1011 rkT = -_rkQ;
1012 }
1013 else
1014 {
1015 rkT = _rkQ;
1016 }
1017
1018 if (std::abs(fCos) < 1 - 1e-03)
1019 {
1020 // Standard case (slerp)
1021 T fSin = sqrt(1 - (fCos*fCos));
1022 T fAngle = atan2(fSin, fCos);
1023 // FIXME: should check if (std::abs(fSin) >= 1e-3)
1024 T fInvSin = 1.0f / fSin;
1025 T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
1026 T fCoeff1 = sin(_fT * fAngle) * fInvSin;
1027 return _rkP * fCoeff0 + rkT * fCoeff1;
1028 }
1029 else
1030 {
1031 // There are two situations:
1032 // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
1033 // so we can do a linear interpolation safely.
1034 // 2. "rkP" and "rkQ" are almost inverse of each
1035 // other (fCos ~= -1), there
1036 // are an infinite number of possibilities interpolation.
1037 // but we haven't have method to fix this case, so just use
1038 // linear interpolation here.
1039 Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
1040 // taking the complement requires renormalisation
1041 t.Normalize();
1042 return t;
1043 }
1044 }
1045
1054 public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
1055 const T _deltaT) const
1056 {
1057 Quaternion<T> deltaQ;
1058 Vector3<T> theta = _angularVelocity * _deltaT / 2;
1059 T thetaMagSq = theta.SquaredLength();
1060 T s;
1061 if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
1062 {
1063 deltaQ.W() = 1.0 - thetaMagSq / 2.0;
1064 s = 1.0 - thetaMagSq / 6.0;
1065 }
1066 else
1067 {
1068 double thetaMag = sqrt(thetaMagSq);
1069 deltaQ.W() = cos(thetaMag);
1070 s = sin(thetaMag) / thetaMag;
1071 }
1072 deltaQ.X() = theta.X() * s;
1073 deltaQ.Y() = theta.Y() * s;
1074 deltaQ.Z() = theta.Z() * s;
1075 return deltaQ * (*this);
1076 }
1077
1080 public: inline T W() const
1081 {
1082 return this->qw;
1083 }
1084
1087 public: inline T X() const
1088 {
1089 return this->qx;
1090 }
1091
1094 public: inline T Y() const
1095 {
1096 return this->qy;
1097 }
1098
1101 public: inline T Z() const
1102 {
1103 return this->qz;
1104 }
1105
1108 public: inline T &W()
1109 {
1110 return this->qw;
1111 }
1112
1115 public: inline T &X()
1116 {
1117 return this->qx;
1118 }
1119
1122 public: inline T &Y()
1123 {
1124 return this->qy;
1125 }
1126
1129 public: inline T &Z()
1130 {
1131 return this->qz;
1132 }
1133
1137 public: inline void GZ_DEPRECATED(7) X(T _v)
1138 {
1139 this->SetX(_v);
1140 }
1141
1144 public: inline void SetX(T _v)
1145 {
1146 this->qx = _v;
1147 }
1148
1152 public: inline void GZ_DEPRECATED(7) Y(T _v)
1153 {
1154 this->SetY(_v);
1155 }
1156
1159 public: inline void SetY(T _v)
1160 {
1161 this->qy = _v;
1162 }
1163
1164
1168 public: inline void GZ_DEPRECATED(7) Z(T _v)
1169 {
1170 this->SetZ(_v);
1171 }
1172
1175 public: inline void SetZ(T _v)
1176 {
1177 this->qz = _v;
1178 }
1179
1183 public: inline void GZ_DEPRECATED(7) W(T _v)
1184 {
1185 this->SetW(_v);
1186 }
1187
1190 public: inline void SetW(T _v)
1191 {
1192 this->qw = _v;
1193 }
1194
1199 public: friend std::ostream &operator<<(std::ostream &_out,
1200 const gz::math::Quaternion<T> &_q)
1201 {
1202 Vector3<T> v(_q.Euler());
1203 _out << v;
1204 return _out;
1205 }
1206
1213 {
1214 Angle roll, pitch, yaw;
1215
1216 // Skip white spaces
1217 _in.setf(std::ios_base::skipws);
1218 _in >> roll >> pitch >> yaw;
1219
1220 if (!_in.fail())
1221 {
1222 _q.SetFromEuler(Vector3<T>(*roll, *pitch, *yaw));
1223 }
1224
1225 return _in;
1226 }
1227
1235 public: bool Equal(const Quaternion &_q, const T &_tol) const
1236 {
1237 return equal(this->qx, _q.qx, _tol) &&
1238 equal(this->qy, _q.qy, _tol) &&
1239 equal(this->qz, _q.qz, _tol) &&
1240 equal(this->qw, _q.qw, _tol);
1241 }
1242
1244 private: T qw;
1245
1247 private: T qx;
1248
1250 private: T qy;
1251
1253 private: T qz;
1254 };
1255
1256 namespace detail {
1257
1258 template<typename T> constexpr Quaternion<T>
1259 gQuaternionIdentity(1, 0, 0, 0);
1260
1261 template<typename T> constexpr Quaternion<T>
1262 gQuaternionZero(0, 0, 0, 0);
1263
1264 } // namespace detail
1265
1266 template<typename T> const Quaternion<T>
1267 &Quaternion<T>::Identity = detail::gQuaternionIdentity<T>;
1268
1269 template<typename T> const Quaternion<T>
1270 &Quaternion<T>::Zero = detail::gQuaternionZero<T>;
1271
1274
1277 }
1278 }
1279}
1280#endif