12#ifndef JAU_MATH_QUATERNION_HPP_
13#define JAU_MATH_QUATERNION_HPP_
48 std::enable_if_t<std::is_floating_point_v<Value_type>,
bool> =
true >
81 : m_x(0), m_y(0), m_z(0), m_w(1) {}
84 : m_x(
x), m_y(
y), m_z(
z), m_w(
w) {}
97 return m_w*m_w + m_x*m_x + m_y*m_y + m_z*m_z;
122 return std::sqrt(magnitudeSQ);
145 return m_x *
x + m_y *
y + m_z *
z + m_w *
w;
152 return dot(quat.x(), quat.y(), quat.z(), quat.w());
174 m_x = m_y = m_z =
zero; m_w =
one;
290 return set( m_w * rhs.m_x + m_x * rhs.m_w + m_y * rhs.m_z - m_z * rhs.m_y,
291 m_w * rhs.m_y - m_x * rhs.m_z + m_y * rhs.m_w + m_z * rhs.m_x,
292 m_w * rhs.m_z + m_x * rhs.m_y - m_y * rhs.m_x + m_z * rhs.m_w,
293 m_w * rhs.m_w - m_x * rhs.m_x - m_y * rhs.m_y - m_z * rhs.m_z );
337 return set( m_x * qw + m_y * qz - m_z * qy + m_w * qx,
338 -m_x * qz + m_y * qw + m_z * qx + m_w * qy,
339 m_x * qy - m_y * qx + m_z * qw + m_w * qz,
340 -m_x * qx - m_y * qy - m_z * qz + m_w * qw);
372 return set( m_x * cos + m_w * sin,
373 m_y * cos + m_z * sin,
374 -m_y * sin + m_z * cos,
375 -m_x * sin + m_w * cos);
390 return set( m_x * cos - m_z * sin,
391 m_y * cos + m_w * sin,
392 m_x * sin + m_z * cos,
393 -m_y * sin + m_w * cos);
408 return set( m_x * cos + m_y * sin,
409 -m_x * sin + m_y * cos,
410 m_z * cos + m_w * sin,
411 -m_z * sin + m_w * cos);
465 return rotateByAngleY(headingY).rotateByAngleZ(attitudeZ).rotateByAngleX(bankX).normalize();
505 +
two * ( m_y*m_w*vecZ - m_z*m_w*vecY + m_y*m_x*vecY + m_z*m_x*vecZ );
512 +
two * ( m_x*m_y*vecX + m_z*m_y*vecZ + m_w*m_z*vecX - m_x*m_w*vecZ );
518 +
two * ( m_x*m_z*vecX + m_y*m_z*vecY - m_w*m_y*vecX + m_w*m_x*vecY );
538 if (changeAmnt ==
zero) {
540 }
else if (changeAmnt ==
one) {
549 value_type cosHalfTheta = a.m_x * bx + a.m_y * by + a.m_z * bz + a.m_w * bw;
553 if( cosHalfTheta >= 0.95f ) {
555 scale0 =
one - changeAmnt;
557 }
else if ( cosHalfTheta <= -0.99f ) {
563 if( cosHalfTheta <= -std::numeric_limits<value_type>::epsilon() ) {
569 cosHalfTheta *= -
one;
571 const value_type halfTheta = std::acos(cosHalfTheta);
572 const value_type sinHalfTheta = std::sqrt(
one - cosHalfTheta*cosHalfTheta);
575 if ( std::abs(sinHalfTheta) < 0.001f ){
582 scale0 = std::sin((
one - changeAmnt) * halfTheta) / sinHalfTheta;
583 scale1 = std::sin(changeAmnt * halfTheta) / sinHalfTheta;
587 m_x = a.m_x * scale0 + bx * scale1;
588 m_y = a.m_y * scale0 + by * scale1;
589 m_z = a.m_z * scale0 + bz * scale1;
590 m_w = a.m_w * scale0 + bw * scale1;
616 Vec3& xAxisOut,
Vec3& yAxisOut,
Vec3& zAxisOut)
noexcept {
623 xAxisOut.cross(yAxisOut, zAxisOut).normalize();
627 yAxisOut.cross(zAxisOut, xAxisOut).normalize();
640 return setFromAxes(xAxisOut, yAxisOut, zAxisOut).normalize();
665 const value_type factor = v1.length() * v2.length();
672 Vec3 tmpPivotVec = v1.cross(v2);
680 if (std::abs(v1.x) > std::abs(v1.y)) {
681 if (std::abs(v1.x) > std::abs(v1.z)) {
687 if (std::abs(v1.y) > std::abs(v1.z)) {
693 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
694 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
695 tmpPivotVec[(dominantIndex + 2) % 3] =
zero;
719 const value_type factor = v1.length() * v2.length();
726 Vec3 tmpPivotVec = v1.cross(v2);
734 if (std::abs(v1.x) > std::abs(v1.y)) {
735 if (std::abs(v1.x) > std::abs(v1.z)) {
741 if (std::abs(v1.y) > std::abs(v1.z)) {
747 tmpPivotVec[dominantIndex] = -v1[ (dominantIndex + 1) % 3 ];
748 tmpPivotVec[(dominantIndex + 1) % 3] = v1[ dominantIndex ];
749 tmpPivotVec[(dominantIndex + 2) % 3] =
zero;
790 if( vector.is_zero() ) {
795 m_x = vector.x * sin;
796 m_y = vector.y * sin;
797 m_z = vector.z * sin;
798 m_w = std::cos(halfangle);
811 const value_type sqrLength = m_x*m_x + m_y*m_y + m_z*m_z;
817 angle = std::acos(m_w) *
two;
819 axis.set( m_x * invLength,
842 return setFromEuler(angradXYZ.x, angradXYZ.y, angradXYZ.z);
877 const value_type sinHeadingY = std::sin(angle);
878 const value_type cosHeadingY = std::cos(angle);
879 angle = attitudeZ *
half;
880 const value_type sinAttitudeZ = std::sin(angle);
881 const value_type cosAttitudeZ = std::cos(angle);
882 angle = bankX *
half;
887 const value_type cosHeadingXcosAttitude = cosHeadingY * cosAttitudeZ;
888 const value_type sinHeadingXsinAttitude = sinHeadingY * sinAttitudeZ;
889 const value_type cosHeadingXsinAttitude = cosHeadingY * sinAttitudeZ;
890 const value_type sinHeadingXcosAttitude = sinHeadingY * cosAttitudeZ;
892 m_w = cosHeadingXcosAttitude * cosBankX - sinHeadingXsinAttitude * sinBankX;
893 m_x = cosHeadingXcosAttitude * sinBankX + sinHeadingXsinAttitude * cosBankX;
894 m_y = sinHeadingXcosAttitude * cosBankX + cosHeadingXsinAttitude * sinBankX;
895 m_z = cosHeadingXsinAttitude * cosBankX - sinHeadingXcosAttitude * sinBankX;
920 const value_type unit = sqx + sqy + sqz + sqw;
923 if (
test > 0.499f * unit) {
925 two * std::atan2(m_x, m_w),
927 }
else if (
test < -0.499f * unit) {
929 -
two * std::atan2(m_x, m_w),
932 return Vec3( std::atan2(
two * m_x * m_w -
two * m_y * m_z, -sqx + sqy - sqz + sqw),
933 std::atan2(
two * m_y * m_w -
two * m_x * m_z, sqx - sqy - sqz + sqw),
934 std::asin(
two *
test / unit) );
963 m_x = ( m21 - m12 ) * S;
964 m_y = ( m02 - m20 ) * S;
965 m_z = ( m10 - m01 ) * S;
966 }
else if ( m00 > m11 && m00 > m22) {
968 m_w = ( m21 - m12 ) * S;
970 m_y = ( m10 + m01 ) * S;
971 m_z = ( m02 + m20 ) * S;
972 }
else if ( m11 > m22 ) {
974 m_w = ( m02 - m20 ) * S;
975 m_x = ( m20 + m01 ) * S;
977 m_z = ( m21 + m12 ) * S;
980 m_w = ( m10 - m01 ) * S;
981 m_x = ( m02 + m20 ) * S;
982 m_y = ( m21 + m12 ) * S;
1003 return setFromMat(m.m00, m.m01, m.m02, m.m10, m.m11, m.m12, m.m20, m.m21, m.m22);
1019 xAxis.y, yAxis.y, zAxis.y,
1020 xAxis.z, yAxis.z, zAxis.z);
1064 srecip =
two / norm;
1080 m.m00 =
one - ( yy + zz );
1081 m.m01 = ( xy - zw );
1082 m.m02 = ( xz + yw );
1085 m.m10 = ( xy + zw );
1086 m.m11 =
one - ( xx + zz );
1087 m.m12 = ( yz - xw );
1090 m.m20 = ( xz - yw );
1091 m.m21 = ( yz + xw );
1092 m.m22 =
one - ( xx + yy );
1095 m.m30 = m.m31 = m.m32 =
zero;
1110 tmp.getColumn(2, zAxis);
1111 tmp.getColumn(1, yAxis);
1112 tmp.getColumn(0, xAxis);
1124 toAxes(xAxis, yAxis, zAxis, tmp);
1146 return "Quat[x "+std::to_string(m_x)+
", y "+std::to_string(m_y)+
", z "+std::to_string(m_z)+
", w "+std::to_string(m_w)+
"]";
1151 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1157 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1163 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1169 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1175 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1181 std::enable_if_t<std::is_floating_point_v<T>,
bool> =
true>
1183 return out << v.toString();
1187static_assert(
alignof(float) ==
alignof(
Quat4f));
1191 typename std::enable_if_t<std::is_floating_point_v<Value_type>,
bool> sf_type>
1205 typename std::enable_if_t<std::is_floating_point_v<Value_type>,
bool> sf_type>
1208 return res.setFromMat(m00, m01, m02, m10, m11, m12, m20, m21, m22);
1212 typename std::enable_if_t<std::is_floating_point_v<Value_type>,
bool> sf_type>
1216 return mul( quat.toMatrix(tmp) );
Basic 4x4 value_type matrix implementation using fields for intensive use-cases (host operations).
Quaternion< value_type, std::is_floating_point_v< Value_type > > Quat
constexpr_cxx26 Matrix4 & rotate(const value_type ang_rad, const value_type x, const value_type y, const value_type z) noexcept
Rotate this matrix about give axis and angle in radians, i.e.
constexpr Matrix4() noexcept
Creates a new identity matrix.
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.
constexpr value_type magnitudeSquared() const noexcept
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 Mat4 toMatrix() const noexcept
Transform this quaternion to a normalized 4x4 column matrix representing the rotation.
static constexpr const value_type allowed_deviation
constexpr Quaternion & conjugate() noexcept
Conjugates this quaternion [-x, -y, -z, w].
static constexpr const value_type one
constexpr void toAxes(Vec3 &xAxis, Vec3 &yAxis, Vec3 &zAxis, Matrix4< value_type > &tmp) const noexcept
Extracts this quaternion's orthogonal rotation axes.
static constexpr const value_type zero
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 Quaternion & setFromMat(const Mat4 &m) noexcept
Compute the quaternion from a 3x3 column rotation matrix from Mat4 instance.
constexpr_cxx26 value_type magnitude() const noexcept
Return the magnitude of this quaternion, i.e.
constexpr Mat4 & toMatrix(Mat4 &m) 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
static constexpr const value_type two
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
Matrix4< value_type > Mat4
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...
Vector3F< value_type > Vec3
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.
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.
static constexpr const value_type half
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 value_type length() const noexcept
Return the length of a vector, a.k.a the norm or magnitude
std::enable_if_t< std::is_floating_point_v< T >, bool > 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_t< std::is_floating_point_v< T >, bool > 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_t< std::is_floating_point_v< T >, bool > 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 Quaternion< T > operator-(const Quaternion< T > &lhs, const Quaternion< T > &rhs) noexcept
Quat & getRotation(Quat &res) const noexcept
Returns the rotation [m00 .
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
Matrix4 & setToRotation(const Quat &q)
Set this matrix to rotation using the given Quaternion.
std::ostream & operator<<(std::ostream &out, const Addr48Bit &a)