Added a ToEulerAngle function

src/Quaternion.cpp

@@ -102,3 +102,19 @@ Matrix<3, 3> Quaternion::ToRotationMatrix() const
         2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)};
     return rotationMatrix;
 };
+
+Matrix<3, 1> Quaternion::ToEulerAngle() const
+{
+    float sqv1 = this->v1 * this->v1;
+    float sqv2 = this->v2 * this->v2;
+    float sqv3 = this->v3 * this->v3;
+    float sqw = this->w * this->w;
+
+    Matrix<3, 1> eulerAngle;
+    {
+        atan2(2.0 * (this->v1 * this->v2 + this->v3 * this->w), (sqv1 - sqv2 - sqv3 + sqw));
+        asin(-2.0 * (this->v1 * this->v3 - this->v2 * this->w) / (sqv1 + sqv2 + sqv3 + sqw));
+        atan2(2.0 * (this->v2 * this->v3 + this->v1 * this->w), (-sqv1 - sqv2 + sqv3 + sqw));
+    };
+    return eulerAngle;
+}

src/Quaternion.h

@@ -69,8 +69,18 @@ public:
      */
     void Normalize();
 
+    /**
+     * @brief Convert the quaternion to a rotation matrix
+     * @return The rotation matrix
+     */
     Matrix<3, 3> ToRotationMatrix() const;
 
+    /**
+     * @brief Convert the quaternion to an Euler angle representation
+     * @return The Euler angle representation of the quaternion
+     */
+    Matrix<3, 1> ToEulerAngle() const;
+
     // Give people an easy way to access the elements
     float &w{matrix[0]};
     float &v1{matrix[1]};