Ocean
Loading...
Searching...
No Matches
Quaternion.h
Go to the documentation of this file.
1/*
2 * Copyright (c) Meta Platforms, Inc. and affiliates.
3 *
4 * This source code is licensed under the MIT license found in the
5 * LICENSE file in the root directory of this source tree.
6 */
7
8#ifndef META_OCEAN_MATH_QUATERNION_H
9#define META_OCEAN_MATH_QUATERNION_H
10
11#include "ocean/math/Math.h"
12#include "ocean/math/Vector3.h"
13#include "ocean/math/Vector4.h"
14
16
17namespace Ocean
18{
19
20// Forward declaration.
21template <typename T> class RotationT;
22
23// Forward declaration.
24template <typename T> class EulerT;
25
26// Forward declaration.
27template <typename T> class SquareMatrixT3;
28
29// Forward declaration.
30template <typename T> class HomogenousMatrixT4;
31
32// Forward declaration.
33template <typename T> class QuaternionT;
34
35/**
36 * Definition of the Quaternion object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION either with single or double precision float data type.
37 * @see QuaternionT
38 * @ingroup math
39 */
41
42/**
43 * Instantiation of the QuaternionT template class using a double precision float data type.
44 * @see QuaternionT
45 * @ingroup math
46 */
48
49/**
50 * Instantiation of the QuaternionT template class using a single precision float data type.
51 * @see QuaternionT
52 * @ingroup math
53 */
55
56/**
57 * Definition of a typename alias for vectors with QuaternionT objects.
58 * @see QuaternionT
59 * @ingroup math
60 */
61template <typename T>
62using QuaternionsT = std::vector<QuaternionT<T>>;
63
64/**
65 * Definition of a vector holding quaternion objects.
66 * @see Quaternion
67 * @ingroup math
68 */
69typedef std::vector<Quaternion> Quaternions;
70
71/**
72 * Definition of a vector holding quaternion objects with single precision float data type.
73 * @see Quaternion
74 * @ingroup math
75 */
76typedef std::vector<QuaternionF> QuaternionsF;
77
78/**
79 * Definition of a vector holding quaternion objects with double precision float data type.
80 * @see Quaternion
81 * @ingroup math
82 */
83typedef std::vector<QuaternionD> QuaternionsD;
84
85/**
86 * This class implements a unit quaternion rotation.
87 * A quaternion is defined by four values:
88 * <pre>
89 * w + xi + yj + zk
90 * with
91 * i * i = -1, j * j = -1, k * k = -1
92 * </pre>
93 * The elements are stored in the following order: w, x, y, z
94 * @tparam T Data type used to represent the quaternion values
95 * @see Quaternion, QuaternionF, QuaternionD, Rotation, Euler, SquareMatrix3, ExponentialMap.
96 * @ingroup math
97 */
98template <typename T>
100{
101 public:
102
103 /**
104 * Definition of the used data type.
105 */
106 typedef T Type;
107
108 public:
109
110 /**
111 * Creates a new quaternion with default values, representation an identity rotation.
112 */
113 QuaternionT() = default;
114
115 /**
116 * Creates a new quaternion with either default values (representation an identity rotation) or an invalid rotation.
117 * @param toIdentity True, to create an identity rotation; False, to created an invalid rotation
118 */
119 explicit QuaternionT(const bool toIdentity);
120
121 /**
122 * Copies a quaternion with different element data type than T.
123 * @param quaternion The quaternion object to be copied
124 * @tparam U The element data type of the given quaternion
125 */
126 template <typename U>
127 explicit inline QuaternionT(const QuaternionT<U>& quaternion);
128
129 /**
130 * Creates a new quaternion by four given values.
131 * @param w W value of the quaternion, with range (-infinity, infinity)
132 * @param x X value of the quaternion, with range (-infinity, infinity)
133 * @param y Y value of the quaternion, with range (-infinity, infinity)
134 * @param z Z value of the quaternion, with range (-infinity, infinity)
135 */
136 QuaternionT(const T w, const T x, const T y, const T z);
137
138 /**
139 * Creates a new quaternion by a given axis and rotation angle.
140 * @param axis The axis of the angle-axis rotation with unit length
141 * @param angle The angle of the angle-axis rotation in radian, with range (-infinity, infinity), however will be converted to the range [0.0, 2 * PI)
142 */
143 QuaternionT(const VectorT3<T>& axis, const T angle);
144
145 /**
146 * Creates a quaternion object based on two given unit vectors.
147 * The resulting rotation defines a transformation that rotates that reference vector into the offset vector: Quaternion(reference, offset) = offset_Q_reference.<br>
148 * The following equation holds:
149 * <pre>
150 * offset = Quaternion(reference, offset) * reference.
151 * </pre>
152 * @param reference The reference vector, with length 1
153 * @param offset The offset vector, with length 1
154 */
155 QuaternionT(const VectorT3<T>& reference, const VectorT3<T>& offset);
156
157 /**
158 * Creates a new quaternion by a given angle-axis rotation.
159 * @param rotation The rotation to create the quaternion from, must be valid
160 */
161 explicit QuaternionT(const RotationT<T>& rotation);
162
163 /**
164 * Creates a new quaternion by a given Euler rotation.
165 * @param euler Rotation to create the quaternion from, must be valid
166 */
167 explicit QuaternionT(const EulerT<T>& euler);
168
169 /**
170 * Creates a new quaternion by a given 3x3 rotation matrix.
171 * @param matrix Rotation matrix to create the quaternion from, with determinant 1
172 */
173 explicit QuaternionT(const SquareMatrixT3<T>& matrix);
174
175 /**
176 * Creates a new quaternion by a given 4x4 homogeneous transformation matrix.
177 * @param transformation The transformation matrix to create the quaternion from
178 */
179 explicit QuaternionT(const HomogenousMatrixT4<T>& transformation);
180
181 /**
182 * Creates a new quaternion by a 4D vector.
183 * @param vector The vector holding the four quaternion parameter
184 */
185 explicit QuaternionT(const VectorT4<T>& vector);
186
187 /**
188 * Creates a new quaternion by an array with at least four elements.
189 * @param arrayValue Array with elements, must be valid
190 */
191 explicit QuaternionT(const T* arrayValue);
192
193 /**
194 * Returns the normalized quaternion.
195 * Beware: This function does not throw an exception if the quaternion is singular and cannot be normalized.<br>
196 * Thus ensure that the quaternion is valid before calling this function.<br>
197 * In case, this quaternion is not valid, a default quaternion will be returned.
198 * @return Normalized quaternion
199 * @see isValid(), normalize().
200 */
202
203 /**
204 * Normalizes the quaternion in place.
205 * @return True, if the quaternion is not singular and could be normalized.
206 * @see normalized().
207 */
208 bool normalize();
209
210 /**
211 * Normalizes the quaternion and returns the result as parameter.
212 * @param normalizedQuaternion The resulting normalized quaternion
213 * @return True, if the quaternion is not singular and could be normalized.
214 * @see normalized().
215 */
216 bool normalize(QuaternionT<T>& normalizedQuaternion) const;
217
218 /**
219 * Returns the inverted quaternion.
220 * Beware: This function does not throw an exception if the quaternion is singular and cannot be inverted.<br>
221 * Thus ensure that the quaternion is valid before calling this function.<br>
222 * In case, this quaternion is not valid, a default quaternion will be returned.
223 * @return Inverted quaternion
224 * @see isValid(), invert().
225 */
227
228 /**
229 * Inverts this quaternion in place.
230 * @return True, if the quaternion is not singular and could be inverted.
231 * @see inverted().
232 */
233 bool invert();
234
235 /**
236 * Inverts this quaternion and returns the result as parameter.
237 * @param invertedQuaternion The resulting inverted quaternion
238 * @return True, if the quaternion is not singular and could be inverted.
239 * @see inverted().
240 */
241 bool invert(QuaternionT<T>& invertedQuaternion) const;
242
243 /**
244 * Returns the conjugate of this quaternion.
245 * The conjugated quaternion is defined as: (w - xi - yj - zk)
246 * @return Conjugate quaternion
247 */
249
250 /**
251 * Returns the norm of this quaternion.
252 * @return Quaternion norm
253 */
254 T norm() const;
255
256 /**
257 * Returns the square of the quaternion norm.
258 * @return Square of quaternion norm
259 */
260 T sqr() const;
261
262 /**
263 * Returns the dot product between this quaternion and a second quaternion.
264 * The function actually returns: w0 * w1 + x0 * x1 + y0 * y1 + z0 * z1, with quaternions (w0, x0, y0, z0) and (w1, x1, y1, z1).
265 * @param quaternion The second quaternion for dot product calculation
266 * @return The dot product between both quaternions
267 */
268 T dot(const QuaternionT<T>& quaternion) const;
269
270 /**
271 * Returns the rotation angle defined by the quaternion.
272 * This angle is calculated based on the real part of the quaternion.<br>
273 * Beware: Ensure that this quaternion is valid.
274 * @return Rotation angle in radian, with range [0, 2PI), 0 in case of an invalid quaternion
275 */
276 T angle() const;
277
278 /**
279 * Returns the angle between two quaternion rotations.
280 * This function may return angles larger than PI.
281 * Beware: Ensure that this quaternion is valid.
282 * @param quaternion Second quaternion for angle determination, must be valid
283 * @return Rotation angle in radian, with range [0, 2PI), 0 in case of an invalid quaternion
284 * @see smallestAngle().
285 */
286 T angle(const QuaternionT<T>& quaternion) const;
287
288 /**
289 * Returns the smallest angle between two quaternion rotations.
290 * In contrast to angle(), this function returns the smallest (absolute) angle with range [0, PI).
291 * Beware: Ensure that this quaternion is valid.
292 * @param quaternion Second quaternion for angle determination, must be valid
293 * @return Rotation angle in radian, with range [0, PI), 0 in case of an invalid quaternion
294 * @see angle().
295 */
296 T smallestAngle(const QuaternionT<T>& quaternion) const;
297
298 /**
299 * Returns the cosine value of the half angle between two quaternion rotations.
300 * To determine whether two quaternion rotations have a lesser angle offset than 30 degrees<br>
301 * this cos2 value must be larger than the cosine of 15 degrees.<br>
302 * Beware: Ensure that this quaternion is valid.
303 * @param quaternion Second quaternion for value determination, must be valid
304 * @return Cosine value of the half angle
305 */
306 T cos2(const QuaternionT<T>& quaternion) const;
307
308 /**
309 * Spherical linear interpolation between two quaternions.
310 * This quaternion will be the resulting interpolation if 'factor == 0'.
311 * @param quaternion The right quaternion which will be the resulting interpolation if 'factor == 1'
312 * @param factor Interpolation factor has to be between [0, 1]
313 * @return The interpolated quaternion
314 */
315 QuaternionT<T> slerp(const QuaternionT<T>& quaternion, T factor) const;
316
317 /**
318 * Returns whether this quaternion is a valid unit quaternion.
319 * @return True, if so
320 */
321 bool isValid() const;
322
323 /**
324 * Returns the w value of the quaternion.
325 * @return w value
326 */
327 inline const T& w() const;
328
329 /**
330 * Returns the w value of the quaternion.
331 * @return w value
332 */
333 inline T& w();
334
335 /**
336 * Returns the x value of the quaternion.
337 * @return x value
338 */
339 inline const T& x() const;
340
341 /**
342 * Returns the x value of the quaternion.
343 * @return x value
344 */
345 inline T& x();
346
347 /**
348 * Returns the y value of the quaternion.
349 * @return x value
350 */
351 inline const T& y() const;
352
353 /**
354 * Returns the y value of the quaternion.
355 * @return x value
356 */
357 inline T& y();
358
359 /**
360 * Returns the z value of the quaternion.
361 * @return z value
362 */
363 inline const T& z() const;
364
365 /**
366 * Returns the z value of the quaternion.
367 * @return z value
368 */
369 inline T& z();
370
371 /**
372 * Returns whether two quaternions are equal up to a specified epsilon.
373 * @param quaternion Second quaternion to compare
374 * @param eps The epsilon to be used, with range [0, infinity)
375 * @return True, if so
376 */
377 inline bool isEqual(const QuaternionT<T>& quaternion, const T eps) const;
378
379 /**
380 * Returns whether two quaternions are identical up to a small epsilon.
381 * @param right The right quaternion
382 * @return True, if so
383 */
384 bool operator==(const QuaternionT<T>& right) const;
385
386 /**
387 * Returns whether two quaternions are not identical up to a small epsilon.
388 * @param right The right quaternion
389 * @return True, if so
390 */
391 inline bool operator!=(const QuaternionT<T>& right) const;
392
393 /**
394 * Combines two quaternion to a new combined rotation.
395 * @param right The right quaternion
396 * @return Combined quaternion
397 */
399
400 /**
401 * Combines a angle-axis rotation with this quaternion.
402 * @param right The right angle-axis rotation
403 * @return Combined quaternion rotation
404 */
405 inline QuaternionT<T> operator*(const RotationT<T>& right) const;
406
407 /**
408 * Rotates a 3D vector by this quaternion.
409 * @param vector 3D vector to rotate
410 * @return Rotated 3D vector
411 */
412 VectorT3<T> operator*(const VectorT3<T>& vector) const;
413
414 /**
415 * Combines and assigns two quaternions.
416 * @param right The right quaternion
417 * @return Reference to this quaternion
418 */
420
421 /**
422 * Combines and assigns a quaternion with a rotation.
423 * @param right The right quaternion
424 * @return Reference to this quaternion
425 */
427
428 /**
429 * Element operator.
430 * Beware: No range check will be done!
431 * @param index The index of the element to return [0, 3]
432 * @return Specified element
433 */
434 inline T operator[](const unsigned int index) const;
435
436 /**
437 * Element operator.
438 * Beware: No range check will be done!
439 * @param index The index of the element to return [0, 3]
440 * @return Specified element
441 */
442 inline T& operator[](const unsigned int index);
443
444 /**
445 * Element operator.
446 * Beware: No range check will be done!
447 * @param index The index of the element to return [0, 3]
448 * @return Specified element
449 */
450 inline T operator()(const unsigned int index) const;
451
452 /**
453 * Element operator.
454 * Beware: No range check will be done!
455 * @param index The index of the element to return [0, 3]
456 * @return Specified element
457 */
458 inline T& operator()(const unsigned int index);
459
460 /**
461 * Access operator.
462 * @return Pointer to the internal values
463 */
464 inline const T* operator()() const;
465
466 /**
467 * Access operator.
468 * @return Pointer to the internal values
469 */
470 inline T* operator()();
471
472 protected:
473
474 /// The four values of the quaternion.
475 T values_[4] = {T(1), T(0), T(0), T(0)};
476};
477
478template <typename T>
479QuaternionT<T>::QuaternionT(const bool toIdentity)
480{
481 if (toIdentity)
482 {
483 values_[0] = T(1);
484 values_[1] = T(0);
485 values_[2] = T(0);
486 values_[3] = T(0);
487
488 ocean_assert(isValid() == true);
489 }
490 else
491 {
492 values_[0] = T(0);
493 values_[1] = T(0);
494 values_[2] = T(0);
495 values_[3] = T(0);
496
497 ocean_assert(isValid() == false);
498 }
499}
500
501template <typename T>
502template <typename U>
504{
505 values_[0] = T(quaternion[0]);
506 values_[1] = T(quaternion[1]);
507 values_[2] = T(quaternion[2]);
508 values_[3] = T(quaternion[3]);
509}
510
511template <typename T>
512QuaternionT<T>::QuaternionT(const T w, const T x, const T y, const T z)
513{
514 values_[0] = w;
515 values_[1] = x;
516 values_[2] = y;
517 values_[3] = z;
518}
519
520template <typename T>
521QuaternionT<T>::QuaternionT(const VectorT3<T>& axis, const T angle)
522{
523 ocean_assert_accuracy(axis.isUnit(NumericT<T>::weakEps()));
524
525 const T angleValue = angle * T(0.5);
526 const T sinValue = NumericT<T>::sin(angleValue);
527
528 values_[0] = NumericT<T>::cos(angleValue);
529 values_[1] = sinValue * axis[0];
530 values_[2] = sinValue * axis[1];
531 values_[3] = sinValue * axis[2];
532
533 ocean_assert_accuracy(isValid());
534}
535
536template <typename T>
538{
539 ocean_assert_accuracy(reference.isUnit(NumericT<T>::weakEps()));
540 ocean_assert_accuracy(offset.isUnit(NumericT<T>::weakEps()));
541
542 if (reference == offset)
543 {
544 values_[0] = T(1);
545 values_[1] = T(0);
546 values_[2] = T(0);
547 values_[3] = T(0);
548 }
549 else if (reference == -offset)
550 {
551 const VectorT3<T> perpendicular(reference.perpendicular().normalized());
552
553 values_[0] = T(0);
554 values_[1] = perpendicular[0];
555 values_[2] = perpendicular[1];
556 values_[3] = perpendicular[2];
557 }
558 else
559 {
560 const VectorT3<T> axis(reference.cross(offset));
561
562 values_[0] = T(1) + reference * offset;
563 values_[1] = axis.x();
564 values_[2] = axis.y();
565 values_[3] = axis.z();
566
567 normalize();
568 }
569
570 ocean_assert(isValid());
571 ocean_assert_accuracy(offset.isEqual(*this * reference, NumericT<T>::weakEps()));
572}
573
574template <typename T>
576{
577 ocean_assert(rotation.isValid());
578
579 const T angle = rotation.angle() * T(0.5);
580 const T sinValue = NumericT<T>::sin(angle);
581
582 values_[0] = NumericT<T>::cos(angle);
583 values_[1] = sinValue * rotation[0];
584 values_[2] = sinValue * rotation[1];
585 values_[3] = sinValue * rotation[2];
586
587 ocean_assert(isValid());
588}
589
590template <typename T>
592{
593 ocean_assert(euler.isValid());
594
595 const T roll = euler.roll() * T(0.5);
596 const T pitch = euler.pitch() * T(0.5);
597 const T yaw = euler.yaw() * T(0.5);
598
599 const T cosRoll = NumericT<T>::cos(roll);
600 const T sinRoll = NumericT<T>::sin(roll);
601
602 const T cosPitch = NumericT<T>::cos(pitch);
603 const T sinPitch = NumericT<T>::sin(pitch);
604
605 const T cosYaw = NumericT<T>::cos(yaw);
606 const T sinYaw = NumericT<T>::sin(yaw);
607
608 const T cc = cosRoll * cosYaw;
609 const T cs = cosRoll * sinYaw;
610
611 const T sc = sinRoll * cosYaw;
612 const T ss = sinRoll * sinYaw;
613
614 values_[0] = cosPitch * cc + sinPitch * ss;
615 values_[1] = cosPitch * ss + sinPitch * cc;
616 values_[2] = cosPitch * cs - sinPitch * sc;
617 values_[3] = cosPitch * sc - sinPitch * cs;
618
619 normalize();
620 ocean_assert(isValid());
621}
622
623template <typename T>
625{
626 ocean_assert(!matrix.isSingular());
627
628 ocean_assert_accuracy(NumericT<T>::isWeakEqual(matrix.determinant(), T(1.0)));
629 const T trace = matrix.trace() + T(1.0);
630
631 if (trace > T(2.0) * NumericT<T>::eps() * NumericT<T>::eps())
632 {
633 values_[0] = T(0.5) * NumericT<T>::sqrt(trace);
634 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[0]));
635
636 const T factor = T(0.25) / values_[0];
637
638 values_[1] = (matrix(2, 1) - matrix(1, 2)) * factor;
639 values_[2] = (matrix(0, 2) - matrix(2, 0)) * factor;
640 values_[3] = (matrix(1, 0) - matrix(0, 1)) * factor;
641 }
642 else
643 {
644 if (matrix(0, 0) > matrix(1, 1) && matrix(0, 0) > matrix(2, 2))
645 {
646 values_[1] = T(0.5) * NumericT<T>::sqrt(matrix(0, 0) - matrix(1, 1) - matrix(2, 2) + T(1.0));
647 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[1]));
648 const T factor = T(0.25) / values_[1];
649
650 values_[0] = (matrix(2, 1) - matrix(1, 2)) * factor;
651 values_[2] = (matrix(0, 1) + matrix(1, 0)) * factor;
652 values_[3] = (matrix(0, 2) + matrix(2, 0)) * factor;
653 }
654 else if (matrix(1, 1) > matrix(2, 2))
655 {
656 values_[2] = T(0.5) * NumericT<T>::sqrt(matrix(1, 1) - matrix(0, 0) - matrix(2, 2) + T(1.0));
657 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[2]));
658 const T factor = T(0.25) / values_[2];
659
660 values_[0] = (matrix(0, 2) - matrix(2, 0)) * factor;
661 values_[1] = (matrix(0, 1) + matrix(1, 0)) * factor;
662 values_[3] = (matrix(1, 2) + matrix(2, 1)) * factor;
663 }
664 else
665 {
666 values_[3] = T(0.5) * NumericT<T>::sqrt(matrix(2, 2) - matrix(0, 0) - matrix(1, 1) + T(1.0));
667 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[3]));
668 const T factor = T(0.25) / values_[3];
669
670 values_[0] = (matrix(1, 0) - matrix(0, 1)) * factor;
671 values_[1] = (matrix(0, 2) + matrix(2, 0)) * factor;
672 values_[2] = (matrix(1, 2) + matrix(2, 1)) * factor;
673 }
674 }
675
676 normalize();
677 ocean_assert(isValid());
678}
679
680template <typename T>
682{
683 *this = QuaternionT<T>(transformation.orthonormalRotationMatrix());
684 ocean_assert(isValid());
685}
686
687template <typename T>
689{
690 memcpy(values_, vector(), sizeof(T) * 4);
691}
692
693template <typename T>
694QuaternionT<T>::QuaternionT(const T* arrayValue)
695{
696 ocean_assert(arrayValue);
697 memcpy(values_, arrayValue, sizeof(T) * 4);
698}
699
700template <typename T>
702{
703 const T normValue = norm();
704
705 if (NumericT<T>::isEqualEps(normValue))
706 {
707 return QuaternionT<T>(true);
708 }
709
710 const T factor = T(1.0) / normValue;
711
712 return QuaternionT<T>(values_[0] * factor, values_[1] * factor, values_[2] * factor, values_[3] * factor);
713}
714
715template <typename T>
717{
718 const T normValue = norm();
719
720 if (NumericT<T>::isEqualEps(normValue))
721 {
722 return false;
723 }
724
725 const T factor = T(1.0) / normValue;
726
727 values_[0] *= factor;
728 values_[1] *= factor;
729 values_[2] *= factor;
730 values_[3] *= factor;
731
732 return true;
733}
734
735template <typename T>
736bool QuaternionT<T>::normalize(QuaternionT<T>& normalizedQuaternion) const
737{
738 const T normValue = norm();
739
740 if (NumericT<T>::isEqualEps(normValue))
741 {
742 return false;
743 }
744
745 const T factor = T(1.0) / normValue;
746
747 normalizedQuaternion.values_[0] = values_[0] * factor;
748 normalizedQuaternion.values_[1] = values_[1] * factor;
749 normalizedQuaternion.values_[2] = values_[2] * factor;
750 normalizedQuaternion.values_[3] = values_[3] * factor;
751
752 return true;
753}
754
755template <typename T>
757{
758 const T square = sqr();
759
760 if (NumericT<T>::isEqualEps(square))
761 {
762 return QuaternionT<T>(true);
763 }
764
765 const T factor = T(1) / square;
766
767 return QuaternionT<T>(values_[0] * factor, -values_[1] * factor, -values_[2] * factor, -values_[3] * factor);
768}
769
770template <typename T>
772{
773 const T square = sqr();
774
775 if (NumericT<T>::isEqualEps(square))
776 {
777 return false;
778 }
779
780 const T factor = T(1.0) / square;
781
782 values_[0] *= factor;
783 values_[1] *= -factor;
784 values_[2] *= -factor;
785 values_[3] *= -factor;
786
787 return true;
788}
789
790template <typename T>
791bool QuaternionT<T>::invert(QuaternionT<T>& invertedQuaternion) const
792{
793 const T square = sqr();
794
795 if (NumericT<T>::isEqualEps(square))
796 {
797 return false;
798 }
799
800 const T factor = T(1.0) / square;
801
802 invertedQuaternion.values_[0] = values_[0] * factor;
803 invertedQuaternion.values_[1] = values_[1] * -factor;
804 invertedQuaternion.values_[2] = values_[2] * -factor;
805 invertedQuaternion.values_[3] = values_[3] * -factor;
806
807 return true;
808}
809
810template <typename T>
812{
813 return QuaternionT<T>(values_[0], -values_[1], -values_[2], -values_[3]);
814}
815
816template <typename T>
818{
819 return NumericT<T>::sqrt(values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3]);
820}
821
822template <typename T>
824{
825 return values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3];
826}
827
828template <typename T>
829T QuaternionT<T>::dot(const QuaternionT<T>& quaternion) const
830{
831 return values_[0] * quaternion.values_[0] + values_[1] * quaternion.values_[1] + values_[2] * quaternion.values_[2] + values_[3] * quaternion.values_[3];
832}
833
834template <typename T>
836{
837 ocean_assert(isValid());
838
840}
841
842template <typename T>
843T QuaternionT<T>::angle(const QuaternionT<T>& quaternion) const
844{
845 ocean_assert(isValid() && quaternion.isValid());
846
847 return (inverted() * quaternion).angle();
848}
849
850template <typename T>
852{
853 ocean_assert(isValid() && quaternion.isValid());
854
855 return NumericT<T>::abs(NumericT<T>::angleAdjustNull(angle(quaternion)));
856}
857
858template <typename T>
859T QuaternionT<T>::cos2(const QuaternionT<T>& quaternion) const
860{
861 ocean_assert(isValid() && quaternion.isValid());
862
863 return (inverted() * quaternion).w();
864}
865
866template <typename T>
867QuaternionT<T> QuaternionT<T>::slerp(const QuaternionT<T>& quaternion, T factor) const
868{
869 ocean_assert(factor >= 0 && factor <= T(1.0));
870 ocean_assert(isValid() && quaternion.isValid());
871
872 T sigma = minmax(T(-1), w() * quaternion.w() + x() * quaternion.x() + y() * quaternion.y() + z() * quaternion.z(), T(1));
873
874 QuaternionT<T> adjustedQuaternion(quaternion);
875
876 if (sigma < 0)
877 {
878 sigma = -sigma;
879 adjustedQuaternion = QuaternionT<T>(-quaternion.w(), -quaternion.x(), -quaternion.y(), -quaternion.z());
880 }
881
882 const T angle = NumericT<T>::acos(sigma);
883
884 T factorA = T(1.0) - factor;
885 T factorB = factor;
886
887 if (NumericT<T>::abs(angle) > T(0.05))
888 {
889 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(NumericT<T>::sin(angle)));
890
891 const T sinAngleValue = T(1.0) / NumericT<T>::sin(angle);
892
893 factorA = NumericT<T>::sin((T(1.0) - factor) * angle) * sinAngleValue;
894 factorB = NumericT<T>::sin(factor * angle) * sinAngleValue;
895 }
896
897 return QuaternionT<T>(factorA * w() + factorB * adjustedQuaternion.w(), factorA * x() + factorB * adjustedQuaternion.x(), factorA * y() + factorB * adjustedQuaternion.y(), factorA * z() + factorB * adjustedQuaternion.z()).normalized();
898}
899
900template <typename T>
902{
903 return NumericT<T>::isWeakEqual(norm(), T(1.0));
904}
905
906template <typename T>
907inline const T& QuaternionT<T>::w() const
908{
909 return values_[0];
910}
911
912template <typename T>
914{
915 return values_[0];
916}
917
918template <typename T>
919inline const T& QuaternionT<T>::x() const
920{
921 return values_[1];
922}
923
924template <typename T>
926{
927 return values_[1];
928}
929
930template <typename T>
931inline const T& QuaternionT<T>::y() const
932{
933 return values_[2];
934}
935
936template <typename T>
938{
939 return values_[2];
940}
941
942template <typename T>
943inline const T& QuaternionT<T>::z() const
944{
945 return values_[3];
946}
947
948template <typename T>
950{
951 return values_[3];
952}
953
954template <typename T>
955inline bool QuaternionT<T>::isEqual(const QuaternionT<T>& quaternion, const T eps) const
956{
957 return (NumericT<T>::isEqual(values_[0], quaternion.values_[0], eps) && NumericT<T>::isEqual(values_[1], quaternion.values_[1], eps) && NumericT<T>::isEqual(values_[2], quaternion.values_[2], eps) && NumericT<T>::isEqual(values_[3], quaternion.values_[3], eps))
958 || (NumericT<T>::isEqual(values_[0], -quaternion.values_[0], eps) && NumericT<T>::isEqual(values_[1], -quaternion.values_[1], eps) && NumericT<T>::isEqual(values_[2], -quaternion.values_[2], eps) && NumericT<T>::isEqual(values_[3], -quaternion.values_[3], eps));
959}
960
961template <typename T>
963{
964 return isEqual(right, NumericT<T>::eps());
965}
966
967template <typename T>
969{
970 return QuaternionT(values_[0] * right.values_[0] - values_[1] * right.values_[1] - values_[2] * right.values_[2] - values_[3] * right.values_[3],
971 values_[0] * right.values_[1] + values_[1] * right.values_[0] + values_[2] * right.values_[3] - values_[3] * right.values_[2],
972 values_[0] * right.values_[2] - values_[1] * right.values_[3] + values_[2] * right.values_[0] + values_[3] * right.values_[1],
973 values_[0] * right.values_[3] + values_[1] * right.values_[2] - values_[2] * right.values_[1] + values_[3] * right.values_[0]);
974}
975
976template <typename T>
978{
979 ocean_assert(isValid());
980
981 const QuaternionT<T> quaternion(0, vector[0], vector[1], vector[2]);
982 const QuaternionT<T> result(*this * quaternion * inverted());
983
984 return VectorT3<T>(result.values_[1], result.values_[2], result.values_[3]);
985}
986
987template <typename T>
988inline bool QuaternionT<T>::operator!=(const QuaternionT<T>& right) const
989{
990 return !(*this == right);
991}
992
993template <typename T>
995{
996 return *this * QuaternionT(right);
997}
998
999template <typename T>
1001{
1002 *this = *this * right;
1003 return *this;
1004}
1005
1006template <typename T>
1008{
1009 *this = *this * QuaternionT(right);
1010 return *this;
1011}
1012
1013template <typename T>
1014inline T QuaternionT<T>::operator[](const unsigned int index) const
1015{
1016 ocean_assert(index < 4u);
1017 return values_[index];
1018}
1019
1020template <typename T>
1021inline T& QuaternionT<T>::operator[](const unsigned int index)
1022{
1023 ocean_assert(index < 4u);
1024 return values_[index];
1025}
1026
1027template <typename T>
1028inline T QuaternionT<T>::operator()(const unsigned int index) const
1029{
1030 ocean_assert(index < 4u);
1031 return values_[index];
1032}
1033
1034template <typename T>
1035inline T& QuaternionT<T>::operator()(const unsigned int index)
1036{
1037 ocean_assert(index < 4u);
1038 return values_[index];
1039}
1040
1041template <typename T>
1042inline const T* QuaternionT<T>::operator()() const
1043{
1044 return values_;
1045}
1046
1047template <typename T>
1049{
1050 return values_;
1051}
1052
1053template <typename T>
1054std::ostream& operator<<(std::ostream& stream, const QuaternionT<T>& quaternion)
1055{
1056 stream << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1057
1058 return stream;
1059}
1060
1061template <bool tActive, typename T>
1062MessageObject<tActive>& operator<<(MessageObject<tActive>& messageObject, const QuaternionT<T>& quaternion)
1063{
1064 return messageObject << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1065}
1066
1067template <bool tActive, typename T>
1068MessageObject<tActive>& operator<<(MessageObject<tActive>&& messageObject, const QuaternionT<T>& quaternion)
1069{
1070 return messageObject << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1071}
1072
1073}
1074
1075#endif // META_OCEAN_MATH_QUATERNION_H
This class implements an euler rotation with angles: yaw, pitch and roll.
Definition Euler.h:80
const T & yaw() const
Returns the yaw angle.
Definition Euler.h:315
const T & roll() const
Returns the roll angle.
Definition Euler.h:339
bool isValid() const
Returns whether the euler rotation holds valid parameters.
Definition Euler.h:351
const T & pitch() const
Returns the pitch angle.
Definition Euler.h:327
This class implements a 4x4 homogeneous transformation matrix using floating point values with the pr...
Definition HomogenousMatrix4.h:110
SquareMatrixT3< T > orthonormalRotationMatrix() const
Returns the 3x3 orthonormal rotation matrix of the 4x4 transformation (by forcing a orthogonal and no...
Definition HomogenousMatrix4.h:1538
This class provides basic numeric functionalities.
Definition Numeric.h:57
static T angleAdjustPositive(const T angle)
Adjusts an arbitrary angle into the range of [0.0, 2PI).
Definition Numeric.h:1764
static T sin(const T value)
Returns the sine of a given value.
Definition Numeric.h:1568
static T abs(const T value)
Returns the absolute value of a given value.
Definition Numeric.h:1220
static bool isWeakEqual(const T first, const T second)
Returns whether two values a equal up to a weak epsilon.
Definition Numeric.h:2575
static T sqrt(const T value)
Returns the square root of a given value.
Definition Numeric.h:1533
static bool isEqual(const T first, const T second)
Returns whether two values are equal up to a small epsilon.
Definition Numeric.h:2389
static T cos(const T value)
Returns the cosine of a given value.
Definition Numeric.h:1584
static T acos(const T value)
Returns the arccosine of a given value.
Definition Numeric.h:2910
This class implements a unit quaternion rotation.
Definition Quaternion.h:100
T & w()
Returns the w value of the quaternion.
Definition Quaternion.h:913
T cos2(const QuaternionT< T > &quaternion) const
Returns the cosine value of the half angle between two quaternion rotations.
Definition Quaternion.h:859
T sqr() const
Returns the square of the quaternion norm.
Definition Quaternion.h:823
T & operator()(const unsigned int index)
Element operator.
Definition Quaternion.h:1035
QuaternionT(const T *arrayValue)
Creates a new quaternion by an array with at least four elements.
Definition Quaternion.h:694
T * operator()()
Access operator.
Definition Quaternion.h:1048
bool normalize()
Normalizes the quaternion in place.
Definition Quaternion.h:716
QuaternionT(const VectorT3< T > &reference, const VectorT3< T > &offset)
Creates a quaternion object based on two given unit vectors.
Definition Quaternion.h:537
QuaternionT(const EulerT< T > &euler)
Creates a new quaternion by a given Euler rotation.
Definition Quaternion.h:591
QuaternionT(const QuaternionT< U > &quaternion)
Copies a quaternion with different element data type than T.
Definition Quaternion.h:503
bool operator==(const QuaternionT< T > &right) const
Returns whether two quaternions are identical up to a small epsilon.
Definition Quaternion.h:962
T & operator[](const unsigned int index)
Element operator.
Definition Quaternion.h:1021
const T & x() const
Returns the x value of the quaternion.
Definition Quaternion.h:919
T norm() const
Returns the norm of this quaternion.
Definition Quaternion.h:817
T dot(const QuaternionT< T > &quaternion) const
Returns the dot product between this quaternion and a second quaternion.
Definition Quaternion.h:829
T angle(const QuaternionT< T > &quaternion) const
Returns the angle between two quaternion rotations.
Definition Quaternion.h:843
T & x()
Returns the x value of the quaternion.
Definition Quaternion.h:925
bool normalize(QuaternionT< T > &normalizedQuaternion) const
Normalizes the quaternion and returns the result as parameter.
Definition Quaternion.h:736
QuaternionT(const SquareMatrixT3< T > &matrix)
Creates a new quaternion by a given 3x3 rotation matrix.
Definition Quaternion.h:624
bool isValid() const
Returns whether this quaternion is a valid unit quaternion.
Definition Quaternion.h:901
const T & w() const
Returns the w value of the quaternion.
Definition Quaternion.h:907
QuaternionT< T > normalized() const
Returns the normalized quaternion.
Definition Quaternion.h:701
bool invert(QuaternionT< T > &invertedQuaternion) const
Inverts this quaternion and returns the result as parameter.
Definition Quaternion.h:791
QuaternionT()=default
Creates a new quaternion with default values, representation an identity rotation.
T smallestAngle(const QuaternionT< T > &quaternion) const
Returns the smallest angle between two quaternion rotations.
Definition Quaternion.h:851
QuaternionT(const bool toIdentity)
Creates a new quaternion with either default values (representation an identity rotation) or an inval...
Definition Quaternion.h:479
T operator()(const unsigned int index) const
Element operator.
Definition Quaternion.h:1028
T angle() const
Returns the rotation angle defined by the quaternion.
Definition Quaternion.h:835
QuaternionT< T > & operator*=(const RotationT< T > &right)
Combines and assigns a quaternion with a rotation.
Definition Quaternion.h:1007
bool isEqual(const QuaternionT< T > &quaternion, const T eps) const
Returns whether two quaternions are equal up to a specified epsilon.
Definition Quaternion.h:955
QuaternionT< T > slerp(const QuaternionT< T > &quaternion, T factor) const
Spherical linear interpolation between two quaternions.
Definition Quaternion.h:867
QuaternionT(const VectorT3< T > &axis, const T angle)
Creates a new quaternion by a given axis and rotation angle.
Definition Quaternion.h:521
T & z()
Returns the z value of the quaternion.
Definition Quaternion.h:949
const T * operator()() const
Access operator.
Definition Quaternion.h:1042
bool invert()
Inverts this quaternion in place.
Definition Quaternion.h:771
bool operator!=(const QuaternionT< T > &right) const
Returns whether two quaternions are not identical up to a small epsilon.
Definition Quaternion.h:988
QuaternionT(const RotationT< T > &rotation)
Creates a new quaternion by a given angle-axis rotation.
Definition Quaternion.h:575
T Type
Definition of the used data type.
Definition Quaternion.h:106
T values_[4]
The four values of the quaternion.
Definition Quaternion.h:475
QuaternionT(const VectorT4< T > &vector)
Creates a new quaternion by a 4D vector.
Definition Quaternion.h:688
QuaternionT< T > conjugate() const
Returns the conjugate of this quaternion.
Definition Quaternion.h:811
T & y()
Returns the y value of the quaternion.
Definition Quaternion.h:937
QuaternionT< T > inverted() const
Returns the inverted quaternion.
Definition Quaternion.h:756
const T & y() const
Returns the y value of the quaternion.
Definition Quaternion.h:931
const T & z() const
Returns the z value of the quaternion.
Definition Quaternion.h:943
QuaternionT(const T w, const T x, const T y, const T z)
Creates a new quaternion by four given values.
Definition Quaternion.h:512
QuaternionT< T > operator*(const RotationT< T > &right) const
Combines a angle-axis rotation with this quaternion.
Definition Quaternion.h:994
QuaternionT< T > & operator*=(const QuaternionT< T > &right)
Combines and assigns two quaternions.
Definition Quaternion.h:1000
QuaternionT< T > operator*(const QuaternionT< T > &right) const
Combines two quaternion to a new combined rotation.
Definition Quaternion.h:968
QuaternionT(const HomogenousMatrixT4< T > &transformation)
Creates a new quaternion by a given 4x4 homogeneous transformation matrix.
Definition Quaternion.h:681
T operator[](const unsigned int index) const
Element operator.
Definition Quaternion.h:1014
VectorT3< T > operator*(const VectorT3< T > &vector) const
Rotates a 3D vector by this quaternion.
Definition Quaternion.h:977
This class implements a axis-angle rotation using floating point values.
Definition Rotation.h:79
T angle() const
Returns the angle of the rotation.
Definition Rotation.h:754
bool isValid() const
Returns whether this rotation has valid parameters.
Definition Rotation.h:673
This class implements a 3x3 square matrix.
Definition SquareMatrix3.h:88
T determinant() const
Returns the determinant of the matrix.
Definition SquareMatrix3.h:1283
bool isSingular() const
Returns whether this matrix is singular (and thus cannot be inverted).
Definition SquareMatrix3.h:1341
T trace() const
Returns the trace of the matrix which is the sum of the diagonal elements.
Definition SquareMatrix3.h:1291
This class implements a vector with three elements.
Definition Vector3.h:97
bool isUnit(const T eps=NumericT< T >::eps()) const
Returns whether this vector is a unit vector (whether the vector has the length 1).
Definition Vector3.h:873
VectorT3< T > perpendicular() const
Returns a vector that is perpendicular to this vector.
Definition Vector3.h:768
const T & y() const noexcept
Returns the y value.
Definition Vector3.h:824
VectorT3< T > cross(const VectorT3< T > &vector) const
Returns the cross product of two vectors.
Definition Vector3.h:609
const T & x() const noexcept
Returns the x value.
Definition Vector3.h:812
const T & z() const noexcept
Returns the z value.
Definition Vector3.h:836
bool isEqual(const VectorT3< T > &vector, const T eps) const
Returns whether two vectors are equal up to a specified epsilon.
Definition Vector3.h:879
This class implements a vector with four elements.
Definition Vector4.h:97
T minmax(const T &lowerBoundary, const T &value, const T &upperBoundary)
This function fits a given parameter into a specified value range.
Definition base/Utilities.h:927
unsigned int sqr(const char value)
Returns the square value of a given value.
Definition base/Utilities.h:1053
QuaternionT< Scalar > Quaternion
Definition of the Quaternion object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION either with sin...
Definition Quaternion.h:40
std::vector< QuaternionF > QuaternionsF
Definition of a vector holding quaternion objects with single precision float data type.
Definition Quaternion.h:76
std::vector< QuaternionD > QuaternionsD
Definition of a vector holding quaternion objects with double precision float data type.
Definition Quaternion.h:83
QuaternionT< float > QuaternionF
Instantiation of the QuaternionT template class using a single precision float data type.
Definition Quaternion.h:54
std::vector< QuaternionT< T > > QuaternionsT
Definition of a typename alias for vectors with QuaternionT objects.
Definition Quaternion.h:62
QuaternionT< double > QuaternionD
Instantiation of the QuaternionT template class using a double precision float data type.
Definition Quaternion.h:47
std::vector< Quaternion > Quaternions
Definition of a vector holding quaternion objects.
Definition Quaternion.h:69
The namespace covering the entire Ocean framework.
Definition Accessor.h:15
std::ostream & operator<<(std::ostream &stream, const HighPerformanceStatistic &highPerformanceStatistic)
Definition HighPerformanceTimer.h:963