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_accuracy(NumericT<T>::isWeakEqual(matrix.determinant(), T(1.0)));
627 const T trace = matrix.trace() + T(1.0);
628
629 if (trace > T(2.0) * NumericT<T>::eps() * NumericT<T>::eps())
630 {
631 values_[0] = T(0.5) * NumericT<T>::sqrt(trace);
632 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[0]));
633
634 const T factor = T(0.25) / values_[0];
635
636 values_[1] = (matrix(2, 1) - matrix(1, 2)) * factor;
637 values_[2] = (matrix(0, 2) - matrix(2, 0)) * factor;
638 values_[3] = (matrix(1, 0) - matrix(0, 1)) * factor;
639 }
640 else
641 {
642 if (matrix(0, 0) > matrix(1, 1) && matrix(0, 0) > matrix(2, 2))
643 {
644 values_[1] = T(0.5) * NumericT<T>::sqrt(matrix(0, 0) - matrix(1, 1) - matrix(2, 2) + T(1.0));
645 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[1]));
646 const T factor = T(0.25) / values_[1];
647
648 values_[0] = (matrix(2, 1) - matrix(1, 2)) * factor;
649 values_[2] = (matrix(0, 1) + matrix(1, 0)) * factor;
650 values_[3] = (matrix(0, 2) + matrix(2, 0)) * factor;
651 }
652 else if (matrix(1, 1) > matrix(2, 2))
653 {
654 values_[2] = T(0.5) * NumericT<T>::sqrt(matrix(1, 1) - matrix(0, 0) - matrix(2, 2) + T(1.0));
655 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[2]));
656 const T factor = T(0.25) / values_[2];
657
658 values_[0] = (matrix(0, 2) - matrix(2, 0)) * factor;
659 values_[1] = (matrix(0, 1) + matrix(1, 0)) * factor;
660 values_[3] = (matrix(1, 2) + matrix(2, 1)) * factor;
661 }
662 else
663 {
664 values_[3] = T(0.5) * NumericT<T>::sqrt(matrix(2, 2) - matrix(0, 0) - matrix(1, 1) + T(1.0));
665 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(values_[3]));
666 const T factor = T(0.25) / values_[3];
667
668 values_[0] = (matrix(1, 0) - matrix(0, 1)) * factor;
669 values_[1] = (matrix(0, 2) + matrix(2, 0)) * factor;
670 values_[2] = (matrix(1, 2) + matrix(2, 1)) * factor;
671 }
672 }
673
674 normalize();
675 ocean_assert(isValid());
676}
677
678template <typename T>
680{
681 *this = QuaternionT<T>(transformation.orthonormalRotationMatrix());
682 ocean_assert(isValid());
683}
684
685template <typename T>
687{
688 memcpy(values_, vector(), sizeof(T) * 4);
689}
690
691template <typename T>
692QuaternionT<T>::QuaternionT(const T* arrayValue)
693{
694 ocean_assert(arrayValue);
695 memcpy(values_, arrayValue, sizeof(T) * 4);
696}
697
698template <typename T>
700{
701 const T normValue = norm();
702
703 if (NumericT<T>::isEqualEps(normValue))
704 {
705 return QuaternionT<T>(true);
706 }
707
708 const T factor = T(1.0) / normValue;
709
710 return QuaternionT<T>(values_[0] * factor, values_[1] * factor, values_[2] * factor, values_[3] * factor);
711}
712
713template <typename T>
715{
716 const T normValue = norm();
717
718 if (NumericT<T>::isEqualEps(normValue))
719 {
720 return false;
721 }
722
723 const T factor = T(1.0) / normValue;
724
725 values_[0] *= factor;
726 values_[1] *= factor;
727 values_[2] *= factor;
728 values_[3] *= factor;
729
730 return true;
731}
732
733template <typename T>
734bool QuaternionT<T>::normalize(QuaternionT<T>& normalizedQuaternion) const
735{
736 const T normValue = norm();
737
738 if (NumericT<T>::isEqualEps(normValue))
739 {
740 return false;
741 }
742
743 const T factor = T(1.0) / normValue;
744
745 normalizedQuaternion.values_[0] = values_[0] * factor;
746 normalizedQuaternion.values_[1] = values_[1] * factor;
747 normalizedQuaternion.values_[2] = values_[2] * factor;
748 normalizedQuaternion.values_[3] = values_[3] * factor;
749
750 return true;
751}
752
753template <typename T>
755{
756 const T square = sqr();
757
758 if (NumericT<T>::isEqualEps(square))
759 {
760 return QuaternionT<T>(true);
761 }
762
763 const T factor = T(1) / square;
764
765 return QuaternionT<T>(values_[0] * factor, -values_[1] * factor, -values_[2] * factor, -values_[3] * factor);
766}
767
768template <typename T>
770{
771 const T square = sqr();
772
773 if (NumericT<T>::isEqualEps(square))
774 {
775 return false;
776 }
777
778 const T factor = T(1.0) / square;
779
780 values_[0] *= factor;
781 values_[1] *= -factor;
782 values_[2] *= -factor;
783 values_[3] *= -factor;
784
785 return true;
786}
787
788template <typename T>
789bool QuaternionT<T>::invert(QuaternionT<T>& invertedQuaternion) const
790{
791 const T square = sqr();
792
793 if (NumericT<T>::isEqualEps(square))
794 {
795 return false;
796 }
797
798 const T factor = T(1.0) / square;
799
800 invertedQuaternion.values_[0] = values_[0] * factor;
801 invertedQuaternion.values_[1] = values_[1] * -factor;
802 invertedQuaternion.values_[2] = values_[2] * -factor;
803 invertedQuaternion.values_[3] = values_[3] * -factor;
804
805 return true;
806}
807
808template <typename T>
810{
811 return QuaternionT<T>(values_[0], -values_[1], -values_[2], -values_[3]);
812}
813
814template <typename T>
816{
817 return NumericT<T>::sqrt(values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3]);
818}
819
820template <typename T>
822{
823 return values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3];
824}
825
826template <typename T>
827T QuaternionT<T>::dot(const QuaternionT<T>& quaternion) const
828{
829 return values_[0] * quaternion.values_[0] + values_[1] * quaternion.values_[1] + values_[2] * quaternion.values_[2] + values_[3] * quaternion.values_[3];
830}
831
832template <typename T>
834{
835 ocean_assert(isValid());
836
838}
839
840template <typename T>
841T QuaternionT<T>::angle(const QuaternionT<T>& quaternion) const
842{
843 ocean_assert(isValid() && quaternion.isValid());
844
845 return (inverted() * quaternion).angle();
846}
847
848template <typename T>
850{
851 ocean_assert(isValid() && quaternion.isValid());
852
853 return NumericT<T>::abs(NumericT<T>::angleAdjustNull(angle(quaternion)));
854}
855
856template <typename T>
857T QuaternionT<T>::cos2(const QuaternionT<T>& quaternion) const
858{
859 ocean_assert(isValid() && quaternion.isValid());
860
861 return (inverted() * quaternion).w();
862}
863
864template <typename T>
865QuaternionT<T> QuaternionT<T>::slerp(const QuaternionT<T>& quaternion, T factor) const
866{
867 ocean_assert(factor >= 0 && factor <= T(1.0));
868 ocean_assert(isValid() && quaternion.isValid());
869
870 T sigma = minmax(T(-1), w() * quaternion.w() + x() * quaternion.x() + y() * quaternion.y() + z() * quaternion.z(), T(1));
871
872 QuaternionT<T> adjustedQuaternion(quaternion);
873
874 if (sigma < 0)
875 {
876 sigma = -sigma;
877 adjustedQuaternion = QuaternionT<T>(-quaternion.w(), -quaternion.x(), -quaternion.y(), -quaternion.z());
878 }
879
880 const T angle = NumericT<T>::acos(sigma);
881
882 T factorA = T(1.0) - factor;
883 T factorB = factor;
884
885 if (NumericT<T>::abs(angle) > T(0.05))
886 {
887 ocean_assert_accuracy(NumericT<T>::isNotEqualEps(NumericT<T>::sin(angle)));
888
889 const T sinAngleValue = T(1.0) / NumericT<T>::sin(angle);
890
891 factorA = NumericT<T>::sin((T(1.0) - factor) * angle) * sinAngleValue;
892 factorB = NumericT<T>::sin(factor * angle) * sinAngleValue;
893 }
894
895 return QuaternionT<T>(factorA * w() + factorB * adjustedQuaternion.w(), factorA * x() + factorB * adjustedQuaternion.x(), factorA * y() + factorB * adjustedQuaternion.y(), factorA * z() + factorB * adjustedQuaternion.z()).normalized();
896}
897
898template <typename T>
900{
901 return NumericT<T>::isWeakEqual(norm(), T(1.0));
902}
903
904template <typename T>
905inline const T& QuaternionT<T>::w() const
906{
907 return values_[0];
908}
909
910template <typename T>
912{
913 return values_[0];
914}
915
916template <typename T>
917inline const T& QuaternionT<T>::x() const
918{
919 return values_[1];
920}
921
922template <typename T>
924{
925 return values_[1];
926}
927
928template <typename T>
929inline const T& QuaternionT<T>::y() const
930{
931 return values_[2];
932}
933
934template <typename T>
936{
937 return values_[2];
938}
939
940template <typename T>
941inline const T& QuaternionT<T>::z() const
942{
943 return values_[3];
944}
945
946template <typename T>
948{
949 return values_[3];
950}
951
952template <typename T>
953inline bool QuaternionT<T>::isEqual(const QuaternionT<T>& quaternion, const T eps) const
954{
955 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))
956 || (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));
957}
958
959template <typename T>
961{
962 return isEqual(right, NumericT<T>::eps());
963}
964
965template <typename T>
967{
968 return QuaternionT(values_[0] * right.values_[0] - values_[1] * right.values_[1] - values_[2] * right.values_[2] - values_[3] * right.values_[3],
969 values_[0] * right.values_[1] + values_[1] * right.values_[0] + values_[2] * right.values_[3] - values_[3] * right.values_[2],
970 values_[0] * right.values_[2] - values_[1] * right.values_[3] + values_[2] * right.values_[0] + values_[3] * right.values_[1],
971 values_[0] * right.values_[3] + values_[1] * right.values_[2] - values_[2] * right.values_[1] + values_[3] * right.values_[0]);
972}
973
974template <typename T>
976{
977 ocean_assert(isValid());
978
979 const QuaternionT<T> quaternion(0, vector[0], vector[1], vector[2]);
980 const QuaternionT<T> result(*this * quaternion * inverted());
981
982 return VectorT3<T>(result.values_[1], result.values_[2], result.values_[3]);
983}
984
985template <typename T>
986inline bool QuaternionT<T>::operator!=(const QuaternionT<T>& right) const
987{
988 return !(*this == right);
989}
990
991template <typename T>
993{
994 return *this * QuaternionT(right);
995}
996
997template <typename T>
999{
1000 *this = *this * right;
1001 return *this;
1002}
1003
1004template <typename T>
1006{
1007 *this = *this * QuaternionT(right);
1008 return *this;
1009}
1010
1011template <typename T>
1012inline T QuaternionT<T>::operator[](const unsigned int index) const
1013{
1014 ocean_assert(index < 4u);
1015 return values_[index];
1016}
1017
1018template <typename T>
1019inline T& QuaternionT<T>::operator[](const unsigned int index)
1020{
1021 ocean_assert(index < 4u);
1022 return values_[index];
1023}
1024
1025template <typename T>
1026inline T QuaternionT<T>::operator()(const unsigned int index) const
1027{
1028 ocean_assert(index < 4u);
1029 return values_[index];
1030}
1031
1032template <typename T>
1033inline T& QuaternionT<T>::operator()(const unsigned int index)
1034{
1035 ocean_assert(index < 4u);
1036 return values_[index];
1037}
1038
1039template <typename T>
1040inline const T* QuaternionT<T>::operator()() const
1041{
1042 return values_;
1043}
1044
1045template <typename T>
1047{
1048 return values_;
1049}
1050
1051template <typename T>
1052std::ostream& operator<<(std::ostream& stream, const QuaternionT<T>& quaternion)
1053{
1054 stream << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1055
1056 return stream;
1057}
1058
1059template <bool tActive, typename T>
1060MessageObject<tActive>& operator<<(MessageObject<tActive>& messageObject, const QuaternionT<T>& quaternion)
1061{
1062 return messageObject << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1063}
1064
1065template <bool tActive, typename T>
1066MessageObject<tActive>& operator<<(MessageObject<tActive>&& messageObject, const QuaternionT<T>& quaternion)
1067{
1068 return messageObject << "[" << quaternion.w() << ", " << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << "]";
1069}
1070
1071}
1072
1073#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:2572
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:2386
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:2907
This class implements a unit quaternion rotation.
Definition Quaternion.h:100
T & w()
Returns the w value of the quaternion.
Definition Quaternion.h:911
T cos2(const QuaternionT< T > &quaternion) const
Returns the cosine value of the half angle between two quaternion rotations.
Definition Quaternion.h:857
T sqr() const
Returns the square of the quaternion norm.
Definition Quaternion.h:821
T & operator()(const unsigned int index)
Element operator.
Definition Quaternion.h:1033
QuaternionT(const T *arrayValue)
Creates a new quaternion by an array with at least four elements.
Definition Quaternion.h:692
T * operator()()
Access operator.
Definition Quaternion.h:1046
bool normalize()
Normalizes the quaternion in place.
Definition Quaternion.h:714
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:960
T & operator[](const unsigned int index)
Element operator.
Definition Quaternion.h:1019
const T & x() const
Returns the x value of the quaternion.
Definition Quaternion.h:917
T norm() const
Returns the norm of this quaternion.
Definition Quaternion.h:815
T dot(const QuaternionT< T > &quaternion) const
Returns the dot product between this quaternion and a second quaternion.
Definition Quaternion.h:827
T angle(const QuaternionT< T > &quaternion) const
Returns the angle between two quaternion rotations.
Definition Quaternion.h:841
T & x()
Returns the x value of the quaternion.
Definition Quaternion.h:923
bool normalize(QuaternionT< T > &normalizedQuaternion) const
Normalizes the quaternion and returns the result as parameter.
Definition Quaternion.h:734
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:899
const T & w() const
Returns the w value of the quaternion.
Definition Quaternion.h:905
QuaternionT< T > normalized() const
Returns the normalized quaternion.
Definition Quaternion.h:699
bool invert(QuaternionT< T > &invertedQuaternion) const
Inverts this quaternion and returns the result as parameter.
Definition Quaternion.h:789
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:849
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:1026
T angle() const
Returns the rotation angle defined by the quaternion.
Definition Quaternion.h:833
QuaternionT< T > & operator*=(const RotationT< T > &right)
Combines and assigns a quaternion with a rotation.
Definition Quaternion.h:1005
bool isEqual(const QuaternionT< T > &quaternion, const T eps) const
Returns whether two quaternions are equal up to a specified epsilon.
Definition Quaternion.h:953
QuaternionT< T > slerp(const QuaternionT< T > &quaternion, T factor) const
Spherical linear interpolation between two quaternions.
Definition Quaternion.h:865
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:947
const T * operator()() const
Access operator.
Definition Quaternion.h:1040
bool invert()
Inverts this quaternion in place.
Definition Quaternion.h:769
bool operator!=(const QuaternionT< T > &right) const
Returns whether two quaternions are not identical up to a small epsilon.
Definition Quaternion.h:986
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:686
QuaternionT< T > conjugate() const
Returns the conjugate of this quaternion.
Definition Quaternion.h:809
T & y()
Returns the y value of the quaternion.
Definition Quaternion.h:935
QuaternionT< T > inverted() const
Returns the inverted quaternion.
Definition Quaternion.h:754
const T & y() const
Returns the y value of the quaternion.
Definition Quaternion.h:929
const T & z() const
Returns the z value of the quaternion.
Definition Quaternion.h:941
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:992
QuaternionT< T > & operator*=(const QuaternionT< T > &right)
Combines and assigns two quaternions.
Definition Quaternion.h:998
QuaternionT< T > operator*(const QuaternionT< T > &right) const
Combines two quaternion to a new combined rotation.
Definition Quaternion.h:966
QuaternionT(const HomogenousMatrixT4< T > &transformation)
Creates a new quaternion by a given 4x4 homogeneous transformation matrix.
Definition Quaternion.h:679
T operator[](const unsigned int index) const
Element operator.
Definition Quaternion.h:1012
VectorT3< T > operator*(const VectorT3< T > &vector) const
Rotates a 3D vector by this quaternion.
Definition Quaternion.h:975
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:746
bool isValid() const
Returns whether this rotation has valid parameters.
Definition Rotation.h:665
This class implements a 3x3 square matrix.
Definition SquareMatrix3.h:88
T determinant() const
Returns the determinant of the matrix.
Definition SquareMatrix3.h:1283
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:903
unsigned int sqr(const char value)
Returns the square value of a given value.
Definition base/Utilities.h:1029
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