Gazebo Math

API Reference

8.0.0
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::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>
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
147 public: void Invert()
148 {
149 this->Normalize();
150 // this->qw = this->qw;
151 this->qx = -this->qx;
152 this->qy = -this->qy;
153 this->qz = -this->qz;
154 }
155
158 public: inline Quaternion<T> Inverse() const
159 {
160 T s = 0;
161 Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
162
163 // use s to test if quaternion is valid
164 s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
165
166 if (equal<T>(s, static_cast<T>(0)))
167 {
168 q.qw = 1.0;
169 q.qx = 0.0;
170 q.qy = 0.0;
171 q.qz = 0.0;
172 }
173 else
174 {
175 // deal with non-normalized quaternion
176 // div by s so q * qinv = identity
177 q.qw = q.qw / s;
178 q.qx = -q.qx / s;
179 q.qy = -q.qy / s;
180 q.qz = -q.qz / s;
181 }
182 return q;
183 }
184
192 public: Quaternion<T> Log() const
193 {
195 result.qw = 0.0;
196
197 if (std::abs(this->qw) < 1.0)
198 {
199 T fAngle = acos(this->qw);
200 T fSin = sin(fAngle);
201 if (std::abs(fSin) >= 1e-3)
202 {
203 T fCoeff = fAngle/fSin;
204 result.qx = fCoeff*this->qx;
205 result.qy = fCoeff*this->qy;
206 result.qz = fCoeff*this->qz;
207 return result;
208 }
209 }
210
211 result.qx = this->qx;
212 result.qy = this->qy;
213 result.qz = this->qz;
214
215 return result;
216 }
217
225 public: Quaternion<T> Exp() const
226 {
227 T fAngle = sqrt(this->qx*this->qx+
228 this->qy*this->qy+this->qz*this->qz);
229 T fSin = sin(fAngle);
230
232 result.qw = cos(fAngle);
233
234 if (std::abs(fSin) >= 1e-3)
235 {
236 T fCoeff = fSin/fAngle;
237 result.qx = fCoeff*this->qx;
238 result.qy = fCoeff*this->qy;
239 result.qz = fCoeff*this->qz;
240 }
241 else
242 {
243 result.qx = this->qx;
244 result.qy = this->qy;
245 result.qz = this->qz;
246 }
247
248 return result;
249 }
250
252 public: void Normalize()
253 {
254 T s = 0;
255
256 s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
257 this->qy * this->qy + this->qz * this->qz));
258
259 if (equal<T>(s, static_cast<T>(0)))
260 {
261 this->qw = T(1.0);
262 this->qx = T(0.0);
263 this->qy = T(0.0);
264 this->qz = T(0.0);
265 }
266 else
267 {
268 this->qw /= s;
269 this->qx /= s;
270 this->qy /= s;
271 this->qz /= s;
272 }
273 }
274
277 public: Quaternion<T> Normalized() const
278 {
279 Quaternion<T> result = *this;
280 result.Normalize();
281 return result;
282 }
283
289 public: void SetFromAxisAngle(T _ax, T _ay, T _az, T _aa)
290 {
291 T l;
292
293 l = _ax * _ax + _ay * _ay + _az * _az;
294
295 if (equal<T>(l, static_cast<T>(0)))
296 {
297 this->qw = 1;
298 this->qx = 0;
299 this->qy = 0;
300 this->qz = 0;
301 }
302 else
303 {
304 _aa *= 0.5;
305 l = sin(_aa) / sqrt(l);
306 this->qw = cos(_aa);
307 this->qx = _ax * l;
308 this->qy = _ay * l;
309 this->qz = _az * l;
310 }
311
312 this->Normalize();
313 }
314
318 public: void SetFromAxisAngle(const Vector3<T> &_axis, T _a)
319 {
320 this->SetFromAxisAngle(_axis.X(), _axis.Y(), _axis.Z(), _a);
321 }
322
328 public: void Set(T _w, T _x, T _y, T _z)
329 {
330 this->qw = _w;
331 this->qx = _x;
332 this->qy = _y;
333 this->qz = _z;
334 }
335
341 public: void SetFromEuler(const Vector3<T> &_vec)
342 {
343 this->SetFromEuler(_vec.X(), _vec.Y(), _vec.Z());
344 }
345
350 public: void SetFromEuler(T _roll, T _pitch, T _yaw)
351 {
352 T phi, the, psi;
353
354 phi = _roll / T(2.0);
355 the = _pitch / T(2.0);
356 psi = _yaw / T(2.0);
357
358 this->qw = T(cos(phi) * cos(the) * cos(psi) +
359 sin(phi) * sin(the) * sin(psi));
360 this->qx = T(sin(phi) * cos(the) * cos(psi) -
361 cos(phi) * sin(the) * sin(psi));
362 this->qy = T(cos(phi) * sin(the) * cos(psi) +
363 sin(phi) * cos(the) * sin(psi));
364 this->qz = T(cos(phi) * cos(the) * sin(psi) -
365 sin(phi) * sin(the) * cos(psi));
366
367 this->Normalize();
368 }
369
372 public: Vector3<T> Euler() const
373 {
375
376 T tol = static_cast<T>(1e-15);
377
378 Quaternion<T> copy = *this;
379 T squ;
380 T sqx;
381 T sqy;
382 T sqz;
383
384 copy.Normalize();
385
386 squ = copy.qw * copy.qw;
387 sqx = copy.qx * copy.qx;
388 sqy = copy.qy * copy.qy;
389 sqz = copy.qz * copy.qz;
390
391 // Pitch
392 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
393 if (sarg <= T(-1.0))
394 {
395 vec.Y(T(-0.5*GZ_PI));
396 }
397 else if (sarg >= T(1.0))
398 {
399 vec.Y(T(0.5*GZ_PI));
400 }
401 else
402 {
403 vec.Y(T(asin(sarg)));
404 }
405
406 // If the pitch angle is PI/2 or -PI/2, we can only compute
407 // the sum roll + yaw. However, any combination that gives
408 // the right sum will produce the correct orientation, so we
409 // set yaw = 0 and compute roll.
410 // pitch angle is PI/2
411 if (std::abs(sarg - 1) < tol)
412 {
413 vec.Z(0);
414 vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
415 squ - sqx + sqy - sqz)));
416 }
417 // pitch angle is -PI/2
418 else if (std::abs(sarg + 1) < tol)
419 {
420 vec.Z(0);
421 vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
422 squ - sqx + sqy - sqz)));
423 }
424 else
425 {
426 // Roll
427 vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
428 squ - sqx - sqy + sqz)));
429
430 // Yaw
431 vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
432 squ + sqx - sqy - sqz)));
433 }
434
435 return vec;
436 }
437
442 {
444 result.SetFromEuler(_vec);
445 return result;
446 }
447
453 public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
454 {
455 return EulerToQuaternion(Vector3<T>(_x, _y, _z));
456 }
457
460 public: T Roll() const
461 {
462 return this->Euler().X();
463 }
464
467 public: T Pitch() const
468 {
469 return this->Euler().Y();
470 }
471
474 public: T Yaw() const
475 {
476 return this->Euler().Z();
477 }
478
482 public: void AxisAngle(Vector3<T> &_axis, T &_angle) const
483 {
484 T sqLen = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
485 if (equal<T>(sqLen, static_cast<T>(0), static_cast<T>(1e-12)))
486 {
487 _angle = 0.0;
488 _axis.Set(1, 0, 0);
489 }
490 else
491 {
492 _angle = 2.0 * acos(this->qw);
493 T invLen = 1.0 / sqrt(sqLen);
494 _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
495 }
496 }
497
505 public: void SetFromMatrix(const Matrix3<T> &_mat)
506 {
507 const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
508 if (trace > 0.0000001)
509 {
510 qw = sqrt(1 + trace) / 2;
511 const T s = 1.0 / (4 * qw);
512 qx = (_mat(2, 1) - _mat(1, 2)) * s;
513 qy = (_mat(0, 2) - _mat(2, 0)) * s;
514 qz = (_mat(1, 0) - _mat(0, 1)) * s;
515 }
516 else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
517 {
518 qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
519 const T s = 1.0 / (4 * qx);
520 qw = (_mat(2, 1) - _mat(1, 2)) * s;
521 qy = (_mat(1, 0) + _mat(0, 1)) * s;
522 qz = (_mat(0, 2) + _mat(2, 0)) * s;
523 }
524 else if (_mat(1, 1) > _mat(2, 2))
525 {
526 qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
527 const T s = 1.0 / (4 * qy);
528 qw = (_mat(0, 2) - _mat(2, 0)) * s;
529 qx = (_mat(0, 1) + _mat(1, 0)) * s;
530 qz = (_mat(1, 2) + _mat(2, 1)) * s;
531 }
532 else
533 {
534 qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
535 const T s = 1.0 / (4 * qz);
536 qw = (_mat(1, 0) - _mat(0, 1)) * s;
537 qx = (_mat(0, 2) + _mat(2, 0)) * s;
538 qy = (_mat(1, 2) + _mat(2, 1)) * s;
539 }
540 }
541
551 public: void SetFrom2Axes(const Vector3<T> &_v1,
552 const Vector3<T> &_v2)
553 {
554 // generally, we utilize the fact that a quat (w, x, y, z) represents
555 // rotation of angle 2*w about axis (x, y, z)
556 //
557 // so we want to take get a vector half-way between no rotation and the
558 // double rotation, which is
559 // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
560 // if _v1 and _v2 are unit quaternions
561 //
562 // since we normalize the result anyway, we can omit the division,
563 // getting the result:
564 // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
565 //
566 // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
567 // is multiplied by k = norm(_v1)*norm(_v2)
568
569 const T kCosTheta = _v1.Dot(_v2);
570 const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
571
572 if (fabs(kCosTheta/k + 1) < 1e-6)
573 {
574 // the vectors are opposite
575 // any vector orthogonal to _v1
577 {
578 const Vector3<T> _v1Abs(_v1.Abs());
579 if (_v1Abs.X() < _v1Abs.Y())
580 {
581 if (_v1Abs.X() < _v1Abs.Z())
582 {
583 other.Set(1, 0, 0);
584 }
585 else
586 {
587 other.Set(0, 0, 1);
588 }
589 }
590 else
591 {
592 if (_v1Abs.Y() < _v1Abs.Z())
593 {
594 other.Set(0, 1, 0);
595 }
596 else
597 {
598 other.Set(0, 0, 1);
599 }
600 }
601 }
602
603 const Vector3<T> axis(_v1.Cross(other).Normalize());
604
605 qw = 0;
606 qx = axis.X();
607 qy = axis.Y();
608 qz = axis.Z();
609 }
610 else
611 {
612 // the vectors are in general position
613 const Vector3<T> axis(_v1.Cross(_v2));
614 qw = kCosTheta + k;
615 qx = axis.X();
616 qy = axis.Y();
617 qz = axis.Z();
618 this->Normalize();
619 }
620 }
621
624 public: void Scale(T _scale)
625 {
627 T angle;
628
629 // Convert to axis-and-angle
630 this->AxisAngle(axis, angle);
631 angle *= _scale;
632
633 this->SetFromAxisAngle(axis.X(), axis.Y(), axis.Z(), angle);
634 }
635
640 {
641 Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
642 this->qy + _qt.qy, this->qz + _qt.qz);
643 return result;
644 }
645
650 {
651 *this = *this + _qt;
652
653 return *this;
654 }
655
660 {
661 Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
662 this->qy - _qt.qy, this->qz - _qt.qz);
663 return result;
664 }
665
670 {
671 *this = *this - _qt;
672 return *this;
673 }
674
678 public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
679 {
680 return Quaternion<T>(
681 this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
682 this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
683 this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
684 this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
685 }
686
690 public: Quaternion<T> operator*(const T &_f) const
691 {
692 return Quaternion<T>(this->qw*_f, this->qx*_f,
693 this->qy*_f, this->qz*_f);
694 }
695
700 {
701 *this = *this * _qt;
702 return *this;
703 }
704
708 public: Vector3<T> operator*(const Vector3<T> &_v) const
709 {
711 Vector3<T> qvec(this->qx, this->qy, this->qz);
712 uv = qvec.Cross(_v);
713 uuv = qvec.Cross(uv);
714 uv *= (2.0f * this->qw);
715 uuv *= 2.0f;
716
717 return _v + uv + uuv;
718 }
719
726 public: bool operator==(const Quaternion<T> &_qt) const
727 {
728 return this->Equal(_qt, static_cast<T>(0.001));
729 }
730
737 public: bool operator!=(const Quaternion<T> &_qt) const
738 {
739 return !(*this == _qt);
740 }
741
744 public: Quaternion<T> operator-() const
745 {
746 return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
747 }
748
752 public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
753 {
754 Quaternion<T> tmp(static_cast<T>(0),
755 _vec.X(), _vec.Y(), _vec.Z());
756 tmp = (*this) * (tmp * this->Inverse());
757 return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
758 }
759
764 {
765 Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
766
767 tmp = this->Inverse() * (tmp * (*this));
768
769 return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
770 }
771
774 public: bool IsFinite() const
775 {
776 // std::isfinite works with floating point values, need to explicit
777 // cast to avoid ambiguity in vc++.
778 return std::isfinite(static_cast<double>(this->qw)) &&
779 std::isfinite(static_cast<double>(this->qx)) &&
780 std::isfinite(static_cast<double>(this->qy)) &&
781 std::isfinite(static_cast<double>(this->qz));
782 }
783
785 public: inline void Correct()
786 {
787 // std::isfinite works with floating point values, need to explicit
788 // cast to avoid ambiguity in vc++.
789 if (!std::isfinite(static_cast<double>(this->qx)))
790 this->qx = 0;
791 if (!std::isfinite(static_cast<double>(this->qy)))
792 this->qy = 0;
793 if (!std::isfinite(static_cast<double>(this->qz)))
794 this->qz = 0;
795 if (!std::isfinite(static_cast<double>(this->qw)))
796 this->qw = 1;
797
798 if (equal(this->qw, static_cast<T>(0)) &&
799 equal(this->qx, static_cast<T>(0)) &&
800 equal(this->qy, static_cast<T>(0)) &&
801 equal(this->qz, static_cast<T>(0)))
802 {
803 this->qw = 1;
804 }
805 }
806
809 public: Vector3<T> XAxis() const
810 {
811 T fTy = 2.0f*this->qy;
812 T fTz = 2.0f*this->qz;
813
814 T fTwy = fTy*this->qw;
815 T fTwz = fTz*this->qw;
816 T fTxy = fTy*this->qx;
817 T fTxz = fTz*this->qx;
818 T fTyy = fTy*this->qy;
819 T fTzz = fTz*this->qz;
820
821 return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
822 }
823
826 public: Vector3<T> YAxis() const
827 {
828 T fTx = 2.0f*this->qx;
829 T fTy = 2.0f*this->qy;
830 T fTz = 2.0f*this->qz;
831 T fTwx = fTx*this->qw;
832 T fTwz = fTz*this->qw;
833 T fTxx = fTx*this->qx;
834 T fTxy = fTy*this->qx;
835 T fTyz = fTz*this->qy;
836 T fTzz = fTz*this->qz;
837
838 return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
839 }
840
843 public: Vector3<T> ZAxis() const
844 {
845 T fTx = 2.0f*this->qx;
846 T fTy = 2.0f*this->qy;
847 T fTz = 2.0f*this->qz;
848 T fTwx = fTx*this->qw;
849 T fTwy = fTy*this->qw;
850 T fTxx = fTx*this->qx;
851 T fTxz = fTz*this->qx;
852 T fTyy = fTy*this->qy;
853 T fTyz = fTz*this->qy;
854
855 return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
856 }
857
860 public: void Round(int _precision)
861 {
862 this->qx = precision(this->qx, _precision);
863 this->qy = precision(this->qy, _precision);
864 this->qz = precision(this->qz, _precision);
865 this->qw = precision(this->qw, _precision);
866 }
867
872 public: T Dot(const Quaternion<T> &_q) const
873 {
874 return this->qw*_q.qw + this->qx * _q.qx +
875 this->qy*_q.qy + this->qz*_q.qz;
876 }
877
888 public: static Quaternion<T> Squad(T _fT,
889 const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
890 const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
891 bool _shortestPath = false)
892 {
893 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
895 Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
896 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
897 }
898
907 public: static Quaternion<T> Slerp(T _fT,
908 const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
909 bool _shortestPath = false)
910 {
911 T fCos = _rkP.Dot(_rkQ);
913
914 // Do we need to invert rotation?
915 if (fCos < 0.0f && _shortestPath)
916 {
917 fCos = -fCos;
918 rkT = -_rkQ;
919 }
920 else
921 {
922 rkT = _rkQ;
923 }
924
925 if (std::abs(fCos) < 1 - 1e-03)
926 {
927 // Standard case (slerp)
928 T fSin = sqrt(1 - (fCos*fCos));
929 T fAngle = atan2(fSin, fCos);
930 // FIXME: should check if (std::abs(fSin) >= 1e-3)
931 T fInvSin = 1.0f / fSin;
932 T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
933 T fCoeff1 = sin(_fT * fAngle) * fInvSin;
934 return _rkP * fCoeff0 + rkT * fCoeff1;
935 }
936 else
937 {
938 // There are two situations:
939 // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
940 // so we can do a linear interpolation safely.
941 // 2. "rkP" and "rkQ" are almost inverse of each
942 // other (fCos ~= -1), there
943 // are an infinite number of possibilities interpolation.
944 // but we haven't have method to fix this case, so just use
945 // linear interpolation here.
946 Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
947 // taking the complement requires renormalisation
948 t.Normalize();
949 return t;
950 }
951 }
952
962 const T _deltaT) const
963 {
966 T thetaMagSq = theta.SquaredLength();
967 T s;
968 if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
969 {
970 deltaQ.W() = 1.0 - thetaMagSq / 2.0;
971 s = 1.0 - thetaMagSq / 6.0;
972 }
973 else
974 {
975 double thetaMag = sqrt(thetaMagSq);
976 deltaQ.W() = cos(thetaMag);
977 s = sin(thetaMag) / thetaMag;
978 }
979 deltaQ.X() = theta.X() * s;
980 deltaQ.Y() = theta.Y() * s;
981 deltaQ.Z() = theta.Z() * s;
982 return deltaQ * (*this);
983 }
984
987 public: inline T W() const
988 {
989 return this->qw;
990 }
991
994 public: inline T X() const
995 {
996 return this->qx;
997 }
998
1001 public: inline T Y() const
1002 {
1003 return this->qy;
1004 }
1005
1008 public: inline T Z() const
1009 {
1010 return this->qz;
1011 }
1012
1015 public: inline T &W()
1016 {
1017 return this->qw;
1018 }
1019
1022 public: inline T &X()
1023 {
1024 return this->qx;
1025 }
1026
1029 public: inline T &Y()
1030 {
1031 return this->qy;
1032 }
1033
1036 public: inline T &Z()
1037 {
1038 return this->qz;
1039 }
1040
1043 public: inline void SetX(T _v)
1044 {
1045 this->qx = _v;
1046 }
1047
1050 public: inline void SetY(T _v)
1051 {
1052 this->qy = _v;
1053 }
1054
1055
1058 public: inline void SetZ(T _v)
1059 {
1060 this->qz = _v;
1061 }
1062
1065 public: inline void SetW(T _v)
1066 {
1067 this->qw = _v;
1068 }
1069
1076 {
1077 Vector3<T> v(_q.Euler());
1078 _out << v;
1079 return _out;
1080 }
1081
1088 {
1089 Angle roll, pitch, yaw;
1090
1091 // Skip white spaces
1092 _in.setf(std::ios_base::skipws);
1093 _in >> roll >> pitch >> yaw;
1094
1095 if (!_in.fail())
1096 {
1097 _q.SetFromEuler(Vector3<T>(*roll, *pitch, *yaw));
1098 }
1099
1100 return _in;
1101 }
1102
1110 public: bool Equal(const Quaternion &_q, const T &_tol) const
1111 {
1112 return equal(this->qx, _q.qx, _tol) &&
1113 equal(this->qy, _q.qy, _tol) &&
1114 equal(this->qz, _q.qz, _tol) &&
1115 equal(this->qw, _q.qw, _tol);
1116 }
1117
1119 private: T qw;
1120
1122 private: T qx;
1123
1125 private: T qy;
1126
1128 private: T qz;
1129 };
1130
1131 namespace detail {
1132
1133 template<typename T> constexpr Quaternion<T>
1134 gQuaternionIdentity(1, 0, 0, 0);
1135
1136 template<typename T> constexpr Quaternion<T>
1137 gQuaternionZero(0, 0, 0, 0);
1138
1139 } // namespace detail
1140
1141 template<typename T> const Quaternion<T>
1142 &Quaternion<T>::Identity = detail::gQuaternionIdentity<T>;
1143
1144 template<typename T> const Quaternion<T>
1145 &Quaternion<T>::Zero = detail::gQuaternionZero<T>;
1146
1149
1152 } // namespace GZ_MATH_VERSION_NAMESPACE
1153} // namespace gz::math
1154#endif // GZ_MATH_QUATERNION_HH_