6 files changed, 120 insertions, 3 deletions
diff --git a/src/Common/Lambda.hpp b/src/Common/Lambda.hpp
new file mode 100644
index 0000000..8bcfd89
--- /dev/null
+++ b/src/Common/Lambda.hpp
@@ -0,0 +1,14 @@
+#pragma once
+
+#define _LAMBDA_ARG1 auto a
+#define _LAMBDA_ARG2 auto a, auto b
+#define _LAMBDA_ARG3 auto a, auto b, auto c
+
+#define _LAMBDA_PARAM1 a
+#define _LAMBDA_PARAM2 a, b
+#define _LAMBDA_PARAM3 a, b, c
+
+// A macro to create a lambda function around a (usually std) function,
+// where the template arguments aren't properly deduced.
+// Up to 3 arguments are supported.
+#define LAMBDA(f, n) [](_LAMBDA_ARG##n) { return f(_LAMBDA_PARAM##n); }
diff --git a/src/Entities/Player.cpp b/src/Entities/Player.cpp
index 6c94490..4fa481e 100644
--- a/src/Entities/Player.cpp
+++ b/src/Entities/Player.cpp
@@ -16,10 +16,16 @@ void Player::update(const Time& time, GFX::Window& window, GFX::Camera& camera,
     auto rotation_speed = 0.1f;
 
     auto direction = m_transform.right() * x + Vec3(0, y, 0) + m_transform.forward() * z;
-    auto position = m_transform.position() + direction * move_speed;
+    auto destination = m_transform.position() + direction * move_speed;
 
-    auto collisions = colliding_terrain(position, world);
-    if (collisions.empty()) move_to(position);
+    auto collisions = colliding_terrain(destination, world);
+    if (!collisions.empty()) {
+        for (auto collision : collisions) {
+            destination = collision_reposition(m_transform.position(), destination, collision);
+        }
+    }
+
+    move_to(destination);
     rotate({r.y() * rotation_speed, r.x() * rotation_speed, 0.0f});
 
     update_camera_position(camera);
@@ -84,4 +90,15 @@ AABB Player::bounding_box_for_position(Position::World position) {
     return s_bounds.offset(box_start);
 }
 
+Position::World Player::position_for_bounding_box(AABB box) {
+    auto center = box.center();
+    return {center.x(), box.min.y(), center.z()};
+}
+
+Position::World Player::collision_reposition(Position::World from, Position::World to, AABB colliding) {
+    if (from == to) return from;
+    auto resulting_bounding_box = bounding_box_for_position(from).cast_box(to - from, colliding);
+    return position_for_bounding_box(resulting_bounding_box); // Ugly, we convert to AABB and back.
+}
+
 }
diff --git a/src/Entities/Player.hpp b/src/Entities/Player.hpp
index 0422509..7238e80 100644
--- a/src/Entities/Player.hpp
+++ b/src/Entities/Player.hpp
@@ -33,6 +33,10 @@ private:
 
     // Creates a bounding box where `position` is at the center of the bottom face.
     static AABB bounding_box_for_position(Position::World position);
+    // Returns position of the center of the bottom face of `box`.
+    static Position::World position_for_bounding_box(AABB box);
+
+    static Position::World collision_reposition(Position::World from, Position::World to, AABB colliding);
 
     Transform m_transform;
 
diff --git a/src/Math/AABB.cpp b/src/Math/AABB.cpp
new file mode 100644
index 0000000..aa814a1
--- /dev/null
+++ b/src/Math/AABB.cpp
@@ -0,0 +1,17 @@
+// This file breaks the Ray.hpp <-> AABB.hpp dependency cycle.
+
+#include "AABB.hpp"
+#include "Ray.hpp"
+
+AABB AABB::cast_box(Vector<3> v, AABB against) const {
+    auto ray = Ray{center(), v};
+
+    auto expanded_target = against.sum(*this);
+    auto raycast = ray.cast(expanded_target);
+    if (!raycast.hit) return *this;
+
+    auto result_from = raycast.point - size() / 2.0;
+    auto result_to = result_from + size();
+
+    return {result_from, result_to};
+}
diff --git a/src/Math/AABB.hpp b/src/Math/AABB.hpp
index 11d26a8..19aa81c 100644
--- a/src/Math/AABB.hpp
+++ b/src/Math/AABB.hpp
@@ -8,6 +8,31 @@ struct AABB {
     AABB(Vector<3> min, Vector<3> max) : min(min), max(max) {}
     explicit AABB(Vector<3> max) : max(max) {}
 
+    Vector<3> size() const { return max - min; }
+    Vector<3> center() const { return (min + max) / 2; }
+
+    Bool contains(Vector<3> point) const {
+        return point.x() >= min.x() && point.x() <= max.x()
+            && point.y() >= min.y() && point.y() <= max.y()
+            && point.z() >= min.z() && point.z() <= max.z();
+    }
+
+    Bool intersects_on_x(AABB other) const {
+        return min.x() <= other.max.x() && max.x() >= other.min.x();
+    }
+
+    Bool intersects_on_y(AABB other) const {
+        return min.y() <= other.max.y() && max.y() >= other.min.y();
+    }
+
+    Bool intersects_on_z(AABB other) const {
+        return min.z() <= other.max.z() && max.z() >= other.min.z();
+    }
+
+    Bool collides(AABB other) const {
+        return intersects_on_x(other) && intersects_on_y(other) && intersects_on_z(other);
+    }
+
     std::array<Vector<3>, 8> corners() const {
         return {{
             {min.x(), min.y(), min.z()},
@@ -23,5 +48,12 @@ struct AABB {
 
     AABB offset(Vector<3> by) const { return {min + by, max + by}; }
 
+    AABB sum(AABB with) const {
+        auto new_size = size() + with.size();
+        return {center() - new_size / 2, center() + new_size / 2};
+    }
+
+    AABB cast_box(Vector<3> v, AABB against) const;
+
     Vector<3> min, max;
 };
diff --git a/src/Math/Ray.hpp b/src/Math/Ray.hpp
new file mode 100644
index 0000000..f83a2b5
--- /dev/null
+++ b/src/Math/Ray.hpp
@@ -0,0 +1,33 @@
+#pragma once
+
+#include "../Common/Lambda.hpp"
+#include "AABB.hpp"
+#include "Vector.hpp"
+
+struct Ray {
+    struct RaycastResult {
+        Bool hit = false;
+        Vec3 point;
+        Vec3 normal;
+    };
+
+    // https://gdbooks.gitbooks.io/3dcollisions/content/Chapter3/raycast_aabb.html
+    RaycastResult cast(AABB box) const {
+        Vec3 t_from = (box.min - origin) / direction;
+        Vec3 t_to = (box.max - origin) / direction;
+
+        Real t_min = t_from.zip(t_to, LAMBDA(std::min, 2)).reduce(LAMBDA(std::max, 2));
+        Real t_max = t_from.zip(t_to, LAMBDA(std::max, 2)).reduce(LAMBDA(std::min, 2));
+
+        if (t_max < 0 || t_min > t_max) return {};
+
+        return {
+            .hit = true,
+            .point = origin + direction * t_min,
+            .normal = t_from.zip(t_to, [](auto a, auto b) { return a < b ? -1.0 : 1.0; })
+        };
+    }
+
+    Vec3 origin;
+    Vec3 direction;
+};