AABB-AABB dynamic-static collision response with sliding

4 files changed, 50 insertions, 13 deletions

diff --git a/src/Entities/Player.cpp b/src/Entities/Player.cpp
index 4fa481e..5d41ad0 100644
--- a/src/Entities/Player.cpp
+++ b/src/Entities/Player.cpp
@@ -19,10 +19,10 @@ void Player::update(const Time& time, GFX::Window& window, GFX::Camera& camera,
     auto destination = m_transform.position() + direction * move_speed;
 
     auto collisions = colliding_terrain(destination, world);
-    if (!collisions.empty()) {
-        for (auto collision : collisions) {
-            destination = collision_reposition(m_transform.position(), destination, collision);
-        }
+    for (auto collision : collisions) {
+        auto original = destination;
+        destination = collision_reposition(m_transform.position(), destination, collision);
+        if (!original.mostly_equal(destination)) break;
     }
 
     move_to(destination);
@@ -97,7 +97,7 @@ Position::World Player::position_for_bounding_box(AABB box) {
 
 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);
+    auto resulting_bounding_box = bounding_box_for_position(from).collision_response(to - from, colliding);
     return position_for_bounding_box(resulting_bounding_box); // Ugly, we convert to AABB and back.
 }
 
diff --git a/src/Math/AABB.cpp b/src/Math/AABB.cpp
index aa814a1..ea48f71 100644
--- a/src/Math/AABB.cpp
+++ b/src/Math/AABB.cpp
@@ -3,15 +3,28 @@
 #include "AABB.hpp"
 #include "Ray.hpp"
 
-AABB AABB::cast_box(Vector<3> v, AABB against) const {
-    auto ray = Ray{center(), v};
+// Returns new AABB after colliding 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 {
+    auto origin = center();
+    Ray ray{origin, v};
+
+    auto v_magnitude = v.magnitude();
 
     auto expanded_target = against.sum(*this);
-    auto raycast = ray.cast(expanded_target);
-    if (!raycast.hit) return *this;
+    auto raycast = ray.cast(expanded_target, v_magnitude);
+    if (!raycast.hit) return offset(v);
+
+    // Slide along the collision plane.
+
+    auto fulfilled_ratio = (raycast.point - origin).magnitude() / v_magnitude;
+    auto remaining_ratio = 1.0 - fulfilled_ratio;
+    auto v_remaining = v * remaining_ratio;
 
-    auto result_from = raycast.point - size() / 2.0;
-    auto result_to = result_from + size();
+    // Project the remaining velocity onto the plane, to which the normal is perpendicular.
+    auto projected_velocity = v_remaining - v_remaining.project_onto(raycast.normal);
 
-    return {result_from, result_to};
+    auto result = raycast.point + projected_velocity;
+    return from_center(result, size());
 }
diff --git a/src/Math/AABB.hpp b/src/Math/AABB.hpp
index 19aa81c..22de760 100644
--- a/src/Math/AABB.hpp
+++ b/src/Math/AABB.hpp
@@ -8,6 +8,13 @@ struct AABB {
     AABB(Vector<3> min, Vector<3> max) : min(min), max(max) {}
     explicit AABB(Vector<3> max) : max(max) {}
 
+    static AABB from_center(Vector<3> center, Vector<3> size) {
+        auto min = center - size / 2.0;
+        auto max = min + size;
+
+        return {min, max};
+    }
+
     Vector<3> size() const { return max - min; }
     Vector<3> center() const { return (min + max) / 2; }
 
@@ -53,7 +60,7 @@ struct AABB {
         return {center() - new_size / 2, center() + new_size / 2};
     }
 
-    AABB cast_box(Vector<3> v, AABB against) const;
+    AABB collision_response(Vector<3> v, AABB against) const;
 
     Vector<3> min, max;
 };
diff --git a/src/Math/Vector.hpp b/src/Math/Vector.hpp
index 4506f0b..acbfa98 100644
--- a/src/Math/Vector.hpp
+++ b/src/Math/Vector.hpp
@@ -100,6 +100,23 @@ struct Vector {
         };
     }
 
+    Vector<3, T> project_onto(const Vector<3, T> other) const {
+        return other * (*this * other) / (other * other);
+    }
+
+    Bool mostly_equal(const Vector other, T epsilon = 0.0001f) const {
+        for (Int i = 0; i < S; i++) {
+            if (!Math::floats_equal(elements[i], other[i], epsilon)) {
+                return false;
+            }
+        }
+        return true;
+    }
+
+    Bool is_zero() const {
+        return mostly_equal(zero());
+    }
+
     T operator[](USize index) const {
         return elements[index];
     }