diff --git a/tempest/math/CMath.hpp b/tempest/math/CMath.hpp index 10ac6dd..fc236ea 100644 --- a/tempest/math/CMath.hpp +++ b/tempest/math/CMath.hpp @@ -61,6 +61,23 @@ class CMath { STORM_ASSERT(x >= 0.0f); return ::sqrt(x); } + + static void normalize(float& x, float& y) { + float m = x * x + y * y; + STORM_ASSERT(m >= 0.0f); + m = 1.0f / CMath::sqrt(m); + x *= m; + y *= m; + } + + static void normalize(float& x, float& y, float& z) { + float m = x * x + y * y + z * z; + STORM_ASSERT(m >= 0.0f); + m = 1.0f / CMath::sqrt(m); + x *= m; + y *= m; + z *= m; + } }; #endif diff --git a/tempest/matrix/C33Matrix.cpp b/tempest/matrix/C33Matrix.cpp index 4513a56..407d997 100644 --- a/tempest/matrix/C33Matrix.cpp +++ b/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; diff --git a/tempest/matrix/C33Matrix.hpp b/tempest/matrix/C33Matrix.hpp index f706b60..69c56ac 100644 --- a/tempest/matrix/C33Matrix.hpp +++ b/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);