17 #ifndef GZ_MATH_QUATERNION_HH_
18 #define GZ_MATH_QUATERNION_HH_
24 #include <gz/math/config.hh>
29 inline namespace GZ_MATH_VERSION_NAMESPACE {
31 template <
typename T>
class Matrix3;
90 : qw(1), qx(0), qy(0), qz(0)
103 public: constexpr
Quaternion(
const T &_w,
const T &_x,
const T &_y,
105 : qw(_w), qx(_x), qy(_y), qz(_z)
114 public:
Quaternion(
const T &_roll,
const T &_pitch,
const T &_yaw)
116 this->SetFromEuler(
Vector3<T>(_roll, _pitch, _yaw));
125 this->SetFromAxisAngle(_axis, _angle);
133 this->SetFromEuler(_rpy);
142 this->SetFromMatrix(_mat);
163 this->qx = -this->qx;
164 this->qy = -this->qy;
165 this->qz = -this->qz;
176 s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
178 if (equal<T>(s,
static_cast<T
>(0)))
209 if (std::abs(this->qw) < 1.0)
211 T fAngle = acos(this->qw);
212 T fSin = sin(fAngle);
213 if (std::abs(fSin) >= 1e-3)
215 T fCoeff = fAngle/fSin;
216 result.qx = fCoeff*this->qx;
217 result.qy = fCoeff*this->qy;
218 result.qz = fCoeff*this->qz;
223 result.qx = this->qx;
224 result.qy = this->qy;
225 result.qz = this->qz;
239 T fAngle = sqrt(this->qx*this->qx+
240 this->qy*this->qy+this->qz*this->qz);
241 T fSin = sin(fAngle);
244 result.qw = cos(fAngle);
246 if (std::abs(fSin) >= 1e-3)
248 T fCoeff = fSin/fAngle;
249 result.qx = fCoeff*this->qx;
250 result.qy = fCoeff*this->qy;
251 result.qz = fCoeff*this->qz;
255 result.qx = this->qx;
256 result.qy = this->qy;
257 result.qz = this->qz;
268 s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
269 this->qy * this->qy + this->qz * this->qz));
271 if (equal<T>(s,
static_cast<T
>(0)))
302 public:
void GZ_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa)
304 this->SetFromAxisAngle(_ax, _ay, _az, _aa);
316 l = _ax * _ax + _ay * _ay + _az * _az;
318 if (equal<T>(l,
static_cast<T
>(0)))
328 l = sin(_aa) / sqrt(l);
342 public:
void GZ_DEPRECATED(7) Axis(const
Vector3<T> &_axis, T _a)
344 this->SetFromAxisAngle(_axis, _a);
352 this->SetFromAxisAngle(_axis.
X(), _axis.
Y(), _axis.
Z(), _a);
360 public:
void Set(T _w, T _x, T _y, T _z)
374 public:
void GZ_DEPRECATED(7) Euler(const
Vector3<T> &_vec)
376 this->SetFromEuler(_vec);
386 this->SetFromEuler(_vec.
X(), _vec.
Y(), _vec.
Z());
394 public:
void GZ_DEPRECATED(7) Euler(T _roll, T _pitch, T _yaw)
396 this->SetFromEuler(_roll, _pitch, _yaw);
407 phi = _roll / T(2.0);
408 the = _pitch / T(2.0);
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));
429 T tol =
static_cast<T
>(1e-15);
439 squ = copy.qw * copy.qw;
440 sqx = copy.qx * copy.qx;
441 sqy = copy.qy * copy.qy;
442 sqz = copy.qz * copy.qz;
445 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
450 else if (sarg >= T(1.0))
456 vec.
Y(T(asin(sarg)));
464 if (std::abs(sarg - 1) < tol)
467 vec.
X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
468 squ - sqx + sqy - sqz)));
471 else if (std::abs(sarg + 1) < tol)
474 vec.
X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
475 squ - sqx + sqy - sqz)));
480 vec.
X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
481 squ - sqx - sqy + sqz)));
484 vec.
Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
485 squ + sqx - sqy - sqz)));
508 return EulerToQuaternion(
Vector3<T>(_x, _y, _z));
515 return this->Euler().X();
522 return this->Euler().Y();
529 return this->Euler().Z();
536 public:
void GZ_DEPRECATED(7) ToAxis(
Vector3<T> &_axis, T &_angle)
const
538 this->AxisAngle(_axis, _angle);
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)))
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);
568 public:
void GZ_DEPRECATED(7) Matrix(const
Matrix3<T> &_mat)
570 this->SetFromMatrix(_mat);
582 const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
583 if (trace > 0.0000001)
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;
591 else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
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;
599 else if (_mat(1, 1) > _mat(2, 2))
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;
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;
627 public:
void GZ_DEPRECATED(7) From2Axes(
630 this->SetFrom2Axes(_v1, _v2);
660 const T kCosTheta = _v1.
Dot(_v2);
663 if (fabs(kCosTheta/k + 1) < 1e-6)
670 if (_v1Abs.
X() < _v1Abs.
Y())
672 if (_v1Abs.
X() < _v1Abs.
Z())
683 if (_v1Abs.
Y() < _v1Abs.
Z())
721 this->AxisAngle(axis, angle);
724 this->SetFromAxisAngle(axis.
X(), axis.
Y(), axis.
Z(), angle);
733 this->qy + _qt.qy, this->qz + _qt.qz);
753 this->qy - _qt.qy, this->qz - _qt.qz);
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);
784 this->qy*_f, this->qz*_f);
802 Vector3<T> qvec(this->qx, this->qy, this->qz);
804 uuv = qvec.
Cross(uv);
805 uv *= (2.0f * this->qw);
808 return _v + uv + uuv;
819 return this->Equal(_qt,
static_cast<T
>(0.001));
830 return !(*
this == _qt);
837 return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
846 _vec.
X(), _vec.
Y(), _vec.
Z());
847 tmp = (*this) * (tmp * this->Inverse());
858 tmp = this->Inverse() * (tmp * (*this));
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)))
902 T fTy = 2.0f*this->qy;
903 T fTz = 2.0f*this->qz;
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;
912 return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
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;
929 return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
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;
946 return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
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);
965 return this->qw*_q.qw + this->qx * _q.qx +
966 this->qy*_q.qy + this->qz*_q.qz;
982 bool _shortestPath =
false)
984 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
985 Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
987 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
1000 bool _shortestPath =
false)
1002 T fCos = _rkP.Dot(_rkQ);
1006 if (fCos < 0.0f && _shortestPath)
1016 if (std::abs(fCos) < 1 - 1e-03)
1019 T fSin = sqrt(1 - (fCos*fCos));
1020 T fAngle = atan2(fSin, fCos);
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;
1053 const T _deltaT)
const
1056 Vector3<T> theta = _angularVelocity * _deltaT / 2;
1059 if (thetaMagSq * thetaMagSq / 24.0 <
MIN_D)
1061 deltaQ.W() = 1.0 - thetaMagSq / 2.0;
1062 s = 1.0 - thetaMagSq / 6.0;
1066 double thetaMag = sqrt(thetaMagSq);
1067 deltaQ.W() = cos(thetaMag);
1068 s = sin(thetaMag) / thetaMag;
1070 deltaQ.X() = theta.
X() * s;
1071 deltaQ.Y() = theta.
Y() * s;
1072 deltaQ.Z() = theta.
Z() * s;
1073 return deltaQ * (*this);
1078 public:
inline T
W()
const
1085 public:
inline T
X()
const
1092 public:
inline T
Y()
const
1099 public:
inline T
Z()
const
1106 public:
inline T &
W()
1113 public:
inline T &
X()
1120 public:
inline T &
Y()
1127 public:
inline T &
Z()
1135 public:
inline void GZ_DEPRECATED(7) X(T _v)
1150 public:
inline void GZ_DEPRECATED(7) Y(T _v)
1166 public:
inline void GZ_DEPRECATED(7) Z(T _v)
1181 public:
inline void GZ_DEPRECATED(7) W(T _v)
1212 Angle roll, pitch, yaw;
1215 _in.
setf(std::ios_base::skipws);
1216 _in >> roll >> pitch >> yaw;
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);
1256 template<
typename T> constexpr Quaternion<T>
1257 gQuaternionIdentity(1, 0, 0, 0);
1259 template<
typename T> constexpr Quaternion<T>
1260 gQuaternionZero(0, 0, 0, 0);
1264 template<
typename T>
const Quaternion<T>
1267 template<
typename T>
const Quaternion<T>