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 IGNITION_MATH_QUATERNION_HH_
18 #define IGNITION_MATH_QUATERNION_HH_
19 
20 #include <ignition/math/Helpers.hh>
21 #include <ignition/math/Angle.hh>
22 #include <ignition/math/Vector3.hh>
23 #include <ignition/math/Matrix3.hh>
24 #include <ignition/math/config.hh>
25 
26 namespace ignition
27 {
28  namespace math
29  {
30  inline namespace IGNITION_MATH_VERSION_NAMESPACE
31  {
32  template <typename T> class Matrix3;
33 
36  template<typename T>
37  class Quaternion
38  {
40  public: static const Quaternion Identity;
41 
43  public: static const Quaternion Zero;
44 
46  public: Quaternion()
47  : qw(1), qx(0), qy(0), qz(0)
48  {
49  // quaternion not normalized, because that breaks
50  // Pose::CoordPositionAdd(...)
51  }
52 
58  public: Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
59  : qw(_w), qx(_x), qy(_y), qz(_z)
60  {}
61 
66  public: Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
67  {
68  this->Euler(Vector3<T>(_roll, _pitch, _yaw));
69  }
70 
74  public: Quaternion(const Vector3<T> &_axis, const T &_angle)
75  {
76  this->Axis(_axis, _angle);
77  }
78 
81  public: explicit Quaternion(const Vector3<T> &_rpy)
82  {
83  this->Euler(_rpy);
84  }
85 
89  public: explicit Quaternion(const Matrix3<T> &_mat)
90  {
91  this->Matrix(_mat);
92  }
93 
96  public: Quaternion(const Quaternion<T> &_qt)
97  {
98  this->qw = _qt.qw;
99  this->qx = _qt.qx;
100  this->qy = _qt.qy;
101  this->qz = _qt.qz;
102  }
103 
105  public: ~Quaternion() {}
106 
109  public: Quaternion<T> &operator=(const Quaternion<T> &_qt)
110  {
111  this->qw = _qt.qw;
112  this->qx = _qt.qx;
113  this->qy = _qt.qy;
114  this->qz = _qt.qz;
115 
116  return *this;
117  }
118 
120  public: void Invert()
121  {
122  this->Normalize();
123  // this->qw = this->qw;
124  this->qx = -this->qx;
125  this->qy = -this->qy;
126  this->qz = -this->qz;
127  }
128 
131  public: inline Quaternion<T> Inverse() const
132  {
133  T s = 0;
134  Quaternion<T> q(this->qw, this->qx, this->qy, this->qz);
135 
136  // use s to test if quaternion is valid
137  s = q.qw * q.qw + q.qx * q.qx + q.qy * q.qy + q.qz * q.qz;
138 
139  if (equal<T>(s, static_cast<T>(0)))
140  {
141  q.qw = 1.0;
142  q.qx = 0.0;
143  q.qy = 0.0;
144  q.qz = 0.0;
145  }
146  else
147  {
148  // deal with non-normalized quaternion
149  // div by s so q * qinv = identity
150  q.qw = q.qw / s;
151  q.qx = -q.qx / s;
152  q.qy = -q.qy / s;
153  q.qz = -q.qz / s;
154  }
155  return q;
156  }
157 
160  public: Quaternion<T> Log() const
161  {
162  // If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x, y, z) is unit length,
163  // then log(q) = A*(x*i+y*j+z*k). If sin(A) is near zero, use log(q) =
164  // sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.
165 
166  Quaternion<T> result;
167  result.qw = 0.0;
168 
169  if (std::abs(this->qw) < 1.0)
170  {
171  T fAngle = acos(this->qw);
172  T fSin = sin(fAngle);
173  if (std::abs(fSin) >= 1e-3)
174  {
175  T fCoeff = fAngle/fSin;
176  result.qx = fCoeff*this->qx;
177  result.qy = fCoeff*this->qy;
178  result.qz = fCoeff*this->qz;
179  return result;
180  }
181  }
182 
183  result.qx = this->qx;
184  result.qy = this->qy;
185  result.qz = this->qz;
186 
187  return result;
188  }
189 
192  public: Quaternion<T> Exp() const
193  {
194  // If q = A*(x*i+y*j+z*k) where (x, y, z) is unit length, then
195  // exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k). If sin(A) is near zero,
196  // use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.
197 
198  T fAngle = sqrt(this->qx*this->qx+
199  this->qy*this->qy+this->qz*this->qz);
200  T fSin = sin(fAngle);
201 
202  Quaternion<T> result;
203  result.qw = cos(fAngle);
204 
205  if (std::abs(fSin) >= 1e-3)
206  {
207  T fCoeff = fSin/fAngle;
208  result.qx = fCoeff*this->qx;
209  result.qy = fCoeff*this->qy;
210  result.qz = fCoeff*this->qz;
211  }
212  else
213  {
214  result.qx = this->qx;
215  result.qy = this->qy;
216  result.qz = this->qz;
217  }
218 
219  return result;
220  }
221 
223  public: void Normalize()
224  {
225  T s = 0;
226 
227  s = T(sqrt(this->qw * this->qw + this->qx * this->qx +
228  this->qy * this->qy + this->qz * this->qz));
229 
230  if (equal<T>(s, static_cast<T>(0)))
231  {
232  this->qw = T(1.0);
233  this->qx = T(0.0);
234  this->qy = T(0.0);
235  this->qz = T(0.0);
236  }
237  else
238  {
239  this->qw /= s;
240  this->qx /= s;
241  this->qy /= s;
242  this->qz /= s;
243  }
244  }
245 
251  public: void Axis(T _ax, T _ay, T _az, T _aa)
252  {
253  T l;
254 
255  l = _ax * _ax + _ay * _ay + _az * _az;
256 
257  if (equal<T>(l, static_cast<T>(0)))
258  {
259  this->qw = 1;
260  this->qx = 0;
261  this->qy = 0;
262  this->qz = 0;
263  }
264  else
265  {
266  _aa *= 0.5;
267  l = sin(_aa) / sqrt(l);
268  this->qw = cos(_aa);
269  this->qx = _ax * l;
270  this->qy = _ay * l;
271  this->qz = _az * l;
272  }
273 
274  this->Normalize();
275  }
276 
280  public: void Axis(const Vector3<T> &_axis, T _a)
281  {
282  this->Axis(_axis.X(), _axis.Y(), _axis.Z(), _a);
283  }
284 
290  public: void Set(T _w, T _x, T _y, T _z)
291  {
292  this->qw = _w;
293  this->qx = _x;
294  this->qy = _y;
295  this->qz = _z;
296  }
297 
303  public: void Euler(const Vector3<T> &_vec)
304  {
305  this->Euler(_vec.X(), _vec.Y(), _vec.Z());
306  }
307 
312  public: void Euler(T _roll, T _pitch, T _yaw)
313  {
314  T phi, the, psi;
315 
316  phi = _roll / T(2.0);
317  the = _pitch / T(2.0);
318  psi = _yaw / T(2.0);
319 
320  this->qw = T(cos(phi) * cos(the) * cos(psi) +
321  sin(phi) * sin(the) * sin(psi));
322  this->qx = T(sin(phi) * cos(the) * cos(psi) -
323  cos(phi) * sin(the) * sin(psi));
324  this->qy = T(cos(phi) * sin(the) * cos(psi) +
325  sin(phi) * cos(the) * sin(psi));
326  this->qz = T(cos(phi) * cos(the) * sin(psi) -
327  sin(phi) * sin(the) * cos(psi));
328 
329  this->Normalize();
330  }
331 
334  public: Vector3<T> Euler() const
335  {
336  Vector3<T> vec;
337 
338  T tol = static_cast<T>(1e-15);
339 
340  Quaternion<T> copy = *this;
341  T squ;
342  T sqx;
343  T sqy;
344  T sqz;
345 
346  copy.Normalize();
347 
348  squ = copy.qw * copy.qw;
349  sqx = copy.qx * copy.qx;
350  sqy = copy.qy * copy.qy;
351  sqz = copy.qz * copy.qz;
352 
353  // Pitch
354  T sarg = -2 * (copy.qx*copy.qz - copy.qw * copy.qy);
355  if (sarg <= T(-1.0))
356  {
357  vec.Y(T(-0.5*IGN_PI));
358  }
359  else if (sarg >= T(1.0))
360  {
361  vec.Y(T(0.5*IGN_PI));
362  }
363  else
364  {
365  vec.Y(T(asin(sarg)));
366  }
367 
368  // If the pitch angle is PI/2 or -PI/2, we can only compute
369  // the sum roll + yaw. However, any combination that gives
370  // the right sum will produce the correct orientation, so we
371  // set yaw = 0 and compute roll.
372  // pitch angle is PI/2
373  if (std::abs(sarg - 1) < tol)
374  {
375  vec.Z(0);
376  vec.X(T(atan2(2 * (copy.qx*copy.qy - copy.qz*copy.qw),
377  squ - sqx + sqy - sqz)));
378  }
379  // pitch angle is -PI/2
380  else if (std::abs(sarg + 1) < tol)
381  {
382  vec.Z(0);
383  vec.X(T(atan2(-2 * (copy.qx*copy.qy - copy.qz*copy.qw),
384  squ - sqx + sqy - sqz)));
385  }
386  else
387  {
388  // Roll
389  vec.X(T(atan2(2 * (copy.qy*copy.qz + copy.qw*copy.qx),
390  squ - sqx - sqy + sqz)));
391 
392  // Yaw
393  vec.Z(T(atan2(2 * (copy.qx*copy.qy + copy.qw*copy.qz),
394  squ + sqx - sqy - sqz)));
395  }
396 
397  return vec;
398  }
399 
403  public: static Quaternion<T> EulerToQuaternion(const Vector3<T> &_vec)
404  {
405  Quaternion<T> result;
406  result.Euler(_vec);
407  return result;
408  }
409 
415  public: static Quaternion<T> EulerToQuaternion(T _x, T _y, T _z)
416  {
417  return EulerToQuaternion(Vector3<T>(_x, _y, _z));
418  }
419 
422  public: T Roll() const
423  {
424  return this->Euler().X();
425  }
426 
429  public: T Pitch() const
430  {
431  return this->Euler().Y();
432  }
433 
436  public: T Yaw() const
437  {
438  return this->Euler().Z();
439  }
440 
444  public: void ToAxis(Vector3<T> &_axis, T &_angle) const
445  {
446  T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz;
447  if (equal<T>(len, static_cast<T>(0)))
448  {
449  _angle = 0.0;
450  _axis.Set(1, 0, 0);
451  }
452  else
453  {
454  _angle = 2.0 * acos(this->qw);
455  T invLen = 1.0 / sqrt(len);
456  _axis.Set(this->qx*invLen, this->qy*invLen, this->qz*invLen);
457  }
458  }
459 
467  void Matrix(const Matrix3<T> &_mat)
468  {
469  const T trace = _mat(0, 0) + _mat(1, 1) + _mat(2, 2);
470  if (trace > 0.0000001)
471  {
472  qw = sqrt(1 + trace) / 2;
473  const T s = 1.0 / (4 * qw);
474  qx = (_mat(2, 1) - _mat(1, 2)) * s;
475  qy = (_mat(0, 2) - _mat(2, 0)) * s;
476  qz = (_mat(1, 0) - _mat(0, 1)) * s;
477  }
478  else if (_mat(0, 0) > _mat(1, 1) && _mat(0, 0) > _mat(2, 2))
479  {
480  qx = sqrt(1.0 + _mat(0, 0) - _mat(1, 1) - _mat(2, 2)) / 2;
481  const T s = 1.0 / (4 * qx);
482  qw = (_mat(2, 1) - _mat(1, 2)) * s;
483  qy = (_mat(1, 0) + _mat(0, 1)) * s;
484  qz = (_mat(0, 2) + _mat(2, 0)) * s;
485  }
486  else if (_mat(1, 1) > _mat(2, 2))
487  {
488  qy = sqrt(1.0 - _mat(0, 0) + _mat(1, 1) - _mat(2, 2)) / 2;
489  const T s = 1.0 / (4 * qy);
490  qw = (_mat(0, 2) - _mat(2, 0)) * s;
491  qx = (_mat(0, 1) + _mat(1, 0)) * s;
492  qz = (_mat(1, 2) + _mat(2, 1)) * s;
493  }
494  else
495  {
496  qz = sqrt(1.0 - _mat(0, 0) - _mat(1, 1) + _mat(2, 2)) / 2;
497  const T s = 1.0 / (4 * qz);
498  qw = (_mat(1, 0) - _mat(0, 1)) * s;
499  qx = (_mat(0, 2) + _mat(2, 0)) * s;
500  qy = (_mat(1, 2) + _mat(2, 1)) * s;
501  }
502  }
503 
513  public: void From2Axes(const Vector3<T> &_v1, const Vector3<T> &_v2)
514  {
515  // generally, we utilize the fact that a quat (w, x, y, z) represents
516  // rotation of angle 2*w about axis (x, y, z)
517  //
518  // so we want to take get a vector half-way between no rotation and the
519  // double rotation, which is
520  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ] / 2
521  // if _v1 and _v2 are unit quaternions
522  //
523  // since we normalize the result anyway, we can omit the division,
524  // getting the result:
525  // [ (1, (0, 0, 0)) + (_v1 dot _v2, _v1 x _v2) ].Normalized()
526  //
527  // if _v1 and _v2 are not normalized, the magnitude (1 + _v1 dot _v2)
528  // is multiplied by k = norm(_v1)*norm(_v2)
529 
530  const T kCosTheta = _v1.Dot(_v2);
531  const T k = sqrt(_v1.SquaredLength() * _v2.SquaredLength());
532 
533  if (fabs(kCosTheta/k + 1) < 1e-6)
534  {
535  // the vectors are opposite
536  // any vector orthogonal to _v1
537  Vector3<T> other;
538  {
539  const Vector3<T> _v1Abs(_v1.Abs());
540  if (_v1Abs.X() < _v1Abs.Y())
541  {
542  if (_v1Abs.X() < _v1Abs.Z())
543  {
544  other.Set(1, 0, 0);
545  }
546  else
547  {
548  other.Set(0, 0, 1);
549  }
550  }
551  else
552  {
553  if (_v1Abs.Y() < _v1Abs.Z())
554  {
555  other.Set(0, 1, 0);
556  }
557  else
558  {
559  other.Set(0, 0, 1);
560  }
561  }
562  }
563 
564  const Vector3<T> axis(_v1.Cross(other).Normalize());
565 
566  qw = 0;
567  qx = axis.X();
568  qy = axis.Y();
569  qz = axis.Z();
570  }
571  else
572  {
573  // the vectors are in general position
574  const Vector3<T> axis(_v1.Cross(_v2));
575  qw = kCosTheta + k;
576  qx = axis.X();
577  qy = axis.Y();
578  qz = axis.Z();
579  this->Normalize();
580  }
581  }
582 
585  public: void Scale(T _scale)
586  {
587  Quaternion<T> b;
588  Vector3<T> axis;
589  T angle;
590 
591  // Convert to axis-and-angle
592  this->ToAxis(axis, angle);
593  angle *= _scale;
594 
595  this->Axis(axis.X(), axis.Y(), axis.Z(), angle);
596  }
597 
601  public: Quaternion<T> operator+(const Quaternion<T> &_qt) const
602  {
603  Quaternion<T> result(this->qw + _qt.qw, this->qx + _qt.qx,
604  this->qy + _qt.qy, this->qz + _qt.qz);
605  return result;
606  }
607 
612  {
613  *this = *this + _qt;
614 
615  return *this;
616  }
617 
621  public: Quaternion<T> operator-(const Quaternion<T> &_qt) const
622  {
623  Quaternion<T> result(this->qw - _qt.qw, this->qx - _qt.qx,
624  this->qy - _qt.qy, this->qz - _qt.qz);
625  return result;
626  }
627 
632  {
633  *this = *this - _qt;
634  return *this;
635  }
636 
640  public: inline Quaternion<T> operator*(const Quaternion<T> &_q) const
641  {
642  return Quaternion<T>(
643  this->qw*_q.qw-this->qx*_q.qx-this->qy*_q.qy-this->qz*_q.qz,
644  this->qw*_q.qx+this->qx*_q.qw+this->qy*_q.qz-this->qz*_q.qy,
645  this->qw*_q.qy-this->qx*_q.qz+this->qy*_q.qw+this->qz*_q.qx,
646  this->qw*_q.qz+this->qx*_q.qy-this->qy*_q.qx+this->qz*_q.qw);
647  }
648 
652  public: Quaternion<T> operator*(const T &_f) const
653  {
654  return Quaternion<T>(this->qw*_f, this->qx*_f,
655  this->qy*_f, this->qz*_f);
656  }
657 
662  {
663  *this = *this * qt;
664  return *this;
665  }
666 
670  public: Vector3<T> operator*(const Vector3<T> &_v) const
671  {
672  Vector3<T> uv, uuv;
673  Vector3<T> qvec(this->qx, this->qy, this->qz);
674  uv = qvec.Cross(_v);
675  uuv = qvec.Cross(uv);
676  uv *= (2.0f * this->qw);
677  uuv *= 2.0f;
678 
679  return _v + uv + uuv;
680  }
681 
685  public: bool operator==(const Quaternion<T> &_qt) const
686  {
687  return equal(this->qx, _qt.qx, static_cast<T>(0.001)) &&
688  equal(this->qy, _qt.qy, static_cast<T>(0.001)) &&
689  equal(this->qz, _qt.qz, static_cast<T>(0.001)) &&
690  equal(this->qw, _qt.qw, static_cast<T>(0.001));
691  }
692 
696  public: bool operator!=(const Quaternion<T> &_qt) const
697  {
698  return !equal(this->qx, _qt.qx, static_cast<T>(0.001)) ||
699  !equal(this->qy, _qt.qy, static_cast<T>(0.001)) ||
700  !equal(this->qz, _qt.qz, static_cast<T>(0.001)) ||
701  !equal(this->qw, _qt.qw, static_cast<T>(0.001));
702  }
703 
706  public: Quaternion<T> operator-() const
707  {
708  return Quaternion<T>(-this->qw, -this->qx, -this->qy, -this->qz);
709  }
710 
714  public: inline Vector3<T> RotateVector(const Vector3<T> &_vec) const
715  {
716  Quaternion<T> tmp(static_cast<T>(0),
717  _vec.X(), _vec.Y(), _vec.Z());
718  tmp = (*this) * (tmp * this->Inverse());
719  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
720  }
721 
726  {
727  Quaternion<T> tmp(0.0, _vec.X(), _vec.Y(), _vec.Z());
728 
729  tmp = this->Inverse() * (tmp * (*this));
730 
731  return Vector3<T>(tmp.qx, tmp.qy, tmp.qz);
732  }
733 
736  public: bool IsFinite() const
737  {
738  // std::isfinite works with floating point values, need to explicit
739  // cast to avoid ambiguity in vc++.
740  return std::isfinite(static_cast<double>(this->qw)) &&
741  std::isfinite(static_cast<double>(this->qx)) &&
742  std::isfinite(static_cast<double>(this->qy)) &&
743  std::isfinite(static_cast<double>(this->qz));
744  }
745 
747  public: inline void Correct()
748  {
749  // std::isfinite works with floating point values, need to explicit
750  // cast to avoid ambiguity in vc++.
751  if (!std::isfinite(static_cast<double>(this->qx)))
752  this->qx = 0;
753  if (!std::isfinite(static_cast<double>(this->qy)))
754  this->qy = 0;
755  if (!std::isfinite(static_cast<double>(this->qz)))
756  this->qz = 0;
757  if (!std::isfinite(static_cast<double>(this->qw)))
758  this->qw = 1;
759 
760  if (equal(this->qw, static_cast<T>(0)) &&
761  equal(this->qx, static_cast<T>(0)) &&
762  equal(this->qy, static_cast<T>(0)) &&
763  equal(this->qz, static_cast<T>(0)))
764  {
765  this->qw = 1;
766  }
767  }
768 
771  public: Vector3<T> XAxis() const
772  {
773  T fTy = 2.0f*this->qy;
774  T fTz = 2.0f*this->qz;
775 
776  T fTwy = fTy*this->qw;
777  T fTwz = fTz*this->qw;
778  T fTxy = fTy*this->qx;
779  T fTxz = fTz*this->qx;
780  T fTyy = fTy*this->qy;
781  T fTzz = fTz*this->qz;
782 
783  return Vector3<T>(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
784  }
785 
788  public: Vector3<T> YAxis() const
789  {
790  T fTx = 2.0f*this->qx;
791  T fTy = 2.0f*this->qy;
792  T fTz = 2.0f*this->qz;
793  T fTwx = fTx*this->qw;
794  T fTwz = fTz*this->qw;
795  T fTxx = fTx*this->qx;
796  T fTxy = fTy*this->qx;
797  T fTyz = fTz*this->qy;
798  T fTzz = fTz*this->qz;
799 
800  return Vector3<T>(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
801  }
802 
805  public: Vector3<T> ZAxis() const
806  {
807  T fTx = 2.0f*this->qx;
808  T fTy = 2.0f*this->qy;
809  T fTz = 2.0f*this->qz;
810  T fTwx = fTx*this->qw;
811  T fTwy = fTy*this->qw;
812  T fTxx = fTx*this->qx;
813  T fTxz = fTz*this->qx;
814  T fTyy = fTy*this->qy;
815  T fTyz = fTz*this->qy;
816 
817  return Vector3<T>(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
818  }
819 
822  public: void Round(int _precision)
823  {
824  this->qx = precision(this->qx, _precision);
825  this->qy = precision(this->qy, _precision);
826  this->qz = precision(this->qz, _precision);
827  this->qw = precision(this->qw, _precision);
828  }
829 
833  public: T Dot(const Quaternion<T> &_q) const
834  {
835  return this->qw*_q.qw + this->qx * _q.qx +
836  this->qy*_q.qy + this->qz*_q.qz;
837  }
838 
849  public: static Quaternion<T> Squad(T _fT,
850  const Quaternion<T> &_rkP, const Quaternion<T> &_rkA,
851  const Quaternion<T> &_rkB, const Quaternion<T> &_rkQ,
852  bool _shortestPath = false)
853  {
854  T fSlerpT = 2.0f*_fT*(1.0f-_fT);
855  Quaternion<T> kSlerpP = Slerp(_fT, _rkP, _rkQ, _shortestPath);
856  Quaternion<T> kSlerpQ = Slerp(_fT, _rkA, _rkB);
857  return Slerp(fSlerpT, kSlerpP, kSlerpQ);
858  }
859 
868  public: static Quaternion<T> Slerp(T _fT,
869  const Quaternion<T> &_rkP, const Quaternion<T> &_rkQ,
870  bool _shortestPath = false)
871  {
872  T fCos = _rkP.Dot(_rkQ);
873  Quaternion<T> rkT;
874 
875  // Do we need to invert rotation?
876  if (fCos < 0.0f && _shortestPath)
877  {
878  fCos = -fCos;
879  rkT = -_rkQ;
880  }
881  else
882  {
883  rkT = _rkQ;
884  }
885 
886  if (std::abs(fCos) < 1 - 1e-03)
887  {
888  // Standard case (slerp)
889  T fSin = sqrt(1 - (fCos*fCos));
890  T fAngle = atan2(fSin, fCos);
891  // FIXME: should check if (std::abs(fSin) >= 1e-3)
892  T fInvSin = 1.0f / fSin;
893  T fCoeff0 = sin((1.0f - _fT) * fAngle) * fInvSin;
894  T fCoeff1 = sin(_fT * fAngle) * fInvSin;
895  return _rkP * fCoeff0 + rkT * fCoeff1;
896  }
897  else
898  {
899  // There are two situations:
900  // 1. "rkP" and "rkQ" are very close (fCos ~= +1),
901  // so we can do a linear interpolation safely.
902  // 2. "rkP" and "rkQ" are almost inverse of each
903  // other (fCos ~= -1), there
904  // are an infinite number of possibilities interpolation.
905  // but we haven't have method to fix this case, so just use
906  // linear interpolation here.
907  Quaternion<T> t = _rkP * (1.0f - _fT) + rkT * _fT;
908  // taking the complement requires renormalisation
909  t.Normalize();
910  return t;
911  }
912  }
913 
922  public: Quaternion<T> Integrate(const Vector3<T> &_angularVelocity,
923  const T _deltaT) const
924  {
925  Quaternion<T> deltaQ;
926  Vector3<T> theta = _angularVelocity * _deltaT * 0.5;
927  T thetaMagSq = theta.SquaredLength();
928  T s;
929  if (thetaMagSq * thetaMagSq / 24.0 < MIN_D)
930  {
931  deltaQ.W() = 1.0 - thetaMagSq / 2.0;
932  s = 1.0 - thetaMagSq / 6.0;
933  }
934  else
935  {
936  double thetaMag = sqrt(thetaMagSq);
937  deltaQ.W() = cos(thetaMag);
938  s = sin(thetaMag) / thetaMag;
939  }
940  deltaQ.X() = theta.X() * s;
941  deltaQ.Y() = theta.Y() * s;
942  deltaQ.Z() = theta.Z() * s;
943  return deltaQ * (*this);
944  }
945 
948  public: inline const T &W() const
949  {
950  return this->qw;
951  }
952 
955  public: inline const T &X() const
956  {
957  return this->qx;
958  }
959 
962  public: inline const T &Y() const
963  {
964  return this->qy;
965  }
966 
969  public: inline const T &Z() const
970  {
971  return this->qz;
972  }
973 
974 
977  public: inline T &W()
978  {
979  return this->qw;
980  }
981 
984  public: inline T &X()
985  {
986  return this->qx;
987  }
988 
991  public: inline T &Y()
992  {
993  return this->qy;
994  }
995 
998  public: inline T &Z()
999  {
1000  return this->qz;
1001  }
1002 
1005  public: inline void X(T _v)
1006  {
1007  this->qx = _v;
1008  }
1009 
1012  public: inline void Y(T _v)
1013  {
1014  this->qy = _v;
1015  }
1016 
1019  public: inline void Z(T _v)
1020  {
1021  this->qz = _v;
1022  }
1023 
1026  public: inline void W(T _v)
1027  {
1028  this->qw = _v;
1029  }
1030 
1035  public: friend std::ostream &operator<<(std::ostream &_out,
1036  const ignition::math::Quaternion<T> &_q)
1037  {
1038  Vector3<T> v(_q.Euler());
1039  _out << precision(v.X(), 6) << " " << precision(v.Y(), 6) << " "
1040  << precision(v.Z(), 6);
1041  return _out;
1042  }
1043 
1048  public: friend std::istream &operator>>(std::istream &_in,
1049  ignition::math::Quaternion<T> &_q)
1050  {
1051  Angle roll, pitch, yaw;
1052 
1053  // Skip white spaces
1054  _in.setf(std::ios_base::skipws);
1055  _in >> roll >> pitch >> yaw;
1056 
1057  _q.Euler(Vector3<T>(*roll, *pitch, *yaw));
1058 
1059  return _in;
1060  }
1061 
1063  private: T qw;
1064 
1066  private: T qx;
1067 
1069  private: T qy;
1070 
1072  private: T qz;
1073  };
1074 
1075  template<typename T> const Quaternion<T>
1076  Quaternion<T>::Identity(1, 0, 0, 0);
1077 
1078  template<typename T> const Quaternion<T>
1079  Quaternion<T>::Zero(0, 0, 0, 0);
1080 
1084  }
1085  }
1086 }
1087 #endif
Quaternion(const Quaternion< T > &_qt)
Copy constructor.
Definition: Quaternion.hh:96
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:579
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition: Quaternion.hh:833
Vector3< T > RotateVectorReverse(Vector3< T > _vec) const
Do the reverse rotation of a vector by this quaternion.
Definition: Quaternion.hh:725
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition: Quaternion.hh:290
static Quaternion< T > Slerp(T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkQ, bool _shortestPath=false)
Spherical linear interpolation between 2 quaternions, given the ends and an interpolation parameter b...
Definition: Quaternion.hh:868
T Roll() const
Get the Euler roll angle in radians.
Definition: Quaternion.hh:422
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition: Quaternion.hh:40
void W(T _v)
Set the w component.
Definition: Quaternion.hh:1026
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition: Quaternion.hh:714
void Matrix(const Matrix3< T > &_mat)
Set from a rotation matrix.
Definition: Quaternion.hh:467
void Axis(const Vector3< T > &_axis, T _a)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:280
Quaternion< T > & operator=(const Quaternion< T > &_qt)
Equal operator.
Definition: Quaternion.hh:109
A quaternion class.
Definition: Matrix3.hh:33
T & X()
Get a mutable x component.
Definition: Quaternion.hh:984
Quaternion< T > Log() const
Return the logarithm.
Definition: Quaternion.hh:160
T Y() const
Get the y value.
Definition: Vector3.hh:654
static const double MIN_D
Double min value. This value will be similar to 2.22507e-308.
Definition: Helpers.hh:249
void Scale(T _scale)
Scale a Quaternion<T>ion.
Definition: Quaternion.hh:585
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition: Quaternion.hh:696
Quaternion< T > Exp() const
Return the exponent.
Definition: Quaternion.hh:192
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:312
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition: Quaternion.hh:66
bool equal(const T &_a, const T &_b, const T &_epsilon=T(1e-6))
check if two values are equal, within a tolerance
Definition: Helpers.hh:545
Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
Constructor.
Definition: Quaternion.hh:58
Vector3< T > XAxis() const
Return the X axis.
Definition: Quaternion.hh:771
An angle and related functions.
Definition: Angle.hh:47
Quaternion(const Matrix3< T > &_mat)
Construct from rotation matrix.
Definition: Quaternion.hh:89
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition: Vector3.hh:188
static Quaternion< T > Squad(T _fT, const Quaternion< T > &_rkP, const Quaternion< T > &_rkA, const Quaternion< T > &_rkB, const Quaternion< T > &_rkQ, bool _shortestPath=false)
Spherical quadratic interpolation given the ends and an interpolation parameter between 0 and 1.
Definition: Quaternion.hh:849
Quaternion< int > Quaternioni
Definition: Quaternion.hh:1083
Quaternion< T > operator-(const Quaternion< T > &_qt) const
Subtraction operator.
Definition: Quaternion.hh:621
const T & Z() const
Get the z component.
Definition: Quaternion.hh:969
Quaternion()
Default Constructor.
Definition: Quaternion.hh:46
void Correct()
Correct any nan values in this quaternion.
Definition: Quaternion.hh:747
T Z() const
Get the z value.
Definition: Vector3.hh:661
const T & W() const
Get the w component.
Definition: Quaternion.hh:948
const T & Y() const
Get the y component.
Definition: Quaternion.hh:962
T & Z()
Get a mutable z component.
Definition: Quaternion.hh:998
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition: Quaternion.hh:334
void Axis(T _ax, T _ay, T _az, T _aa)
Set the quaternion from an axis and angle.
Definition: Quaternion.hh:251
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition: Vector3.hh:123
T & Y()
Get a mutable y component.
Definition: Quaternion.hh:991
Quaternion(const Vector3< T > &_axis, const T &_angle)
Constructor from axis angle.
Definition: Quaternion.hh:74
void Y(T _v)
Set the y component.
Definition: Quaternion.hh:1012
bool operator==(const Quaternion< T > &_qt) const
Equal to operator.
Definition: Quaternion.hh:685
Quaternion< double > Quaterniond
Definition: Quaternion.hh:1081
const T & X() const
Get the x component.
Definition: Quaternion.hh:955
void Invert()
Invert the quaternion.
Definition: Quaternion.hh:120
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition: Vector3.hh:178
T X() const
Get the x value.
Definition: Vector3.hh:647
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition: Quaternion.hh:736
Quaternion< T > operator-=(const Quaternion< T > &_qt)
Subtraction operator.
Definition: Quaternion.hh:631
Quaternion< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition: Quaternion.hh:601
Quaternion< T > operator *(const Quaternion< T > &_q) const
Multiplication operator.
Definition: Quaternion.hh:640
void ToAxis(Vector3< T > &_axis, T &_angle) const
Return rotation as axis and angle.
Definition: Quaternion.hh:444
void Round(int _precision)
Round all values to _precision decimal places.
Definition: Quaternion.hh:822
static const Quaternion Zero
math::Quaternion(0, 0, 0, 0)
Definition: Quaternion.hh:43
void Normalize()
Normalize the quaternion.
Definition: Quaternion.hh:223
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition: Quaternion.hh:303
void Z(T _v)
Set the z component.
Definition: Quaternion.hh:1019
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition: Vector3.hh:198
Quaternion(const Vector3< T > &_rpy)
Constructor.
Definition: Quaternion.hh:81
void X(T _v)
Set the x component.
Definition: Quaternion.hh:1005
T Pitch() const
Get the Euler pitch angle in radians.
Definition: Quaternion.hh:429
T Yaw() const
Get the Euler yaw angle in radians.
Definition: Quaternion.hh:436
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition: Quaternion.hh:403
Vector3< T > YAxis() const
Return the Y axis.
Definition: Quaternion.hh:788
Vector3< T > ZAxis() const
Return the Z axis.
Definition: Quaternion.hh:805
Vector3 Abs() const
Get the absolute value of the vector.
Definition: Vector3.hh:222
Quaternion< T > operator *=(const Quaternion< T > &qt)
Multiplication operator.
Definition: Quaternion.hh:661
Quaternion< T > operator-() const
Unary minus operator.
Definition: Quaternion.hh:706
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:131
Definition: Angle.hh:39
void From2Axes(const Vector3< T > &_v1, const Vector3< T > &_v2)
Set this quaternion to represent rotation from vector _v1 to vector _v2, so that _v2....
Definition: Quaternion.hh:513
~Quaternion()
Destructor.
Definition: Quaternion.hh:105
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:174
T & W()
Get a mutable w component.
Definition: Quaternion.hh:977
A 3x3 matrix class.
Definition: Matrix3.hh:38
friend std::istream & operator>>(std::istream &_in, ignition::math::Quaternion< T > &_q)
Stream extraction operator.
Definition: Quaternion.hh:1048
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition: Quaternion.hh:415
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Quaternion< T > &_q)
Stream insertion operator.
Definition: Quaternion.hh:1035
Quaternion< T > Integrate(const Vector3< T > &_angularVelocity, const T _deltaT) const
Integrate quaternion for constant angular velocity vector along specified interval _deltaT.
Definition: Quaternion.hh:922
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition: Quaternion.hh:611
Quaternion< float > Quaternionf
Definition: Quaternion.hh:1082