feat(matrix): add C33Matrix::FromEulerAnglesZYX()

tempest/matrix/C33Matrix.cpp

@@ -12,7 +12,49 @@ float C33Matrix::Det(float a, float b, float c, float d) {
     return (a * d) - (b * c);
 }
 
-C33Matrix C33Matrix::Rotation(float angle) {
+C33Matrix C33Matrix::RotationAroundX(float angle) {
+    float cosa = CMath::cos(angle);
+    float sina = CMath::sin(angle);
+
+    C33Matrix result;
+
+    result.a0 = 1.0f;
+    result.a1 = 0.0f;
+    result.a2 = 0.0f;
+
+    result.b0 = 0.0f;
+    result.b1 = cosa;
+    result.b2 = sina;
+
+    result.c0 = 0.0f;
+    result.c1 = -sina;
+    result.c2 = cosa;
+
+    return result;
+}
+
+C33Matrix C33Matrix::RotationAroundY(float angle) {
+    float cosa = CMath::cos(angle);
+    float sina = CMath::sin(angle);
+
+    C33Matrix result;
+
+    result.a0 = cosa;
+    result.a1 = 0.0f;
+    result.a2 = -sina;
+
+    result.b0 = 0.0f;
+    result.b1 = 1.0f;
+    result.b2 = 0.0f;
+
+    result.c0 = sina;
+    result.c1 = 0.0f;
+    result.c2 = cosa;
+
+    return result;
+}
+
+C33Matrix C33Matrix::RotationAroundZ(float angle) {
     float cosa = CMath::cos(angle);
     float sina = CMath::sin(angle);
 
@@ -20,19 +62,23 @@ C33Matrix C33Matrix::Rotation(float angle) {
 
     result.a0 = cosa;
     result.a1 = sina;
-    result.a2 = 0.0;
+    result.a2 = 0.0f;
 
     result.b0 = -sina;
     result.b1 = cosa;
-    result.b2 = 0.0;
+    result.b2 = 0.0f;
 
-    result.c0 = 0.0;
-    result.c1 = 0.0;
-    result.c2 = 1.0;
+    result.c0 = 0.0f;
+    result.c1 = 0.0f;
+    result.c2 = 1.0f;
 
     return result;
 }
 
+C33Matrix C33Matrix::Rotation(float angle) {
+    return C33Matrix::RotationAroundZ(angle);
+}
+
 C33Matrix C33Matrix::Rotation(float angle, const C3Vector& axis, bool unit) {
     C3Vector axis_ = axis;
     if (!unit) {
@@ -372,6 +418,98 @@ C33Matrix C33Matrix::AffineInverse(float a) const {
     return matrix;
 }
 
+bool C33Matrix::ToEulerAnglesXYZ(float& xa_, float& ya_, float& za_) {
+    // TODO
+    throw;
+    return false;
+}
+
+bool C33Matrix::ToEulerAnglesXZY(float& xa_, float& za_, float& ya_) {
+    // TODO
+    throw;
+    return false;
+}
+
+bool C33Matrix::ToEulerAnglesYXZ(float& ya_, float& xa_, float& za_) {
+    // TODO
+    throw;
+    return false;
+}
+
+bool C33Matrix::ToEulerAnglesYZX(float& ya_, float& za_, float& xa_) {
+    // TODO
+    throw;
+    return false;
+}
+
+bool C33Matrix::ToEulerAnglesZXY(float& za_, float& xa_, float& ya_) {
+    // TODO
+    throw;
+    return false;
+}
+
+bool C33Matrix::ToEulerAnglesZYX(float& za_, float& ya_, float& xa_) {
+    // TODO
+    throw;
+    return false;
+}
+
+void C33Matrix::FromEulerAnglesXYZ(float yaw, float pitch, float roll) {
+    // TODO
+    throw;
+}
+
+void C33Matrix::FromEulerAnglesXZY(float yaw, float pitch, float roll) {
+    // TODO
+    throw;
+}
+
+void C33Matrix::FromEulerAnglesYXZ(float yaw, float pitch, float roll) {
+    // TODO
+    throw;
+}
+
+void C33Matrix::FromEulerAnglesYZX(float yaw, float pitch, float roll) {
+    // TODO
+    throw;
+}
+
+void C33Matrix::FromEulerAnglesZXY(float yaw, float pitch, float roll) {
+    // TODO
+    throw;
+}
+
+void C33Matrix::FromEulerAnglesZYX(float yaw, float pitch, float roll) {
+    float siny = CMath::sin(yaw);
+    float cosy = CMath::cos(yaw);
+
+    float sinp = CMath::sin(pitch);
+    float cosp = CMath::cos(pitch);
+
+    float sinr = CMath::sin(roll);
+    float cosr = CMath::cos(roll);
+
+    C33Matrix x_ = {
+        1.0f, 0.0f, 0.0f,
+        0.0f, cosr, -sinr,
+        0.0f, sinr, cosr
+    };
+
+    C33Matrix y_ = {
+        cosp, 0.0f, sinp,
+        0.0f, 1.0f, 0.0f,
+        -sinp, 0.0f, cosp
+    };
+
+    C33Matrix z_ = {
+        cosy, -siny, 0.0f,
+        siny, cosy, 0.0f,
+        0.0f, 0.0f, 1.0f
+    };
+
+    *this = (z_ * y_ * x_).Transpose();
+}
+
 C33Matrix operator*(const C33Matrix& l, const C33Matrix& r) {
     float a0 = l.a0 * r.a0 + l.a1 * r.b0 + l.a2 * r.c0;
     float a1 = l.a0 * r.a1 + l.a1 * r.b1 + l.a2 * r.c1;

tempest/matrix/C33Matrix.hpp

@@ -16,6 +16,10 @@ class C33Matrix {
 
     // Static functions
     static float Det(float a, float b, float c, float d);
+    static C33Matrix RotationAroundX(float angle);
+    static C33Matrix RotationAroundY(float angle);
+    static C33Matrix RotationAroundZ(float angle);
+
     static C33Matrix Rotation(float angle);
     static C33Matrix Rotation(float angle, const C3Vector& axis, bool unit);
 
@@ -64,6 +68,19 @@ class C33Matrix {
     C33Matrix Inverse(float det) const;
     C33Matrix AffineInverse(const C3Vector& v) const;
     C33Matrix AffineInverse(float a) const;
+
+    bool ToEulerAnglesXYZ(float& xa_, float& ya_, float& za_);
+    bool ToEulerAnglesXZY(float& xa_, float& za_, float& ya_);
+    bool ToEulerAnglesYXZ(float& ya_, float& xa_, float& za_);
+    bool ToEulerAnglesYZX(float& ya_, float& za_, float& xa_);
+    bool ToEulerAnglesZXY(float& za_, float& xa_, float& ya_);
+    bool ToEulerAnglesZYX(float& za_, float& ya_, float& xa_);
+    void FromEulerAnglesXYZ(float yaw, float pitch, float roll);
+    void FromEulerAnglesXZY(float yaw, float pitch, float roll);
+    void FromEulerAnglesYXZ(float yaw, float pitch, float roll);
+    void FromEulerAnglesYZX(float yaw, float pitch, float roll);
+    void FromEulerAnglesZXY(float yaw, float pitch, float roll);
+    void FromEulerAnglesZYX(float yaw, float pitch, float roll);
 };
 
 C33Matrix operator*(const C33Matrix& l, const C33Matrix& r);