diff --git a/src/Quaternion.cpp b/src/Quaternion.cpp index 4ea9748..fceb948 100644 --- a/src/Quaternion.cpp +++ b/src/Quaternion.cpp @@ -101,4 +101,20 @@ Matrix<3, 3> Quaternion::ToRotationMatrix() const 2 * (this->v1 * this->v2 + this->v3 * this->w), 1 - 2 * (xx - zz), 2 * (this->v2 * this->v3 - this->v1 * this->w), 2 * (this->v1 * this->v3 - this->v2 * this->w), 2 * (this->v2 * this->v3 + this->v1 * this->w), 1 - 2 * (xx - yy)}; return rotationMatrix; -}; \ No newline at end of file +}; + +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; +} \ No newline at end of file diff --git a/src/Quaternion.h b/src/Quaternion.h index 0e66a7b..8be1486 100644 --- a/src/Quaternion.h +++ b/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]};