4 files changed, 29 insertions, 10 deletions
diff --git a/src/Math/AABB.cpp b/src/Math/AABB.cpp
index ea48f71..08c9903 100644
--- a/src/Math/AABB.cpp
+++ b/src/Math/AABB.cpp
@@ -3,10 +3,10 @@
 #include "AABB.hpp"
 #include "Ray.hpp"
 
-// Returns new AABB after colliding with `against`.
+// Returns a pushout vector for avoiding collision with `against`.
 // Algorithm is kind of based on "https://www.gamedev.net/tutorials/_/technical/game-programming/swept-aabb-collision-detection-and-response-r3084/",
 // but very different (and mine's better :3).
-AABB AABB::collision_response(Vector<3> v, AABB against) const {
+Vec3 AABB::pushout(Vector<3> v, AABB against) const {
     auto origin = center();
     Ray ray{origin, v};
 
@@ -14,7 +14,7 @@ AABB AABB::collision_response(Vector<3> v, AABB against) const {
 
     auto expanded_target = against.sum(*this);
     auto raycast = ray.cast(expanded_target, v_magnitude);
-    if (!raycast.hit) return offset(v);
+    if (!raycast.hit) return {};
 
     // Slide along the collision plane.
 
@@ -25,6 +25,6 @@ AABB AABB::collision_response(Vector<3> v, AABB against) const {
     // Project the remaining velocity onto the plane, to which the normal is perpendicular.
     auto projected_velocity = v_remaining - v_remaining.project_onto(raycast.normal);
 
-    auto result = raycast.point + projected_velocity;
-    return from_center(result, size());
+    auto resulting_point = raycast.point + projected_velocity;
+    return resulting_point - (v + origin);
 }
diff --git a/src/Math/AABB.hpp b/src/Math/AABB.hpp
index 53ce6d2..4ad5534 100644
--- a/src/Math/AABB.hpp
+++ b/src/Math/AABB.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <vector>
 #include "Vector.hpp"
 #include "../Common/Lambda.hpp"
 
@@ -41,6 +42,13 @@ struct AABB {
         return intersects_on_x(other) && intersects_on_y(other) && intersects_on_z(other);
     }
 
+    Bool collides(std::vector<AABB> others) {
+        for (auto& other : others) {
+            if (collides(other)) return true;
+        }
+        return false;
+    }
+
     std::array<Vector<3>, 8> corners() const {
         return {{
             {min.x(), min.y(), min.z()},
@@ -68,7 +76,7 @@ struct AABB {
         };
     }
 
-    AABB collision_response(Vector<3> v, AABB against) const;
+    Vec3 pushout(Vector<3> v, AABB against) const;
 
     Vector<3> min, max;
 };
diff --git a/src/Math/Ray.hpp b/src/Math/Ray.hpp
index c2c2274..db37ac2 100644
--- a/src/Math/Ray.hpp
+++ b/src/Math/Ray.hpp
@@ -36,9 +36,12 @@ struct Ray {
         // by the ray at the biggest smaller `t` value.
         // Since a plane always has two normals, we know which one to pick by looking
         // at the direction of the ray.
-        hit.normal = smaller_t_values.zip(direction, [=](Real t, Real d) {
-            return Math::floats_equal(t, biggest_min_t) ? Math::sign(d) : 0;
-        });
+        for (U8 a = 0; a < 3; a++) {
+            if (Math::floats_equal(smaller_t_values[a], biggest_min_t)) {
+                hit.normal[a] = Math::sign(direction[a]);
+                break;
+            }
+        }
 
         // If the smallest `t` is negative, the ray is pointing away from the AABB.
         // If the biggest `t` is smaller than the smallest `t`, the ray is missing the AABB,
diff --git a/src/Math/Vector.hpp b/src/Math/Vector.hpp
index acbfa98..7d07d5c 100644
--- a/src/Math/Vector.hpp
+++ b/src/Math/Vector.hpp
@@ -68,8 +68,12 @@ struct Vector {
         return reduce([](auto x, auto y) { return x + y; });
     }
 
+    T magnitude_squared() const {
+        return map([](auto x) { return x * x;}).sum();
+    }
+
     T magnitude() const {
-        return sqrt(map([](auto x) { return x * x;}).sum());
+        return sqrt(magnitude_squared());
     }
 
     Vector normalize() const {
@@ -81,6 +85,10 @@ struct Vector {
         return (*this - other).magnitude();
     }
 
+    T distance_squared(const Vector other) const {
+        return (*this - other).magnitude_squared();
+    }
+
     Vector<3, T> any_orthogonal() {
         if (Vector a{y(), -x(), 0.0f}; a != zero()) return a;
         if (Vector b{z(), 0.0f, -x()}; b != zero()) return b;