24#ifndef JAU_QUATERNION_HPP_
25#define JAU_QUATERNION_HPP_
61 std::enable_if_t<std::is_floating_point_v<Value_type>,
bool> =
true>
85 constexpr static const value_type allowed_deviation =
value_type(8.4) * std::numeric_limits<value_type>::epsilon();
93 : m_x(0), m_y(0), m_z(0), m_w(1) {}
96 : m_x(x), m_y(y), m_z(z), m_w(w) {}
109 return m_w*m_w + m_x*m_x + m_y*m_y + m_z*m_z;
127 const value_type magnitudeSQ = magnitudeSquared();
134 return std::sqrt(magnitudeSQ);
157 return m_x * x + m_y * y + m_z * z + m_w * w;
164 return dot(quat.x(), quat.y(), quat.z(), quat.w());
186 m_x = m_y = m_z = 0.0f; m_w = 1.0f;
238 const value_type magnitudeSQ = magnitudeSquared();
302 return set( m_w * rhs.m_x + m_x * rhs.m_w + m_y * rhs.m_z - m_z * rhs.m_y,
303 m_w * rhs.m_y - m_x * rhs.m_z + m_y * rhs.m_w + m_z * rhs.m_x,
304 m_w * rhs.m_z + m_x * rhs.m_y - m_y * rhs.m_x + m_z * rhs.m_w,
305 m_w * rhs.m_w - m_x * rhs.m_x - m_y * rhs.m_y - m_z * rhs.m_z );
349 return set( m_x * qw + m_y * qz - m_z * qy + m_w * qx,
350 -m_x * qz + m_y * qw + m_z * qx + m_w * qy,
351 m_x * qy - m_y * qx + m_z * qw + m_w * qz,
352 -m_x * qx - m_y * qy - m_z * qz + m_w * qw);
369 return rotateByAngleNormalAxis(angle, axis.x, axis.y, axis.z);
380 return rotateByAngleX(std::sin(halfAngle), std::cos(halfAngle));
384 return set( m_x * cos + m_w * sin,
385 m_y * cos + m_z * sin,
386 -m_y * sin + m_z * cos,
387 -m_x * sin + m_w * cos);
398 return rotateByAngleY(std::sin(halfAngle), std::cos(halfAngle));
402 return set( m_x * cos - m_z * sin,
403 m_y * cos + m_w * sin,
404 m_x * sin + m_z * cos,
405 -m_y * sin + m_w * cos);
416 return rotateByAngleZ(std::sin(halfAngle), std::cos(halfAngle));
420 return set( m_x * cos + m_y * sin,
421 -m_x * sin + m_y * cos,
422 m_z * cos + m_w * sin,
423 -m_z * sin + m_w * cos);
442 return rotateByEuler(angradXYZ.x, angradXYZ.y, angradXYZ.z);
490 rotateVector(in, out);
517 + 2.0f * ( m_y*m_w*vecZ - m_z*m_w*vecY + m_y*m_x*vecY + m_z*m_x*vecZ );
524 + 2.0f * ( m_x*m_y*vecX + m_z*m_y*vecZ + m_w*m_z*vecX - m_x*m_w*vecZ );
530 + 2.0f * ( m_x*m_z*vecX + m_y*m_z*vecY - m_w*m_y*vecX + m_w*m_x*vecY );
550 if (changeAmnt == 0.0f) {
552 }
else if (changeAmnt == 1.0f) {
561 value_type cosHalfTheta = a.m_x * bx + a.m_y * by + a.m_z * bz + a.m_w * bw;
565 if( cosHalfTheta >= 0.95f ) {
567 scale0 = 1.0f - changeAmnt;
569 }
else if ( cosHalfTheta <= -0.99f ) {
575 if( cosHalfTheta <= -std::numeric_limits<value_type>::epsilon() ) {
581 cosHalfTheta *= -1.0f;
583 const value_type halfTheta = std::acos(cosHalfTheta);
584 const value_type sinHalfTheta = std::sqrt(1.0f - cosHalfTheta*cosHalfTheta);
587 if (
std::abs(sinHalfTheta) < 0.001f ){
594 scale0 = std::sin((1.0f - changeAmnt) * halfTheta) / sinHalfTheta;
595 scale1 = std::sin(changeAmnt * halfTheta) / sinHalfTheta;
599 m_x = a.m_x * scale0 + bx * scale1;
600 m_y = a.m_y * scale0 + by * scale1;
601 m_z = a.m_z * scale0 + bz * scale1;
602 m_w = a.m_w * scale0 + bw * scale1;
628 Vec3& xAxisOut,
Vec3& yAxisOut,
Vec3& zAxisOut)
noexcept {
630 (zAxisOut = directionIn).normalize();
634 (yAxisOut = upIn).normalize();
635 xAxisOut.cross(yAxisOut, zAxisOut).
normalize();
639 yAxisOut.cross(zAxisOut, xAxisOut).
normalize();
652 return setFromAxes(xAxisOut, yAxisOut, zAxisOut).
normalize();
677 const value_type factor = v1.length() * v2.length();
679 return setIdentity();
705 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
706 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
707 tmpPivotVec[(dominantIndex + 2) % 3] = 0.0f;
709 return setFromAngleAxis(theta, tmpPivotVec);
731 const value_type factor = v1.length() * v2.length();
733 return setIdentity();
759 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
760 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
761 tmpPivotVec[(dominantIndex + 2) % 3] = 0.0f;
763 return setFromAngleNormalAxis(theta, tmpPivotVec);
783 return setFromAngleNormalAxis(angle,
Vec3(vector).normalize());
802 if( vector.is_zero() ) {
807 m_x = vector.x * sin;
808 m_y = vector.y * sin;
809 m_z = vector.z * sin;
810 m_w = std::cos(halfangle);
823 const value_type sqrLength = m_x*m_x + m_y*m_y + m_z*m_z;
827 axis.set( 1.0f, 0.0f, 0.0f );
829 angle = std::acos(m_w) * 2.0f;
830 const value_type invLength = 1.0f / std::sqrt(sqrLength);
831 axis.set( m_x * invLength,
854 return setFromEuler(angradXYZ.x, angradXYZ.y, angradXYZ.z);
886 return setIdentity();
889 const value_type sinHeadingY = std::sin(angle);
890 const value_type cosHeadingY = std::cos(angle);
891 angle = attitudeZ * 0.5f;
892 const value_type sinAttitudeZ = std::sin(angle);
893 const value_type cosAttitudeZ = std::cos(angle);
894 angle = bankX * 0.5f;
899 const value_type cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
900 const value_type sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
901 const value_type cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
902 const value_type sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
904 m_w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
905 m_x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
906 m_y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
907 m_z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
932 const value_type unit = sqx + sqy + sqz + sqw;
935 if (
test > 0.499f * unit) {
937 2.0f * std::atan2(m_x, m_w),
939 }
else if (
test < -0.499f * unit) {
941 -2.0 * std::atan2(m_x, m_w),
944 return Vec3( std::atan2(2.0f * m_x * m_w - 2.0f * m_y * m_z, -sqx + sqy - sqz + sqw),
945 std::atan2(2.0f * m_y * m_w - 2.0f * m_x * m_z, sqx - sqy - sqz + sqw),
946 std::asin( 2.0f *
test / unit) );
975 m_x = ( m21 - m12 ) * S;
976 m_y = ( m02 - m20 ) * S;
977 m_z = ( m10 - m01 ) * S;
978 }
else if ( m00 > m11 && m00 > m22) {
979 const value_type S = 0.5f / std::sqrt(1.0f + m00 - m11 - m22);
980 m_w = ( m21 - m12 ) * S;
982 m_y = ( m10 + m01 ) * S;
983 m_z = ( m02 + m20 ) * S;
984 }
else if ( m11 > m22 ) {
985 const value_type S = 0.5f / std::sqrt(1.0f + m11 - m00 - m22);
986 m_w = ( m02 - m20 ) * S;
987 m_x = ( m20 + m01 ) * S;
989 m_z = ( m21 + m12 ) * S;
991 const value_type S = 0.5f / std::sqrt(1.0f + m22 - m00 - m11);
992 m_w = ( m10 - m01 ) * S;
993 m_x = ( m02 + m20 ) * S;
994 m_y = ( m21 + m12 ) * S;
1015 return setFromMat(m.m00, m.m01, m.m02, m.m10, m.m11, m.m12, m.m20, m.m21, m.m22);
1030 return setFromMat(xAxis.x, yAxis.x, zAxis.x,
1031 xAxis.y, yAxis.y, zAxis.y,
1032 xAxis.z, yAxis.z, zAxis.z);
1050 Mat4f m; toMatrix(m);
return m;
1072 srecip = 2.0f / norm;
1093 m.m00 = 1.0f - ( yy + zz );
1094 m.m01 = ( xy - zw );
1095 m.m02 = ( xz + yw );
1098 m.m10 = ( xy + zw );
1099 m.m11 = 1.0f - ( xx + zz );
1100 m.m12 = ( yz - xw );
1103 m.m20 = ( xz - yw );
1104 m.m21 = ( yz + xw );
1105 m.m22 = 1.0f - ( xx + yy );
1108 m.m30 = m.m31 = m.m32 = 0.0f;
1123 tmp.getColumn(2, zAxis);
1124 tmp.getColumn(1, yAxis);
1125 tmp.getColumn(0, xAxis);
1137 toAxes(xAxis, yAxis, zAxis, tmp);
1152 return std::abs(m_x - o.m_x) <= allowed_deviation &&
1153 std::abs(m_y - o.m_y) <= allowed_deviation &&
1154 std::abs(m_z - o.m_z) <= allowed_deviation &&
1155 std::abs(m_w - o.m_w) <= allowed_deviation;
1164 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1170 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1176 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1182 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1188 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1194 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1196 return out << v.toString();
1200static_assert(
alignof(float) ==
alignof(
Quat4f));
Basic 4x4 value_type matrix implementation using fields for intensive use-cases (host operations).
Quaternion implementation supporting Gimbal-Lock free rotations.
constexpr_cxx26 Quaternion & rotateByAngleX(const value_type angle) noexcept
Rotate this quaternion around X axis with the given angle in radians.
constexpr Quaternion & operator+=(const Quaternion &rhs) noexcept
Add a quaternion: this = this + rhs, returns this.
const value_type * const_iterator
Vec3 rotateVector(const Vec3 &in) noexcept
constexpr Quaternion & normalize() noexcept
Normalize a quaternion required if to be used as a rotational quaternion.
constexpr Quaternion(const value_type x, const value_type y, const value_type z, const value_type w) noexcept
constexpr Quaternion & set(const value_type x, const value_type y, const value_type z, const value_type w) noexcept
Set all values of this quaternion using the given components.
constexpr Quaternion(Quaternion &&o) noexcept=default
constexpr Quaternion & conjugate() noexcept
Conjugates this quaternion [-x, -y, -z, w].
constexpr void toAxes(Vec3 &xAxis, Vec3 &yAxis, Vec3 &zAxis, Matrix4< value_type > &tmp) const noexcept
Extracts this quaternion's orthogonal rotation axes.
constexpr_cxx26 Quaternion & setFromEuler(const Vec3 &angradXYZ) noexcept
Initializes this quaternion from the given Euler rotation array angradXYZ in radians.
constexpr Quaternion & operator-=(const Quaternion &rhs) noexcept
Subtract a quaternion: this = this - rhs, returns this.
constexpr Quaternion & operator*=(const value_type rhs) noexcept
Scale this quaternion by a scalar: this = this * rhs, returns this.
constexpr_cxx26 Quaternion & setFromNormalVectors(const Vec3 &v1, const Vec3 &v2) noexcept
Initialize this quaternion from two normalized vectors.
constexpr_cxx26 Quaternion & rotateByAngleNormalAxis(const value_type angle, const value_type axisX, const value_type axisY, const value_type axisZ) noexcept
Rotate this quaternion by the given angle and axis.
constexpr bool isIdentity() const noexcept
Returns true if this quaternion has identity.
constexpr value_type dot(value_type x, value_type y, value_type z, value_type w) const noexcept
Returns the dot product of this quaternion with the given x,y,z and m_w components.
std::string toString() const noexcept
constexpr_cxx26 Quaternion & rotateByEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept
Rotates this quaternion from the given Euler rotation angles in radians.
constexpr Quaternion & invert() noexcept
Invert the quaternion If rotational, will produce a the inverse rotation.
constexpr_cxx26 Vec3 toEuler() noexcept
Transform this quaternion to Euler rotation angles in radians (pitchX, yawY and rollZ).
constexpr_cxx26 value_type magnitude() const noexcept
Return the magnitude of this quaternion, i.e.
constexpr Quaternion & setFromMat(const Mat4f &m) noexcept
Compute the quaternion from a 3x3 column rotation matrix from mat4f instance.
constexpr Mat4f toMatrix() const noexcept
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
constexpr void setY(value_type y) noexcept
Vec3 & rotateVector(const Vec3 &in, Vec3 &out) noexcept
constexpr Quaternion() noexcept
constexpr Quaternion & setIdentity() noexcept
Quaternion & setLookAt(const Vec3 &directionIn, const Vec3 &upIn, Vec3 &xAxisOut, Vec3 &yAxisOut, Vec3 &zAxisOut) noexcept
Set this quaternion to equal the rotation required to point the z-axis at direction and the y-axis to...
constexpr_cxx26 Quaternion & setFromAngleNormalAxis(const value_type angle, const Vec3 &vector) noexcept
constexpr value_type dot(const Quaternion &quat) const noexcept
Returns the dot product of this quaternion with the given quaternion.
constexpr value_type x() const noexcept
Quaternion & setFromAngleAxis(const value_type angle, const Vec3 &vector) noexcept
constexpr Quaternion & rotateByAngleY(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around Y axis with the given angle's sin + cos values.
constexpr Quaternion & operator=(Quaternion &&) noexcept=default
constexpr Quaternion & operator*=(const Quaternion &rhs) noexcept
Multiply this quaternion: this = this * rhs, returns this.
Quaternion & rotateByEuler(const Vec3 &angradXYZ) noexcept
Rotates this quaternion from the given Euler rotation array angradXYZ in radians.
const value_type & const_reference
constexpr Quaternion & operator=(const Quaternion &) noexcept=default
constexpr value_type y() const noexcept
constexpr Quaternion & setFromAxes(const Vec3 &xAxis, const Vec3 &yAxis, const Vec3 &zAxis) noexcept
Initializes this quaternion to represent a rotation formed by the given three orthogonal axes.
constexpr_cxx26 value_type toAngleAxis(Vec3 &axis) const noexcept
Transform the rotational quaternion to axis based rotation angles.
const value_type * const_pointer
constexpr bool operator==(const Quaternion &o) const noexcept
constexpr value_type w() const noexcept
constexpr void setX(value_type x) noexcept
constexpr_cxx26 Quaternion & setFromVectors(const Vec3 &v1, const Vec3 &v2) noexcept
Initialize this quaternion from two vectors.
constexpr Quaternion & setFromMat(const value_type m00, const value_type m01, const value_type m02, const value_type m10, const value_type m11, const value_type m12, const value_type m20, const value_type m21, const value_type m22) noexcept
Compute the quaternion from a 3x3 column rotation matrix.
constexpr_cxx26 Quaternion & setSlerp(const Quaternion &a, const Quaternion &b, const value_type changeAmnt) noexcept
Set this quaternion to a spherical linear interpolation between the given start and end quaternions b...
constexpr Quaternion & rotateByAngleZ(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around Y axis with the given angle's sin + cos values.
constexpr void setZ(value_type z) noexcept
constexpr Quaternion(const Quaternion &o) noexcept=default
constexpr_cxx26 Quaternion & rotateByAngleNormalAxis(const value_type angle, const Vec3 &axis) noexcept
Rotate this quaternion by the given angle and axis.
constexpr_cxx26 Quaternion & rotateByAngleZ(value_type angle) noexcept
Rotate this quaternion around Z axis with the given angle in radians.
Vector3F< value_type, std::is_floating_point_v< Value_type > > Vec3
constexpr Quaternion & rotateByAngleX(const value_type sin, const value_type cos) noexcept
Rotate this quaternion around X axis with the given angle's sin + cos values.
constexpr Mat4f & toMatrix(Mat4f &m) const noexcept
Transform this quaternion to a normalized 4x4 column matrix representing the rotation,...
constexpr void toAxes(Vec3 &xAxis, Vec3 &yAxis, Vec3 &zAxis) const noexcept
Extracts this quaternion's orthogonal rotation axes.
constexpr_cxx26 Quaternion & rotateByAngleY(value_type angle) noexcept
Rotate this quaternion around Y axis with the given angle in radians.
constexpr value_type z() const noexcept
constexpr void setW(value_type w) noexcept
constexpr_cxx26 Quaternion & setFromEuler(const value_type bankX, const value_type headingY, const value_type attitudeZ) noexcept
Initializes this quaternion from the given Euler rotation angles in radians.
3D vector using three value_type components.
constexpr Vector3F & set(const Vec2f &o, const value_type z_) noexcept
TODO constexpr bool operator<=>(const vec3f_t& rhs ) const noexcept { return ... }.
constexpr value_type length() const noexcept
Return the length of a vector, a.k.a the norm or magnitude
constexpr Vector3F cross(const Vector3F &b) const noexcept
cross product this x b
std::string to_string(const alphabet &v) noexcept
constexpr void set(EAD_Event_Type &mask, const EAD_Event_Type bit) noexcept
std::enable_if< std::is_floating_point_v< T >, bool >::type constexpr is_zero(const T &a, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if the given value is less than epsilon, w/ epsilon > 0.
std::enable_if< std::is_floating_point_v< T >, bool >::type constexpr equals(const T &a, const T &b, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if both values are equal, i.e.
std::enable_if< std::is_floating_point_v< T >, bool >::type constexpr is_zero3f(const T &a, const T &b, const T &c, const T &epsilon=std::numeric_limits< T >::epsilon()) noexcept
Returns true if all given values a, b and c are less than epsilon, w/ epsilon > 0.
constexpr T min(const T x, const T y) noexcept
Returns the minimum of two integrals (w/ branching) in O(1)
constexpr T max(const T x, const T y) noexcept
Returns the maximum of two integrals (w/ branching) in O(1)
constexpr T abs(const T x) noexcept
Returns the absolute value of an arithmetic number (w/ branching) in O(1)
constexpr Quaternion< T > operator-(const Quaternion< T > &lhs, const Quaternion< T > &rhs) noexcept
Quaternion< float > Quat4f
constexpr Matrix4< T > operator*(const Matrix4< T > &lhs, const Matrix4< T > &rhs) noexcept
constexpr Quaternion< T > operator+(const Quaternion< T > &lhs, const Quaternion< T > &rhs) noexcept
std::ostream & operator<<(std::ostream &out, const Matrix4< T > &v) noexcept
constexpr const jau::fraction_i64 zero(0l, 1lu)
zero is 0/1
constexpr const jau::fraction_i64 one(1l, 1lu)
one is 10^0 or 1/1