diff --git a/include/J3ML/LinearAlgebra/Quaternion.hpp b/include/J3ML/LinearAlgebra/Quaternion.hpp index a7f4f18..1bbaea3 100644 --- a/include/J3ML/LinearAlgebra/Quaternion.hpp +++ b/include/J3ML/LinearAlgebra/Quaternion.hpp @@ -25,18 +25,24 @@ public: public: /// The default constructor does not initialize any member values. Quaternion() = default; + /// Copy constructor Quaternion(const Quaternion &rhs); + /// Quaternion from Matrix3x3 - explicit Quaternion(const Matrix3x3& ro_mat); + explicit Quaternion(const Matrix3x3 &ro_mat); + /// Quaternion from Matrix4x4 RotatePart. - explicit Quaternion(const Matrix4x4& ro_mat); + explicit Quaternion(const Matrix4x4 &ro_mat); + /// Quaternion from EulerAngleXYZ. - explicit Quaternion(const EulerAngleXYZ& rhs); + explicit Quaternion(const EulerAngleXYZ &rhs); + /// Quaternion from AxisAngle. - explicit Quaternion(const AxisAngle& angle); + explicit Quaternion(const AxisAngle &angle); + /// Quaternion from Vector4 (no conversion). - explicit Quaternion(const Vector4& vector4); + explicit Quaternion(const Vector4 &vector4); /// @param x The factor of i. /// @param y The factor of j. @@ -71,33 +77,39 @@ public: quaternion Q it holds that M * localForward = targetDirection, and M * localUp lies in the plane spanned by the vectors targetDirection and worldUp. @see RotateFromTo() */ - static Quaternion LookAt(const Vector3& localForward, const Vector3& targetDirection, const Vector3& localUp, const Vector3& worldUp); + static Quaternion LookAt(const Vector3 &localForward, const Vector3 &targetDirection, const Vector3 &localUp, const Vector3 &worldUp); - /// Creates a new quaternion that rotates about the positive X axis by the given rotation. - static Quaternion RotateX(float rad); - /// Creates a new quaternion that rotates about the positive Y axis by the given rotation. - static Quaternion RotateY(float rad); - /// Creates a new quaternion that rotates about the positive Z axis by the given rotation. - static Quaternion RotateZ(float rad); + /// Creates a new quaternion that rotates about the positive X axis by the given rotation. + static Quaternion RotateX(float rad); - /// Creates a new quaternion that rotates sourceDirection vector (in world space) to coincide with the - /// targetDirection vector (in world space). - /// Rotation is performed about the origin. - /// The vectors sourceDirection and targetDirection are assumed to be normalized. - /// @note There are multiple such rotations - this function returns the rotation that has the shortest angle - /// (when decomposed to axis-angle notation). - static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection); - static Quaternion RotateFromTo(const Vector4& sourceDirection, const Vector4& targetDirection); + /// Creates a new quaternion that rotates about the positive Y axis by the given rotation. + static Quaternion RotateY(float rad); - /// Creates a new quaternion that - /// 1. rotates sourceDirection vector to coincide with the targetDirection vector, and then - /// 2. rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that - /// sourceDirection must look at targetDirection - static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection, const Vector3& sourceDirection2, const Vector3& targetDirection2); + /// Creates a new quaternion that rotates about the positive Z axis by the given rotation. + static Quaternion RotateZ(float rad); + + /// Creates a new quaternion that rotates sourceDirection vector (in world space) to coincide with the + /// targetDirection vector (in world space). + /// Rotation is performed about the origin. + /// The vectors sourceDirection and targetDirection are assumed to be normalized. + /// @note There are multiple such rotations - this function returns the rotation that has the shortest angle + /// (when decomposed to axis-angle notation). + static Quaternion RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection); + + static Quaternion RotateFromTo(const Vector4 &sourceDirection, const Vector4 &targetDirection); + + /// Creates a new quaternion that + /// 1. rotates sourceDirection vector to coincide with the targetDirection vector, and then + /// 2. rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that + /// sourceDirection must look at targetDirection + static Quaternion + RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection, const Vector3 &sourceDirection2, + const Vector3 &targetDirection2); - /// Returns a uniformly random unitary quaternion. - static Quaternion RandomRotation(RNG &rng); + /// Returns a uniformly random unitary quaternion. + static Quaternion RandomRotation(RNG &rng); + public: /// Inverses this quaternion in-place. /// @note For optimization purposes, this function assumes that the quaternion is unitary, in which @@ -107,8 +119,10 @@ public: /// Returns an inverted copy of this quaternion. [[nodiscard]] Quaternion Inverted() const; + /// Computes the conjugate of this quaternion in-place. void Conjugate(); + /// Returns a conjugated copy of this quaternion. [[nodiscard]] Quaternion Conjugated() const; @@ -121,10 +135,13 @@ public: /// Returns the local +X axis in the post-transformed coordinate space. This is the same as transforming the vector (1,0,0) by this quaternion. [[nodiscard]] Vector3 WorldX() const; + /// Returns the local +Y axis in the post-transformed coordinate space. This is the same as transforming the vector (0,1,0) by this quaternion. [[nodiscard]] Vector3 WorldY() const; + /// Returns the local +Z axis in the post-transformed coordinate space. This is the same as transforming the vector (0,0,1) by this quaternion. [[nodiscard]] Vector3 WorldZ() const; + /// Returns the axis of rotation for this quaternion. [[nodiscard]] Vector3 Axis() const; @@ -132,23 +149,31 @@ public: [[nodiscard]] float Angle() const; [[nodiscard]] float LengthSquared() const; + [[nodiscard]] float Length() const; [[nodiscard]] Matrix3x3 ToMatrix3x3() const; + [[nodiscard]] Matrix4x4 ToMatrix4x4() const; [[nodiscard]] Matrix4x4 ToMatrix4x4(const Vector3 &translation) const; - [[nodiscard]] Vector3 Transform(const Vector3& vec) const; + [[nodiscard]] Vector3 Transform(const Vector3 &vec) const; + [[nodiscard]] Vector3 Transform(float X, float Y, float Z) const; + // Note: We only transform the x,y,z components of 4D vectors, w is left untouched - [[nodiscard]] Vector4 Transform(const Vector4& vec) const; + [[nodiscard]] Vector4 Transform(const Vector4 &vec) const; + [[nodiscard]] Vector4 Transform(float X, float Y, float Z, float W) const; - [[nodiscard]] Quaternion Lerp(const Quaternion& b, float t) const; - static Quaternion Lerp(const Quaternion &source, const Quaternion& target, float t); - [[nodiscard]] Quaternion Slerp(const Quaternion& q2, float t) const; - static Quaternion Slerp(const Quaternion &source, const Quaternion& target, float t); + [[nodiscard]] Quaternion Lerp(const Quaternion &b, float t) const; + + static Quaternion Lerp(const Quaternion &source, const Quaternion &target, float t); + + [[nodiscard]] Quaternion Slerp(const Quaternion &q2, float t) const; + + static Quaternion Slerp(const Quaternion &source, const Quaternion &target, float t); /// Returns the 'from' vector rotated towards the 'to' vector by the given normalized time parameter. /** This function slerps the given 'form' vector toward the 'to' vector. @@ -157,7 +182,7 @@ public: @param t The interpolation time parameter, in the range [0, 1]. Input values outside this range are silently clamped to the [0, 1] interval. @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */ - static Vector3 SlerpVector(const Vector3& from, const Vector3& to, float t); + static Vector3 SlerpVector(const Vector3 &from, const Vector3 &to, float t); /// Returns the 'from' vector rotated towards the 'to' vector by the given absolute angle, in radians. /** This function slerps the given 'from' vector towards the 'to' vector. @@ -167,23 +192,25 @@ public: angle between 'from' and 'to' is smaller than this angle, then the vector 'to' is returned. Input values outside this range are silently clamped to the [0, pi] interval. @return A spherical linear interpolation of the vector 'from' towards the vector 'to'. */ - static Vector3 SlerpVectorAbs(const Vector3 &from, const Vector3& to, float angleRadians); + static Vector3 SlerpVectorAbs(const Vector3 &from, const Vector3 &to, float angleRadians); /// Normalizes this quaternion in-place. /// @returns false if failure, true if success. [[nodiscard]] bool Normalize(); + /// Returns a normalized copy of this quaternion. [[nodiscard]] Quaternion Normalized() const; /// Returns true if the length of this quaternion is one. [[nodiscard]] bool IsNormalized(float epsilon = 1e-5f) const; + [[nodiscard]] bool IsInvertible(float epsilon = 1e-3f) const; /// Returns true if the entries of this quaternion are all finite. [[nodiscard]] bool IsFinite() const; /// Returns true if this quaternion equals rhs, up to the given epsilon. - [[nodiscard]] bool Equals(const Quaternion& rhs, float epsilon = 1e-3f) const; + [[nodiscard]] bool Equals(const Quaternion &rhs, float epsilon = 1e-3f) const; /// Compares whether this Quaternion and the given Quaternion are identical bit-by-bit in the underlying representation. /// @note Prefer using this over e.g. memcmp, since there can be SSE-related padding in the structures. diff --git a/src/J3ML/LinearAlgebra/Quaternion.cpp b/src/J3ML/LinearAlgebra/Quaternion.cpp index 002d281..073c5d6 100644 --- a/src/J3ML/LinearAlgebra/Quaternion.cpp +++ b/src/J3ML/LinearAlgebra/Quaternion.cpp @@ -30,6 +30,8 @@ namespace J3ML::LinearAlgebra { auto field_w = std::sqrt(1.f + m00 + m11 + m22) / 2.f; float w4 = (4.f * field_w); + + x = (m21 - m12) / w4; y = (m02 - m20) / w4; z = (m10 - m01) / w4; @@ -379,4 +381,11 @@ namespace J3ML::LinearAlgebra { z = rhs.z; w = rhs.w; } + + bool Quaternion::Equals(const Quaternion &rhs, float epsilon) const { + return Math::Equal(this->x, rhs.x, epsilon) && + Math::Equal(this->y, rhs.y, epsilon) && + Math::Equal(this->z, rhs.z, epsilon) && + Math::Equal(this->w, rhs.w, epsilon); + } } \ No newline at end of file