241 T fAngle = sqrt(this->qx*this->qx+
242 this->qy*this->qy+this->qz*this->qz);
243 T fSin = sin(fAngle);
246 result.qw = cos(fAngle);
248 if (std::abs(fSin) >= 1e-3)
250 T fCoeff = fSin/fAngle;
251 result.qx = fCoeff*this->qx;
252 result.qy = fCoeff*this->qy;
253 result.qz = fCoeff*this->qz;
257 result.qx = this->qx;
258 result.qy = this->qy;
259 result.qz = this->qz;
304 public:
void GZ_DEPRECATED(7) Axis(T _ax, T _ay, T _az, T _aa)
306 this->SetFromAxisAngle(_ax, _ay, _az, _aa);
409 phi = _roll / T(2.0);
410 the = _pitch / T(2.0);
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));
431 T tol =
static_cast<T
>(1e-15);
441 squ = copy.qw * copy.qw;
442 sqx = copy.qx * copy.qx;
443 sqy = copy.qy * copy.qy;
444 sqz = copy.qz * copy.qz;
447 T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
452 else if (sarg >= T(1.0))
458 vec.
Y(T(asin(sarg)));
466 if (std::abs(sarg - 1) < tol)
469 vec.
X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
470 squ - sqx + sqy - sqz)));
473 else if (std::abs(sarg + 1) < tol)
476 vec.
X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
477 squ - sqx + sqy - sqz)));
482 vec.
X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
483 squ - sqx - sqy + sqz)));
486 vec.
Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
487 squ + sqx - sqy - sqz)));
548 T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
549 if (equal<T>(len,
static_cast<T
>(0)))
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);
584 const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
585 if (trace > 0.0000001)
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;
593 else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
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;
601 else if (_mat(1, 1) > _mat(2, 2))
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;
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;
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);
984 bool _shortestPath =
false)
986 T fSlerpT = 2.0f*_fT*(1.0f-_fT);
987 Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
989 return Slerp(fSlerpT, kSlerpP, kSlerpQ);
1002 bool _shortestPath =
false)
1004 T fCos = _rkP.
Dot(_rkQ);
1008 if (fCos < 0.0f && _shortestPath)
1018 if (std::abs(fCos) < 1 - 1e-03)
1021 T fSin = sqrt(1 - (fCos*fCos));
1022 T fAngle = atan2(fSin, fCos);
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;
1055 const T _deltaT)
const
1058 Vector3<T> theta = _angularVelocity * _deltaT / 2;
1061 if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
1063 deltaQ.
W() = 1.0 - thetaMagSq / 2.0;
1064 s = 1.0 - thetaMagSq / 6.0;
1068 double thetaMag = sqrt(thetaMagSq);
1069 deltaQ.
W() = cos(thetaMag);
1070 s = sin(thetaMag) / thetaMag;
1072 deltaQ.
X() = theta.
X() * s;
1073 deltaQ.
Y() = theta.
Y() * s;
1074 deltaQ.
Z() = theta.
Z() * s;
1075 return deltaQ * (*this);
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);