13 #ifndef ML_QUATERNION_H
14 #define ML_QUATERNION_H
38 template <
typename DT>
91 qx(0), qy(0), qz(0), qw(0) { }
96 qx(0), qy(0), qz(0), qw(d)
104 template <
typename DT2>
107 qx =
static_cast<DT>(q2.
qx);
108 qy =
static_cast<DT>(q2.
qy);
109 qz =
static_cast<DT>(q2.
qz);
110 qw =
static_cast<DT>(q2.
qw);
152 qx(v[0]), qy(v[1]), qz(v[2]), qw(w)
232 printTemplateError(
"TQuaternion::getAsMat4() const, quaternion is not convertible to 4x4 matrix",
234 "Returning Quaterion");
237 *isConvertible =
false;
256 const DT qxqx = qx*qx;
257 const DT qyqy = qy*qy;
258 const DT qzqz = qz*qz;
260 const DT qxqy = qx*qy;
261 const DT qxqz = qx*qz;
262 const DT qxqw = qx*qw;
264 const DT qyqz = qy*qz;
265 const DT qyqw = qy*qw;
267 const DT qzqw = qz*qw;
269 const DT m11 = 1 - s * (qyqy + qzqz);
270 const DT m12 = s * (qxqy - qzqw);
271 const DT m13 = s * (qxqz + qyqw);
273 const DT m21 = s * (qxqy + qzqw);
274 const DT m22 = 1 - s * (qxqx + qzqz);
275 const DT m23 = s * (qyqz - qxqw);
277 const DT m31 = s * (qxqz - qyqw);
278 const DT m32 = s * (qyqz + qxqw);
279 const DT m33 = 1 - s * (qxqx + qyqy);
303 if (isConvertible){ *isConvertible =
true; }
374 qx/=d; qy/=d; qz/=d; qw/=d;
403 *isError = (b.
qx == 0) ||
410 b.
qy != 0 ? qy/b.
qy : qy,
411 b.
qz != 0 ? qz/b.
qz : qz,
412 b.
qw != 0 ? qw/b.
qw : qw) :
449 qw*p.
qw - qx*p.
qx - qy*p.
qy - qz*p.
qz); }
474 if (isError ==
nullptr){
478 "Returning unchanged quaternion");
479 retVal.
set(qx,qy,qz,qw);
484 retVal.
set(qx,qy,qz,qw);
488 if (isError !=
nullptr){ *isError =
false; }
514 inline DT norm()
const {
return qx*qx + qy*qy + qz*qz + qw*qw; }
519 inline DT norm2()
const {
return static_cast<DT>(::sqrt(qx*qx + qy*qy + qz*qz + qw*qw)); }
536 if (isError ==
nullptr){
538 printTemplateError(
"TQuaternion::normalize() const, quaternion can not be normalized",
540 "Returning default quaternion");
549 if (isError){ *isError =
false; }
550 retVal.
set(qx/n, qy/n, qz/n, qw/n);
569 if (isError ==
nullptr){
573 "Returning default quaternion");
582 if (isError !=
nullptr){ *isError =
false; }
596 const DT absVal = absoluteValue();
597 if (isError !=
nullptr){
605 retVal = *
this / absVal;
611 retVal = *
this / absVal;
627 if (isInvertible==
nullptr){
630 "Returning default quaternion");
633 *isInvertible =
false;
638 if (isInvertible !=
nullptr){ *isInvertible =
true; }
639 retVal = conjugate().
mult(1/n);
654 const DT n2 = norm2();
655 const DT lqv = qv().length();
658 if (isError ==
nullptr){
660 printTemplateError(
"TQuaternion::sqrt() const, sqrt of quaternion cannot be calculated",
662 "Returning default quaternion");
671 const DT sqrtSin = ::sqrt(n2)*::sin(0.5*acos(qw/n2));
672 const DT sqrtSinF = sqrtSin*(1/lqv);
673 retVal.
set(sqrtSinF*qx, sqrtSinF*qy, sqrtSinF*qz, ::sqrt(n2) * ::cos(0.5*acos(qw/n2)));
676 if (isError !=
nullptr){ *isError =
false; }
691 const DT len = qv().length();
707 const DT n2 = norm2();
708 const DT qvLen = qv().length();
710 retVal.
set(0, 0, 0, log10(n2));
714 retVal.
set(qv()/qvLen *
static_cast<DT>(acos(qw/n2)), log10(n2));
737 static_cast<DT>(::cos(qw)) * v / len *
static_cast<DT>(::sinh(len)) :
739 ::sin(qw)*::cosh(len)
753 static_cast<DT>(-1)*
static_cast<DT>(::sin(qw))*qv()/len*
static_cast<DT>(::sinh(len)) :
755 ::cos(qw)*::cosh(len)
781 static_cast<DT>(::cosh(qw))*v/len*
static_cast<DT>(::sin(len)) :
783 ::sinh(qw)*::cos(len)
797 static_cast<DT>(::sinh(qw))*v/len*
static_cast<DT>(::sin(len)) :
799 ::cosh(qw)*::cos(len)
807 return sinh().div(cosh(), isError);
813 return cosh().div(sinh(), isError);
841 #define _ML_QUATERNION_CALC_CHECKED(FUNC_NAME, CALC_EXPR) \
842 TQuaternion<DT> retVal; \
844 const Tvec3<DT> &v = qv(); \
845 const DT len = v.length(); \
846 if (MLValueIs0WOM(len)){ \
848 if (isError == NULL){ \
850 printTemplateError("TQuaternion::" FUNC_NAME "() const, " FUNC_NAME \
851 " of quaternion cannot be calculated", \
853 "Returning default quaternion"); \
862 if (isError != NULL){ *isError = false; } \
863 retVal = CALC_EXPR; \
871 #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(mult(TQuaternion<DT>(-v/len,0)).arcsinh());
881 #undef _ML_QUAT_CALC_EXP
886 #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(arccosh());
892 #undef _ML_QUAT_CALC_EXP
897 #define _ML_QUAT_CALC_EXP TQuaternion<DT>(-v/len,0).mult(mult(TQuaternion<DT>(-v/len,0)).arctanh());
903 #undef _ML_QUAT_CALC_EXP
907 #undef _ML_QUATERNION_CALC_CHECKED
942 #ifdef _ML_IMPLEMENT_GLOBAL_QUATERNION_OPERATORS
943 #ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
944 ML_LA_START_NAMESPACE
951 template <
typename DT>
952 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator+(
DT d,
const ML_LA_NAMESPACE::TQuaternion<DT> &q){
953 return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw+d);
957 template <
typename DT>
958 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator+(
const ML_LA_NAMESPACE::TQuaternion<DT> &q,
DT d){
959 return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw+d);
963 template <
typename DT>
964 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator+(
const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
965 const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
966 return qa.operator+(qb);
975 template <
typename DT>
976 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator-(
DT d,
const ML_LA_NAMESPACE::TQuaternion<DT> &q){
977 return ML_LA_NAMESPACE::TQuaternion<DT>(-q.qx, -q.qy, -q.qz, d-q.qw);
981 template <
typename DT>
982 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator-(
const ML_LA_NAMESPACE::TQuaternion<DT> &q,
DT d){
983 return ML_LA_NAMESPACE::TQuaternion<DT>(q.qx, q.qy, q.qz, q.qw-d);
987 template <
typename DT>
988 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator-(
const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
989 const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
990 return qa.operator-(qb);
995 template <
typename DT>
996 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator*(
DT d,
const ML_LA_NAMESPACE::TQuaternion<DT> &q){
1001 template <
typename DT>
1002 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator*(
const ML_LA_NAMESPACE::TQuaternion<DT> &q,
DT d){
1007 template <
typename DT>
1008 inline ML_LA_NAMESPACE::TQuaternion<DT>
operator*(
const ML_LA_NAMESPACE::TQuaternion<DT> &qa,
1009 const ML_LA_NAMESPACE::TQuaternion<DT> &qb){
1010 return qa.operator*(qb);
1014 #ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
1030 #ifdef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCTIONS
1031 #ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
1032 ML_LA_START_NAMESPACE
1037 #ifdef _ML_IMPLEMENT_QUATERNION_SQRT
1038 template <
typename DT>
1039 inline ML_LA_NAMESPACE::TQuaternion<DT> sqrt (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.sqrt(); }
1043 template <
typename DT>
1044 inline ML_LA_NAMESPACE::TQuaternion<DT> exp (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.exp(); }
1047 template <
typename DT>
1048 inline ML_LA_NAMESPACE::TQuaternion<DT> ln (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.ln(); }
1051 template <
typename DT>
1052 inline ML_LA_NAMESPACE::TQuaternion<DT> sin (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.sin(); }
1055 template <
typename DT>
1056 inline ML_LA_NAMESPACE::TQuaternion<DT> cos (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.cos(); }
1059 template <
typename DT>
1060 inline ML_LA_NAMESPACE::TQuaternion<DT> tan (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.tan(); }
1063 template <
typename DT>
1064 inline ML_LA_NAMESPACE::TQuaternion<DT> cotan (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.cotan(); }
1067 template <
typename DT>
1068 inline ML_LA_NAMESPACE::TQuaternion<DT> sinh (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.sinh(); }
1071 template <
typename DT>
1072 inline ML_LA_NAMESPACE::TQuaternion<DT> cosh (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.cosh(); }
1075 template <
typename DT>
1076 inline ML_LA_NAMESPACE::TQuaternion<DT> tanh (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.tanh(); }
1079 template <
typename DT>
1080 inline ML_LA_NAMESPACE::TQuaternion<DT> cotanh (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.cotanh(); }
1083 template <
typename DT>
1084 inline ML_LA_NAMESPACE::TQuaternion<DT> arcsinh(
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arcsinh(); }
1087 template <
typename DT>
1088 inline ML_LA_NAMESPACE::TQuaternion<DT> arccosh(
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arccosh(); }
1091 template <
typename DT>
1092 inline ML_LA_NAMESPACE::TQuaternion<DT> arctanh(
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arctanh(); }
1095 template <
typename DT>
1096 inline ML_LA_NAMESPACE::TQuaternion<DT> arcsin (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arcsin(); }
1099 template <
typename DT>
1100 inline ML_LA_NAMESPACE::TQuaternion<DT> arccos (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arccos(); }
1103 template <
typename DT>
1104 inline ML_LA_NAMESPACE::TQuaternion<DT> arctan (
const ML_LA_NAMESPACE::TQuaternion<DT> &q) {
return q.arctan(); }
1107 #ifndef _ML_IMPLEMENT_GLOBAL_QUATERNION_FUNCS_AND_OPS_IN_GLOBAL_NAMESPACE
1121 template <
typename DT>
1122 inline ostream&
operator<<(ostream& s,
const ML_NAMESPACE::TQuaternion<DT> &v){
1123 return s <<
"(" << v.qx <<
"," << v.qy <<
"," << v.qz <<
"," << v.qw <<
")";
FloatingPointVector< T, 3, DataContainer > cross(const FloatingPointVector< T, 3, DataContainer > &b) const
Returns the cross product for elements, i.e., the returned vector is orthogonal to *this and b.
T length() const
Returns the length of the vector, i.e., norm2().
T operator*(const FloatingPointVector< T, size, DataContainer > &a, const FloatingPointVector< T, size, DataContainer > &b)
Dot product, returns a.dot(b).
T dot(const FloatingPointVector< T, size, DataContainer > &buffer) const
Returns the dot product, i.e., sum of all components multiplied with corresponding components of buff...
FloatingPointVector< T, size, DataContainer > operator+(FloatingPointVector< T, size, DataContainer > lhs, const FloatingPointVector< T, size, DataContainer > &rhs)
Return value is the component-wise addition of lhs and rhs.
FloatingPointVector< T, size, DataContainer > operator-(FloatingPointVector< T, size, DataContainer > lhs, const FloatingPointVector< T, size, DataContainer > &rhs)
Return value is the component-wise subtraction of rhs from lhs.
Declaration of complex type traits.
TQuaternion(DT d)
Builds a quaternion from a scalar by setting all elements to 0 and qw to the scalar; use explicitly t...
DT dot(const TQuaternion< DT > &p) const
Dot product, equivalent to four component vector dot product.
TQuaternion< DT > add(const TQuaternion< DT > &b) const
Adds another quaternion b to this one and returns the sum.
DT & operator[](const size_t i)
Indexing operator.
TQuaternion(DT x, DT y, DT z, DT w)
Creates a new TQuaternion with the DT values given.
DT qx
First element of imaginary part.
TQuaternion< DT > exp() const
Exponential and logarithmic functions: natural exponential, natural logarithm, power.
TQuaternion< DT > normalize(bool *isError=nullptr) const
Returns the normalization of the quaternion.
DT absoluteValue() const
Returns the absolute value of a quaternion, which is the scalar quantity that determines the length o...
TQuaternion< DT > cotanh(bool *isError=nullptr) const
Computes the hyperbolic cotangent; for error handling see _ML_QUATERNION_CALC_CHECKED.
TQuaternion< DT > inverse(bool *isInvertible=nullptr) const
Computes and returns the inverse.
TQuaternion< DT > arctanh() const
Computes the inverse hyperbolic tangent.
TQuaternion< DT > div(const TQuaternion< DT > &d, bool *isError=nullptr) const
Divides a quaternion by another quaternion.
TQuaternion< DT > compDiv(const TQuaternion< DT > &b, bool *isError=nullptr) const
Divides each component by its corresponding component from b.
TQuaternion< DT > sqrt(bool *isError=nullptr) const
Computes the square root of a quaternion.
void set(const FloatingPointVector< DT, 3, Vector3DataContainer< DT > > &v, const DT w)
Sets all quaternion elements from a vector and DT parameter.
TQuaternion< DT > sinh() const
Hyperbolic functions: Hyperbolic sine, hyperbolic cosine, hyperbolic tangent https://en....
TQuaternion< DT > tanh(bool *isError=nullptr) const
Computes the hyperbolic tangent; for error handling see _ML_QUATERNION_CALC_CHECKED.
TQuaternion< DT > cosh() const
Computes the hyperbolic cosine.
FloatingPointVector< DT, 3, Vector3DataContainer< DT > > & qv()
Returns a modifiable reference to the quaternion's imaginary part as vector object,...
TQuaternion< DT > conjugate() const
Computes the conjugate.
TQuaternion< DT > even(const TQuaternion< DT > &p) const
Even product, also known as Grassman inner product.
TQuaternion< DT > negate() const
Computes the negation.
TQuaternion< DT > euclideanMult(const TQuaternion< DT > &q) const
Euclidean product of this quaternion with another one (which is non-commutative).
TQuaternion< DT > arg(bool *isError=nullptr) const
Computes the quaternion argument.
TQuaternion< DT > arcsin(bool *isError=nullptr) const
Inverse trigonometric functions: Inverse sine, inverse cosine, inverse tangent.
DT norm() const
Computes the d-norm.
TQuaternion< DT > mult(DT s) const
Multiplies this quaternion with a scalar factor s.
TQuaternion()
Standard constructor that is setting all elements to 0.
TQuaternion< DT > cos() const
Computes the cosine.
TQuaternion< DT > odd(const TQuaternion< DT > &p) const
Odd product, also known as cross-product or Grassman outer product.
DT value_type
Scalar type used for qx, qy, qz, and qw.
TQuaternion< DT > cotan(bool *isError=nullptr) const
Computes the cotangent; for error handling see _ML_QUATERNION_CALC_CHECKED.
TQuaternion< DT > tan(bool *isError=nullptr) const
Computes the tangent; for error handling see _ML_QUATERNION_CALC_CHECKED.
TQuaternion< DT > sin() const
Trigonometric functions: sine, cosine, tangent, cotangent.
TQuaternion< DT > outer(const TQuaternion< DT > &q) const
Euclidean outer product of this quaternion with another one.
void set(const DT x, const DT y, const DT z, const DT w)
Sets all quaternion elements qx, qy, qz, and qw from parameters x, y, z, and w, respectively.
TQuaternion< DT > mult(const TQuaternion< DT > &q) const
Multiplies this quaternion with another one (which is non-commutative). It is also called Grassman pr...
DT norm2() const
Computes the magnitude of the quaternion.
TQuaternion(const TQuaternion< DT2 > &q2)
Copy constructor from another TQuaternion of same or different type.
TQuaternion< DT > arcsinh() const
Inverse hyperbolic function: arcsinh, arccosh, arctanh.
TQuaternion< DT > ln() const
Computes the natural logarithm.
TQuaternion< DT > arctan(bool *isError=nullptr) const
Inverse tangent; for error handling see _ML_QUATERNION_CALC_CHECKED.
const FloatingPointVector< DT, 3, Vector3DataContainer< DT > > & qv() const
Returns a constant reference to the quaternion's imaginary part as vector object.
DT qz
Third element of imaginary part.
Tmat4< DT > getAsMat4(bool *isConvertible=nullptr) const
Converts this quaternion to a 4x4 matrix.
TQuaternion(const FloatingPointVector< DT, 4 > &v)
Creates a new TQuaternion from the four-dimensional parameter vector.
TQuaternion< DT > pow(const TQuaternion< DT > &quat) const
Computes the power of a quaternion.
DT operator[](const size_t i) const
Constant indexing operator.
TQuaternion< DT > & operator=(const TQuaternion< DT > &q)
Assignment operator.
TQuaternion< DT > arccos(bool *isError=nullptr) const
Inverse cosine; for error handling see _ML_QUATERNION_CALC_CHECKED.
TQuaternion(const FloatingPointVector< DT, 3, Vector3DataContainer< DT > > &v, DT w)
Constructor with a vector and DT parameter.
TQuaternion< DT > sgn(bool *isError=nullptr) const
Returns the sign of the quaternion.
TQuaternion(const TQuaternion &q2)=default
Explicitly generating the default constructor to avoid a warning message with gcc9.
TQuaternion< DT > add(DT s) const
Adds a scalar s to the quaternion and returns the sum. It is equivalent to a 'scalar add' of s to the...
FloatingPointVector< DT, 3, Vector3DataContainer< DT > > getImaginaryPart() const
Returns a copy of the quaternion's imaginary part.
TQuaternion< DT > arccosh() const
Computes the inverse hyperbolic cosine.
DT getRealPart() const
Returns a copy of the quaternion's real part, also called scalar() of a quaternion.
DT qy
Second element of imaginary part.
A 4x4 matrix class consisting of four row vectors.
void set(const DT val)
Sets all values to val.
Forward declarations to resolve header file dependencies.
Specialized base class for the FloatingPointVectorDataContainerBase.
bool MLValueIs0WOM(MLint8 a)
Returns true if value is 0; otherwise, it returns false.
bool MLValuesAreEqualWOM(MLint8 a, MLint8 b)
Returns true if values a and b are equal; otherwise, it returns false.
#define ML_BAD_PARAMETER
A bad/invalid parameter (or even an inappropriate image) has been passed to a module or an algorithm,...
#define ML_CHECK_FLOAT(x)
MLEXPORT std::ostream & operator<<(std::ostream &s, const ml::Field &v)
Overloads the operator '<<' for stream output of Field objects.
#define _ML_QUAT_CALC_EXP
Internal helper macro - do not use.
#define _ML_QUATERNION_CALC_CHECKED(FUNC_NAME, CALC_EXPR)
Defines a code fragment that checks and handles operations for safe expression calculations.
ScopeGuard< Functor > operator+(ScopeGuardOnExit, Functor &&fn)
FloatingPointVector< T, size, DataContainer > operator/(FloatingPointVector< T, size, DataContainer > lhs, MLdouble rhs)
Component-wise division of lhs by specialized rhs of type MLdouble.
void ML_UTILS_EXPORT printTemplateError(const char *location, MLErrorCode reason, const std::string_view &handling)
FloatingPointVector< T, size, DataContainer > & operator/=(FloatingPointVector< T, size, DataContainer > &op1, MLdouble value)
Arithmetic assignment: Component-wise division of *this by scalar value.
TQuaternion< MLdouble > Quaternion
Defines the default Quaternion type that is used by the ML; it uses double as component type.
FloatingPointVector< T, size, DataContainer > & operator-=(FloatingPointVector< T, size, DataContainer > &op1, const FloatingPointVector< T, size, DataContainer > &buffer)
Arithmetic assignment: Component-wise subtraction of buffer from *this.
TQuaternion< MLfloat > Quaternionf
A smaller Quaternion type as a specialization from TQuaternion.
bool operator==(const Tmat2< DT > &a, const Tmat2< DT > &b)
a == b ? Returns true if yes.
T operator*(const FloatingPointVector< T, size, DataContainer > &a, const FloatingPointVector< T, size, DataContainer > &b)
Dot product, returns a.dot(b).
bool operator!=(const Tmat2< DT > &a, const Tmat2< DT > &b)
a != b ? Returns true if yes.
TQuaternion< MLldouble > Quaternionld
A large Quaternion type for further extensions of the ML as a specialization from TQuaternion.
TQuaternion< MLdouble > Quaterniond
The default Quaternion type used in the ML as a specialization from TQuaternion.
FloatingPointVector< T, size, DataContainer > & operator+=(FloatingPointVector< T, size, DataContainer > &op1, const FloatingPointVector< T, size, DataContainer > &buffer)
Arithmetic assignment: Component-wise addition.
FloatingPointVector< T, size, DataContainer > operator-(FloatingPointVector< T, size, DataContainer > lhs, const FloatingPointVector< T, size, DataContainer > &rhs)
Return value is the component-wise subtraction of rhs from lhs.
FloatingPointVector< T, size, DataContainer > & operator*=(FloatingPointVector< T, size, DataContainer > &op1, MLdouble value)
Arithmetic assignment: Component-wise multiplication *this with specialized MLdouble scalar value.