AABB-AABB dynamic-static collision response with sliding

1 files changed, 20 insertions, 7 deletions

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());
 }