Implemented Quaternion::Equals

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.
+    /// Creates a new quaternion that rotates about the positive X axis by the given rotation.
-     static Quaternion RotateX(float rad);
+    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 sourceDirection vector (in world space) to coincide with the
+    /// Creates a new quaternion that rotates about the positive Y axis by the given rotation.
-     /// targetDirection vector (in world space).
+    static Quaternion RotateY(float rad);
-     /// 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
+    /// Creates a new quaternion that rotates about the positive Z axis by the given rotation.
-     /// 1. rotates sourceDirection vector to coincide with the targetDirection vector, and then
+    static Quaternion RotateZ(float rad);
-     /// 2. rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that
+
-     /// sourceDirection must look at targetDirection
+    /// Creates a new quaternion that rotates sourceDirection vector (in world space) to coincide with the
-     static Quaternion RotateFromTo(const Vector3& sourceDirection, const Vector3& targetDirection, const Vector3& sourceDirection2, const Vector3& targetDirection2);
+    /// 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.
+    /// Returns a uniformly random unitary quaternion.
-     static Quaternion RandomRotation(RNG &rng);
+    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;
+    [[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 Lerp(const Quaternion &source, const Quaternion &target, float t);
-    static Quaternion Slerp(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.

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);
+    }
 }