3 files changed, 51 insertions, 18 deletions

diff --git a/src/Entities/Player.cpp b/src/Entities/Player.cpp
index 7a415ec..f87f2f9 100644
--- a/src/Entities/Player.cpp
+++ b/src/Entities/Player.cpp
@@ -53,33 +53,55 @@ AABB Player::bounds() const {
 #define STUCK_THRESHOLD 100
 
 Position::World Player::process_collisions(World::World& world, Position::World from, Position::World to) {
+    if (from.mostly_equal(to)) return to;
+
     auto current_box = bounding_box_for_position(from);
 
     // All the blocks we could theoretically collide with.
+    // NOTE: It isn't updated as new responses are applied,
+    // as that would be (currently) too expensive.
+    // At very high speeds, this may lead to phasing through blocks.
     auto collision_domain = terrain_collision_domain(from, to, world);
 
-    // Sort the pushouts by magnitude, and apply the biggest one.
-    // If we apply a pushout, we need to check again for collisions,
+    // Sort the responses first by the magnitude of the velocity until the collision,
+    // and then by the distance from the entity, so that we slide along the closest block.
+    // If we apply a response, we need to check again for collisions,
     // since we might have slid into another block. (up to STUCK_THRESHOLD times)
-    // If there are no pushouts, we have no collisions,
+    // If there are no more responses, we have no collisions,
     // and the player can move freely to the proposed position.
 
-    std::vector<Vec3> pushouts;
+    struct Response {
+        AABB::CollisionResponse response;
+        Real distance_from_entity_squared;
+        Real magnitude_squared;
+    };
+    std::vector<Response> responses;
     for (UInt stuck = 0; stuck < STUCK_THRESHOLD; stuck++) {
-        // Find the pushout vector for each block we're colliding with.
+        auto v = to - from;
+
         for (auto possible_collision : collision_domain) {
-            auto pushout = current_box.pushout(to - from, possible_collision);
-            if (!pushout.is_zero()) pushouts.push_back(pushout);
+            auto response = current_box.collision_response(v, possible_collision);
+            auto total_velocity = response.v_to_collision + response.v_slide;
+            if (!total_velocity.mostly_equal(v)) {
+                responses.push_back({
+                    response,
+                    possible_collision.center().distance_squared(from),
+                    response.v_to_collision.magnitude_squared(),
+                });
+            }
         }
 
-        if (pushouts.empty()) return to;
+        if (responses.empty()) return to;
 
-        std::sort(pushouts.begin(), pushouts.end(), [=](const Vec3& a, const Vec3& b) -> bool {
-            return a.magnitude_squared() > b.magnitude_squared();
+        std::sort(responses.begin(), responses.end(), [=](const Response& a, const Response& b) -> bool {
+            return std::tie(a.magnitude_squared, a.distance_from_entity_squared) < std::tie(b.magnitude_squared, b.distance_from_entity_squared);
         });
 
-        to += pushouts[0];
-        pushouts.clear();
+        // TODO: This applies the entire response, even though ideally we'd apply it in two parts,
+        // since technically the total velocity is a diagonal of the two components.
+        // This should only be a marginal issue, though.
+        to = from + responses[0].response.v_to_collision + responses[0].response.v_slide;
+        responses.clear();
     }
 
     // We got stuck, don't move.
diff --git a/src/Math/AABB.cpp b/src/Math/AABB.cpp
index 08c9903..b16960e 100644
--- a/src/Math/AABB.cpp
+++ b/src/Math/AABB.cpp
@@ -3,10 +3,11 @@
 #include "AABB.hpp"
 #include "Ray.hpp"
 
-// Returns a pushout vector for avoiding collision with `against`.
+// Returns a velocity vectors that avoid collision with `against`, while sliding along it.
+// If no collision is detected, returns the original velocity.
 // 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).
-Vec3 AABB::pushout(Vector<3> v, AABB against) const {
+AABB::CollisionResponse AABB::collision_response(Vector<3> v, AABB against) const {
     auto origin = center();
     Ray ray{origin, v};
 
@@ -14,7 +15,7 @@ Vec3 AABB::pushout(Vector<3> v, AABB against) const {
 
     auto expanded_target = against.sum(*this);
     auto raycast = ray.cast(expanded_target, v_magnitude);
-    if (!raycast.hit) return {};
+    if (!raycast.hit) return {v};
 
     // Slide along the collision plane.
 
@@ -25,6 +26,8 @@ Vec3 AABB::pushout(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 resulting_point = raycast.point + projected_velocity;
-    return resulting_point - (v + origin);
+    return {
+        .v_to_collision = raycast.point - origin,
+        .v_slide = projected_velocity,
+    };
 }
diff --git a/src/Math/AABB.hpp b/src/Math/AABB.hpp
index 4ad5534..b50ba56 100644
--- a/src/Math/AABB.hpp
+++ b/src/Math/AABB.hpp
@@ -76,7 +76,15 @@ struct AABB {
         };
     }
 
-    Vec3 pushout(Vector<3> v, AABB against) const;
+    struct CollisionResponse {
+        // Velocity until the collision.
+        // Always has the same direction as the original velocity, and lower magnitude.
+        Vec3 v_to_collision;
+        // Velocity sliding along the collision plane.
+        // Projection of the fraction of the original velocity that was blocked by collision, onto the collision plane.
+        Vec3 v_slide;
+    };
+    CollisionResponse collision_response(Vector<3> v, AABB against) const;
 
     Vector<3> min, max;
 };