1 files changed, 57 insertions, 23 deletions

diff --git a/src/Math/Vector.hpp b/src/Math/Vector.hpp
index 2e97217..b6690ef 100644
--- a/src/Math/Vector.hpp
+++ b/src/Math/Vector.hpp
@@ -4,6 +4,7 @@
 #include <sstream>
 #include <iostream>
 #include <iterator>
+#include <cmath>
 
 template <size_t S, typename T = float>
 struct Vector {
@@ -22,62 +23,95 @@ public:
         elements[S - 1] = scalar;
     }
 
-    Vector<S, T> apply(T f(T)) {
-        return apply(static_cast<std::function<T(T)>>(f));
+    Vector<S, T> map(std::function<T(T)> f) const {
+        Vector<S, T> result{};
+        for (int i = 0; i < S; i++) {
+            result[i] = f(elements[i]);
+        }
+        return result;
     }
 
-    Vector<S, T> apply(std::function<T(T)> f) {
+    Vector<S, T> map(std::function<T(size_t, T)> f) const {
         Vector<S, T> result{};
         for (int i = 0; i < S; i++) {
-            result[i] = f(elements[i]);
+            result[i] = f(i, elements[i]);
         }
         return result;
     }
 
+    T reduce(std::function<T(T, T)> f) const {
+        T result = elements[0];
+        for (int i = 1; i < S; i++) {
+            result = f(result, elements[i]);
+        }
+        return result;
+    }
+
+    T sum() const {
+        return reduce([](auto x, auto y) { return x + y; });
+    }
+
+    T magnitude() const {
+        return sqrt(map([](auto x) { return x * x;}).sum());
+    }
+
+    Vector<S, T> normalize() const {
+        auto m = magnitude();
+        return map([=](auto x) { return x / m; });
+    }
+
+    Vector<3, T> cross(Vector<3, T> other) const {
+        return {
+            y() * other.z() - z() * other.y(),
+            z() * other.x() - x() * other.z(),
+            x() * other.y() - y() * other.x(),
+        };
+    }
+
+    T operator[](size_t index) const {
+        return elements[index];
+    }
+
     T& operator[](size_t index) {
         return elements[index];
     }
 
     Vector<S, T> operator+(Vector<S, T> other) const {
-        Vector<S, T> result{};
-        for (int i = 0; i < S; i++) {
-            result[i] = elements[i] + other[i];
-        }
-        return result;
+        return map([&](auto i, auto x) { return x + other[i]; });
     }
 
     Vector<S, T> operator*(T scalar) const {
-        Vector<S, T> result;
-        for (size_t index; index < S; index++) {
-            result = this[index] * scalar;
-        }
-        return result;
+        return map([=](auto x) { return x * scalar; });
     }
 
     T operator*(Vector<S, T> other) const {
-        T result = 0;
-        for (size_t index = 0; index < S; index++) {
-            result += this->elements[index] * other[index];
-        }
-        return result;
+        return map([&](auto i, auto x) { return x * other[i]; }).sum();
+    }
+
+    Vector<S, T> operator-(Vector<S, T> other) const {
+        return map([&](auto i, auto x) { return x - other[i]; });
+    }
+
+    Vector<S, T> operator-() const {
+        return map([](T x) -> T { return -x; });
     }
 
-    T x() {
+    T x() const {
         static_assert(S > 0);
         return elements[0];
     }
 
-    T y() {
+    T y() const {
         static_assert(S > 1);
         return elements[1];
     }
 
-    T z() {
+    T z() const {
         static_assert(S > 2);
         return elements[2];
     }
 
-    T w() {
+    T w() const {
         static_assert(S > 3);
         return elements[3];
     }