feat(matrix): add C33Matrix class

tempest/matrix/C33Matrix.cpp

@@ -0,0 +1,61 @@
+#include "tempest/matrix/C33Matrix.hpp"
+#include "tempest/math/CMath.hpp"
+
+#include <storm/Error.hpp>
+
+
+float C33Matrix::Det(float a, float b, float c, float d) {
+    return (a * d) - (b * c);
+}
+
+C33Matrix C33Matrix::Rotation(float angle) {
+    float cosa = cos(angle);
+    float sina = sin(angle);
+
+    C33Matrix result;
+    result.a0 = cosa;
+    result.a1 = sina;
+    result.a2 = 0.0;
+
+    result.b0 = -sina;
+    result.b1 = cosa;
+    result.b2 = 0.0;
+
+    result.c0 = 0.0;
+    result.c1 = 0.0;
+    result.c2 = 1.0;
+    return result;
+}
+
+C33Matrix C33Matrix::Rotation(float angle, const C3Vector& axis, bool unit) {
+    C3Vector axis_ = axis;
+    if (!unit) {
+        axis_.Normalize();
+    }
+    STORM_ASSERT(axis_.IsUnit());
+
+    float sina = CMath::sin(angle);
+    float cosa = CMath::cos(angle);
+
+    float xs = axis_.x * sina;
+    float ys = axis_.y * sina;
+    float zs = axis_.z * sina;
+
+    float one_c = 1.0f - cosa;
+
+    C33Matrix result;
+
+    result.a0 = axis_.x * axis_.x * one_c + cosa;
+    result.a1 = one_c * axis_.y * axis_.x + zs;
+    result.a2 = one_c * axis_.z * axis_.x - ys;
+
+    result.b0 = one_c * axis_.y * axis_.x - zs;
+    result.b1 = axis_.y * axis_.y * one_c + cosa;
+    result.b2 = one_c * axis_.z * axis_.y + xs;
+
+    result.c0 = one_c * axis_.z * axis_.x + ys;
+    result.c1 = one_c * axis_.z * axis_.y - xs;
+    result.c2 = axis_.z * axis_.z * one_c + cosa;
+
+    return result;
+}