Loading...
Searching...
No Matches
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
24#include <ignition/math/config.hh>
25
26namespace 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>
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
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 {
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,
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,
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
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition Helpers.hh:174
An angle and related functions.
Definition Angle.hh:48
A 3x3 matrix class.
Definition Quaternion.hh:32
A quaternion class.
Definition Quaternion.hh:38
void Matrix(const Matrix3< T > &_mat)
Set from a rotation matrix.
Definition Quaternion.hh:467
void X(T _v)
Set the x component.
Definition Quaternion.hh:1005
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)
Equal operator.
Definition Quaternion.hh:109
T & Y()
Get a mutable y component.
Definition Quaternion.hh:991
friend std::ostream & operator<<(std::ostream &_out, const ignition::math::Quaternion< T > &_q)
Stream insertion operator.
Definition Quaternion.hh:1035
static Quaternion< T > EulerToQuaternion(const Vector3< T > &_vec)
Convert euler angles to quatern.
Definition Quaternion.hh:403
void Normalize()
Normalize the quaternion.
Definition Quaternion.hh:223
T Roll() const
Get the Euler roll angle in radians.
Definition Quaternion.hh:422
Quaternion(const Matrix3< T > &_mat)
Construct from rotation matrix.
Definition Quaternion.hh:89
Vector3< T > RotateVector(const Vector3< T > &_vec) const
Rotate a vector using the quaternion.
Definition Quaternion.hh:714
void Invert()
Invert the quaternion.
Definition Quaternion.hh:120
const T & X() const
Get the x component.
Definition Quaternion.hh:955
Vector3< T > Euler() const
Return the rotation in Euler angles.
Definition Quaternion.hh:334
Vector3< T > XAxis() const
Return the X axis.
Definition Quaternion.hh:771
Quaternion< T > operator*(const T &_f) const
Multiplication operator by a scalar.
Definition Quaternion.hh:652
Quaternion< T > operator*(const Quaternion< T > &_q) const
Multiplication operator.
Definition Quaternion.hh:640
static Quaternion< T > EulerToQuaternion(T _x, T _y, T _z)
Convert euler angles to quatern.
Definition Quaternion.hh:415
Quaternion(const T &_w, const T &_x, const T &_y, const T &_z)
Constructor.
Definition Quaternion.hh:58
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 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
const T & Y() const
Get the y component.
Definition Quaternion.hh:962
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
void Euler(const Vector3< T > &_vec)
Set the quaternion from Euler angles.
Definition Quaternion.hh:303
Quaternion< T > operator-(const Quaternion< T > &_qt) const
Subtraction operator.
Definition Quaternion.hh:621
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
Vector3< T > YAxis() const
Return the Y axis.
Definition Quaternion.hh:788
Quaternion< T > operator-() const
Unary minus operator.
Definition Quaternion.hh:706
T Dot(const Quaternion< T > &_q) const
Dot product.
Definition Quaternion.hh:833
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition Quaternion.hh:131
bool IsFinite() const
See if a quaternion is finite (e.g., not nan)
Definition Quaternion.hh:736
T & X()
Get a mutable x component.
Definition Quaternion.hh:984
T & Z()
Get a mutable z component.
Definition Quaternion.hh:998
Vector3< T > RotateVectorReverse(Vector3< T > _vec) const
Do the reverse rotation of a vector by this quaternion.
Definition Quaternion.hh:725
Quaternion(const Vector3< T > &_axis, const T &_angle)
Constructor from axis angle.
Definition Quaternion.hh:74
Quaternion< T > Log() const
Return the logarithm.
Definition Quaternion.hh:160
static const Quaternion Zero
math::Quaternion(0, 0, 0, 0)
Definition Quaternion.hh:43
void Z(T _v)
Set the z component.
Definition Quaternion.hh:1019
~Quaternion()
Destructor.
Definition Quaternion.hh:105
void Euler(T _roll, T _pitch, T _yaw)
Set the quaternion from Euler angles.
Definition Quaternion.hh:312
T Yaw() const
Get the Euler yaw angle in radians.
Definition Quaternion.hh:436
const T & Z() const
Get the z component.
Definition Quaternion.hh:969
Quaternion< T > operator-=(const Quaternion< T > &_qt)
Subtraction operator.
Definition Quaternion.hh:631
void Correct()
Correct any nan values in this quaternion.
Definition Quaternion.hh:747
void W(T _v)
Set the w component.
Definition Quaternion.hh:1026
Quaternion< T > operator*=(const Quaternion< T > &qt)
Multiplication operator.
Definition Quaternion.hh:661
Quaternion< T > Exp() const
Return the exponent.
Definition Quaternion.hh:192
Quaternion(const Vector3< T > &_rpy)
Constructor.
Definition Quaternion.hh:81
Vector3< T > operator*(const Vector3< T > &_v) const
Vector3 multiplication operator.
Definition Quaternion.hh:670
void Axis(const Vector3< T > &_axis, T _a)
Set the quaternion from an axis and angle.
Definition Quaternion.hh:280
bool operator!=(const Quaternion< T > &_qt) const
Not equal to operator.
Definition Quaternion.hh:696
Quaternion< T > operator+=(const Quaternion< T > &_qt)
Addition operator.
Definition Quaternion.hh:611
friend std::istream & operator>>(std::istream &_in, ignition::math::Quaternion< T > &_q)
Stream extraction operator.
Definition Quaternion.hh:1048
void Scale(T _scale)
Scale a Quaternion<T>ion.
Definition Quaternion.hh:585
Quaternion(const T &_roll, const T &_pitch, const T &_yaw)
Constructor from Euler angles in radians.
Definition Quaternion.hh:66
void Axis(T _ax, T _ay, T _az, T _aa)
Set the quaternion from an axis and angle.
Definition Quaternion.hh:251
static const Quaternion Identity
math::Quaternion(1, 0, 0, 0)
Definition Quaternion.hh:40
const T & W() const
Get the w component.
Definition Quaternion.hh:948
Quaternion()
Default Constructor.
Definition Quaternion.hh:46
T Pitch() const
Get the Euler pitch angle in radians.
Definition Quaternion.hh:429
Vector3< T > ZAxis() const
Return the Z axis.
Definition Quaternion.hh:805
void Set(T _w, T _x, T _y, T _z)
Set this quaternion from 4 floating numbers.
Definition Quaternion.hh:290
T & W()
Get a mutable w component.
Definition Quaternion.hh:977
Quaternion(const Quaternion< T > &_qt)
Copy constructor.
Definition Quaternion.hh:96
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< T > operator+(const Quaternion< T > &_qt) const
Addition operator.
Definition Quaternion.hh:601
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
Vector3 Abs() const
Get the absolute value of the vector.
Definition Vector3.hh:222
void Set(T _x=0, T _y=0, T _z=0)
Set the contents of the vector.
Definition Vector3.hh:178
T Z() const
Get the z value.
Definition Vector3.hh:661
T SquaredLength() const
Return the square of the length (magnitude) of the vector.
Definition Vector3.hh:123
T Y() const
Get the y value.
Definition Vector3.hh:654
Vector3 Normalize()
Normalize the vector length.
Definition Vector3.hh:132
T Dot(const Vector3< T > &_v) const
Return the dot product of this vector and another vector.
Definition Vector3.hh:198
Vector3 Cross(const Vector3< T > &_v) const
Return the cross product of this vector with another vector.
Definition Vector3.hh:188
T X() const
Get the x value.
Definition Vector3.hh:647
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition Helpers.hh:579
Quaternion< int > Quaternioni
Definition Quaternion.hh:1083
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
static const double MIN_D
Double min value. This value will be similar to 2.22507e-308.
Definition Helpers.hh:249
Quaternion< double > Quaterniond
Definition Quaternion.hh:1081
Quaternion< float > Quaternionf
Definition Quaternion.hh:1082
Definition Angle.hh:40