Correctly sort collision responses and apply in correct order

1 files changed, 34 insertions, 12 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.