Non-camera relative movement

1 files changed, 38 insertions, 0 deletions

diff --git a/src/Math/Matrix.hpp b/src/Math/Matrix.hpp
index 9a6b7e6..43f0721 100644
--- a/src/Math/Matrix.hpp
+++ b/src/Math/Matrix.hpp
@@ -3,6 +3,7 @@
 #include <sstream>
 #include <cstddef>
 #include <iostream>
+#include "Rotation.hpp"
 
 template <size_t R, size_t C, typename T = float>
 struct Matrix {
@@ -28,6 +29,43 @@ public:
         return result;
     }
 
+    static Matrix<4, 4> transformation(Vector<3> position) {
+        return {
+            1.0f, 0.0f, 0.0f, position.x(),
+            0.0f, 1.0f, 0.0f, position.y(),
+            0.0f, 0.0f, 1.0f, position.z(),
+            0.0f, 0.0f, 0.0f, 1.0f
+        };
+    }
+
+    static Matrix<4, 4> rotation(Rotation angles) {
+        auto c = angles.vector.map([](auto a) { return cos(a); });
+        auto s = angles.vector.map([](auto a) { return sin(a); });
+
+        Matrix<4, 4> rotation_x{
+            1.0f, 0.0f,  0.0f,   0.0f,
+            0.0f, c.x(), -s.x(), 0.0f,
+            0.0f, s.x(), c.x(),  0.0f,
+            0.0f, 0.0f,  0.0f,   1.0f,
+        };
+
+        Matrix<4, 4> rotation_y{
+            c.y(),  0.0f, s.y(),  0.0f,
+            0.0f,   1.0f, 0.0f,   0.0f,
+            -s.y(), 0.0f, c.y(),  0.0f,
+            0.0f,   0.0f, 0.0f,   1.0f,
+        };
+
+        Matrix<4, 4> rotation_z{
+            c.z(), -s.z(), 0.0f,  0.0f,
+            s.z(), c.z(),  0.0f,  0.0f,
+            0.0f,  0.0f,   1.0f,  0.0f,
+            0.0f,  0.0f,   0.0f,  1.0f,
+        };
+
+        return rotation_x * rotation_y * rotation_z;
+    }
+
     Vector<C, T> row(size_t index) {
         return { &elements[index * C] };
     }