8#ifndef META_OCEAN_MATH_QUATERNION_H
9#define META_OCEAN_MATH_QUATERNION_H
21template <
typename T>
class RotationT;
24template <
typename T>
class EulerT;
27template <
typename T>
class SquareMatrixT3;
30template <
typename T>
class HomogenousMatrixT4;
33template <
typename T>
class QuaternionT;
126 template <
typename U>
327 inline const T&
w()
const;
339 inline const T&
x()
const;
351 inline const T&
y()
const;
363 inline const T&
z()
const;
488 ocean_assert(isValid() ==
true);
497 ocean_assert(isValid() ==
false);
505 values_[0] = T(quaternion[0]);
506 values_[1] = T(quaternion[1]);
507 values_[2] = T(quaternion[2]);
508 values_[3] = T(quaternion[3]);
525 const T angleValue = angle * T(0.5);
529 values_[1] = sinValue * axis[0];
530 values_[2] = sinValue * axis[1];
531 values_[3] = sinValue * axis[2];
533 ocean_assert_accuracy(isValid());
542 if (reference == offset)
549 else if (reference == -offset)
554 values_[1] = perpendicular[0];
555 values_[2] = perpendicular[1];
556 values_[3] = perpendicular[2];
562 values_[0] = T(1) + reference * offset;
563 values_[1] = axis.
x();
564 values_[2] = axis.
y();
565 values_[3] = axis.
z();
570 ocean_assert(isValid());
577 ocean_assert(rotation.
isValid());
579 const T angle = rotation.
angle() * T(0.5);
583 values_[1] = sinValue * rotation[0];
584 values_[2] = sinValue * rotation[1];
585 values_[3] = sinValue * rotation[2];
587 ocean_assert(isValid());
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);
608 const T cc = cosRoll * cosYaw;
609 const T cs = cosRoll * sinYaw;
611 const T sc = sinRoll * cosYaw;
612 const T ss = sinRoll * sinYaw;
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;
620 ocean_assert(isValid());
629 const T trace = matrix.
trace() + T(1.0);
636 const T factor = T(0.25) / values_[0];
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;
644 if (matrix(0, 0) > matrix(1, 1) && matrix(0, 0) > matrix(2, 2))
646 values_[1] = T(0.5) *
NumericT<T>::sqrt(matrix(0, 0) - matrix(1, 1) - matrix(2, 2) + T(1.0));
648 const T factor = T(0.25) / values_[1];
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;
654 else if (matrix(1, 1) > matrix(2, 2))
656 values_[2] = T(0.5) *
NumericT<T>::sqrt(matrix(1, 1) - matrix(0, 0) - matrix(2, 2) + T(1.0));
658 const T factor = T(0.25) / values_[2];
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;
666 values_[3] = T(0.5) *
NumericT<T>::sqrt(matrix(2, 2) - matrix(0, 0) - matrix(1, 1) + T(1.0));
668 const T factor = T(0.25) / values_[3];
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;
677 ocean_assert(isValid());
684 ocean_assert(isValid());
690 memcpy(values_, vector(),
sizeof(T) * 4);
696 ocean_assert(arrayValue);
697 memcpy(values_, arrayValue,
sizeof(T) * 4);
703 const T normValue = norm();
710 const T factor = T(1.0) / normValue;
712 return QuaternionT<T>(values_[0] * factor, values_[1] * factor, values_[2] * factor, values_[3] * factor);
718 const T normValue = norm();
725 const T factor = T(1.0) / normValue;
727 values_[0] *= factor;
728 values_[1] *= factor;
729 values_[2] *= factor;
730 values_[3] *= factor;
738 const T normValue = norm();
745 const T factor = T(1.0) / normValue;
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;
758 const T square =
sqr();
765 const T factor = T(1) / square;
767 return QuaternionT<T>(values_[0] * factor, -values_[1] * factor, -values_[2] * factor, -values_[3] * factor);
773 const T square =
sqr();
780 const T factor = T(1.0) / square;
782 values_[0] *= factor;
783 values_[1] *= -factor;
784 values_[2] *= -factor;
785 values_[3] *= -factor;
793 const T square =
sqr();
800 const T factor = T(1.0) / square;
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;
813 return QuaternionT<T>(values_[0], -values_[1], -values_[2], -values_[3]);
819 return NumericT<T>::sqrt(values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3]);
825 return values_[0] * values_[0] + values_[1] * values_[1] + values_[2] * values_[2] + values_[3] * values_[3];
831 return values_[0] * quaternion.
values_[0] + values_[1] * quaternion.
values_[1] + values_[2] * quaternion.
values_[2] + values_[3] * quaternion.
values_[3];
837 ocean_assert(isValid());
845 ocean_assert(isValid() && quaternion.
isValid());
847 return (inverted() * quaternion).angle();
853 ocean_assert(isValid() && quaternion.
isValid());
861 ocean_assert(isValid() && quaternion.
isValid());
863 return (inverted() * quaternion).w();
869 ocean_assert(factor >= 0 && factor <= T(1.0));
870 ocean_assert(isValid() && quaternion.
isValid());
872 T sigma =
minmax(T(-1), w() * quaternion.
w() + x() * quaternion.
x() + y() * quaternion.
y() + z() * quaternion.
z(), T(1));
879 adjustedQuaternion =
QuaternionT<T>(-quaternion.
w(), -quaternion.
x(), -quaternion.
y(), -quaternion.
z());
884 T factorA = T(1.0) - factor;
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();
979 ocean_assert(isValid());
981 const QuaternionT<T> quaternion(0, vector[0], vector[1], vector[2]);
990 return !(*
this == right);
1002 *
this = *
this * right;
1006template <
typename T>
1013template <
typename T>
1016 ocean_assert(index < 4u);
1017 return values_[index];
1020template <
typename T>
1023 ocean_assert(index < 4u);
1024 return values_[index];
1027template <
typename T>
1030 ocean_assert(index < 4u);
1031 return values_[index];
1034template <
typename T>
1037 ocean_assert(index < 4u);
1038 return values_[index];
1041template <
typename T>
1047template <
typename T>
1053template <
typename T>
1056 stream <<
"[" << quaternion.
w() <<
", " << quaternion.
x() <<
", " << quaternion.
y() <<
", " << quaternion.
z() <<
"]";
1061template <
bool tActive,
typename T>
1062MessageObject<tActive>&
operator<<(MessageObject<tActive>& messageObject,
const QuaternionT<T>& quaternion)
1064 return messageObject <<
"[" << quaternion.w() <<
", " << quaternion.x() <<
", " << quaternion.y() <<
", " << quaternion.z() <<
"]";
1067template <
bool tActive,
typename T>
1068MessageObject<tActive>&
operator<<(MessageObject<tActive>&& messageObject,
const QuaternionT<T>& quaternion)
1070 return messageObject <<
"[" << quaternion.w() <<
", " << quaternion.x() <<
", " << quaternion.y() <<
", " << quaternion.z() <<
"]";
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