Quaternion from EulerAngleXYZ

include/J3ML/LinearAlgebra/Quaternion.hpp

@@ -24,8 +24,8 @@ namespace J3ML::LinearAlgebra
         /// @param data An array of four floats to use for the quaternion, in the order 'x,y,z,w.' (== 'i,j,k,w')
         explicit Quaternion(const float *data);
 
-        explicit Quaternion(const Matrix3x3 &rotationMtrx);
-        explicit Quaternion(const Matrix4x4 &rotationMtrx);
+        explicit Quaternion(const Matrix3x3& rotationMtrx);
+        explicit Quaternion(const Matrix4x4& rotationMtrx);
 
         /// @param x The factor of i.
         /// @param y The factor of j.
@@ -34,6 +34,8 @@ namespace J3ML::LinearAlgebra
         /// @note The input data is not normalized after construction, this has to be done manually.
         Quaternion(float X, float Y, float Z, float W);
 
+        Quaternion(const EulerAngleXYZ& rhs);
+
         /// Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
         /// @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
         /// @param rotationAngleRadians The angle to rotate by, in radians. For example, Pi/4.f equals to 45 degrees, Pi/2.f is 90 degrees, etc.

src/J3ML/LinearAlgebra/Quaternion.cpp

@@ -1,9 +1,10 @@
-#include <J3ML/LinearAlgebra/Vector3.hpp>
-#include <J3ML/LinearAlgebra/Vector4.hpp>
 #include <J3ML/LinearAlgebra/Matrix3x3.hpp>
 #include <J3ML/LinearAlgebra/Matrix4x4.hpp>
 #include <J3ML/LinearAlgebra/Quaternion.hpp>
 #include <J3ML/LinearAlgebra/AxisAngle.hpp>
+#include <J3ML/LinearAlgebra/EulerAngle.hpp>
+#include <J3ML/LinearAlgebra/Vector4.hpp>
+#include <J3ML/LinearAlgebra/Vector3.hpp>
 
 namespace J3ML::LinearAlgebra {
 
@@ -382,4 +383,20 @@ namespace J3ML::LinearAlgebra {
     float Quaternion::LengthSquared() const { return x*x + y*y + z*z + w*w;}
 
     float Quaternion::Length() const { return std::sqrt(LengthSquared()); }
+
+    Quaternion::Quaternion(const EulerAngleXYZ& rhs) {
+        float cos_roll = Math::Cos(0.5f * Math::Radians(rhs.roll));
+        float sin_roll = Math::Sin(0.5f * Math::Radians(rhs.roll));
+
+        float cos_pitch = Math::Cos(0.5f * Math::Radians(rhs.pitch));
+        float sin_pitch = Math::Sin(0.5f * Math::Radians(rhs.pitch));
+
+        float cos_yaw = Math::Cos(0.5f * Math::Radians(rhs.yaw));
+        float sin_yaw = Math::Sin(0.5f * Math::Radians(rhs.yaw));
+
+        x = cos_roll * sin_pitch * sin_yaw + sin_roll * cos_pitch * cos_yaw;
+        y = -sin_roll * cos_pitch * sin_yaw + cos_roll * sin_pitch * cos_yaw;
+        z = cos_roll * cos_pitch * sin_yaw + sin_roll * sin_pitch * cos_yaw;
+        w = -sin_roll * sin_pitch * sin_yaw + cos_roll * cos_pitch * cos_yaw;
+    }
 }

tests/LinearAlgebra/QuaternionTests.hpp

@@ -4,6 +4,7 @@
 #include <jtest/jtest.hpp>
 #include <jtest/Unit.hpp>
 #include <J3ML/LinearAlgebra/Quaternion.hpp>
+#include <J3ML/LinearAlgebra/EulerAngle.hpp>
 #include <J3ML/Algorithm/RNG.hpp>
 
 jtest::Unit QuaternionUnit {"Quaternion"};
@@ -69,6 +70,16 @@ namespace QuaternionTests {
                 Quaternion lerp = q.Lerp(q2, t);
             }
         });
+        QuaternionUnit += Test("From_EulerAngleXYZ", [] {
+            Quaternion expected_result(0.1819093, 0.6706149, 0.1840604, 0.6952024);
+            EulerAngleXYZ e(10, 88, 20);
+            Quaternion from_euler(e);
+
+            jtest::check(Math::EqualAbs(expected_result.x, from_euler.x, 1e-6f));
+            jtest::check(Math::EqualAbs(expected_result.y, from_euler.y, 1e-6f));
+            jtest::check(Math::EqualAbs(expected_result.z, from_euler.z, 1e-6f));
+            jtest::check(Math::EqualAbs(expected_result.w, from_euler.w, 1e-6f));
+        });
         QuaternionUnit += Test("Mat4x4Conversion", [] { throw("Not Implemented"); });
         QuaternionUnit += Test("MulOpQuat", [] { throw("Not Implemented"); });
         QuaternionUnit += Test("DivOpQuat", [] { throw("Not Implemented"); });