Add logic to prevent slanted walls from lifting the player up

common/src/character_controller/mod.rs

@@ -1,6 +1,8 @@
 mod collision;
 mod vector_bounds;
 
+use std::mem::replace;
+
 use tracing::warn;
 
 use crate::{
@@ -144,6 +146,7 @@ fn get_ground_normal(
             }
             bounds.apply_and_add_bound(
                 VectorBound::new_push(collision.normal, collision.normal),
+                &[],
                 &mut allowed_displacement,
                 None,
             );
@@ -219,11 +222,14 @@ fn apply_velocity(
 
     let mut remaining_dt_seconds = ctx.dt_seconds;
 
-    let mut velocity_info = VelocityInfo {
+    let initial_velocity_info = VelocityInfo {
         bounds: VectorBoundGroup::new(&average_velocity),
         average_velocity,
         final_velocity: *velocity,
     };
+    let mut velocity_info = initial_velocity_info.clone();
+
+    let mut ground_collision_handled = false;
 
     let mut all_collisions_resolved = false;
     for _ in 0..MAX_COLLISION_ITERATIONS {
@@ -239,7 +245,14 @@ fn apply_velocity(
                 - collision_result.displacement_vector.magnitude()
                     / expected_displacement.magnitude();
 
-            handle_collision(ctx, collision, &mut velocity_info, ground_normal);
+            handle_collision(
+                ctx,
+                collision,
+                &initial_velocity_info,
+                &mut velocity_info,
+                ground_normal,
+                &mut ground_collision_handled,
+            );
         } else {
             all_collisions_resolved = true;
             break;
@@ -257,23 +270,62 @@ fn apply_velocity(
 fn handle_collision(
     ctx: &CharacterControllerContext,
     collision: Collision,
+    initial_velocity_info: &VelocityInfo,
     velocity_info: &mut VelocityInfo,
     ground_normal: &mut Option<na::UnitVector3<f32>>,
+    ground_collision_handled: &mut bool,
 ) {
     // Collisions are divided into two categories: Ground collisions and wall collisions.
     // Ground collisions will only affect vertical movement of the character, while wall collisions will
-    // push the character away from the wall in a perpendicular direction.
+    // push the character away from the wall in a perpendicular direction. If the character is on the ground,
+    // we have extra logic to ensure that slanted wall collisions do not lift the character off the ground.
     if is_ground(ctx, &collision.normal) {
-        velocity_info.bounds.apply_and_add_bound(
-            VectorBound::new_push(collision.normal, ctx.up),
-            &mut velocity_info.average_velocity,
-            Some(&mut velocity_info.final_velocity),
-        );
+        let stay_on_ground_bounds = [VectorBound::new_pull(collision.normal, ctx.up)];
+        if !*ground_collision_handled {
+            // Wall collisions can turn vertical momentum into unwanted horizontal momentum. This can
+            // occur if the character jumps at the corner between the ground and a slanted wall. If the wall
+            // collision is handled first, this horizontal momentum will push the character away from the wall.
+            // This can also occur if the character is on the ground and walks into a slanted wall. A single frame
+            // of downward momentum caused by gravity can turn into unwanted horizontal momentum that pushes
+            // the character away from the wall. Neither of these issues can occur if the ground collision is
+            // handled first, so when computing how the velocity vectors change, we rewrite history as if
+            // the ground collision was first. This is only necessary for the first ground collision, since
+            // afterwards, there is no more unexpected vertical momentum.
+            let old_velocity_info = replace(velocity_info, initial_velocity_info.clone());
+            velocity_info.bounds.apply_and_add_bound(
+                VectorBound::new_push(collision.normal, ctx.up),
+                &stay_on_ground_bounds,
+                &mut velocity_info.average_velocity,
+                Some(&mut velocity_info.final_velocity),
+            );
+            for bound in old_velocity_info.bounds.bounds() {
+                velocity_info.bounds.apply_and_add_bound(
+                    bound.clone(),
+                    &stay_on_ground_bounds,
+                    &mut velocity_info.average_velocity,
+                    Some(&mut velocity_info.final_velocity),
+                );
+            }
+
+            *ground_collision_handled = true;
+        } else {
+            velocity_info.bounds.apply_and_add_bound(
+                VectorBound::new_push(collision.normal, ctx.up),
+                &stay_on_ground_bounds,
+                &mut velocity_info.average_velocity,
+                Some(&mut velocity_info.final_velocity),
+            );
+        }
 
         *ground_normal = Some(collision.normal);
     } else {
+        let mut stay_on_ground_bounds = Vec::new();
+        if let Some(ground_normal) = ground_normal {
+            stay_on_ground_bounds.push(VectorBound::new_pull(*ground_normal, ctx.up));
+        }
         velocity_info.bounds.apply_and_add_bound(
             VectorBound::new_push(collision.normal, collision.normal),
+            &stay_on_ground_bounds,
             &mut velocity_info.average_velocity,
             Some(&mut velocity_info.final_velocity),
         );

common/src/character_controller/vector_bounds.rs

@@ -27,23 +27,30 @@ impl VectorBoundGroup {
         }
     }
 
-    /// Constrains `vector` with `new_bound` while keeping the existing constraints satisfied.
-    /// All projection transformations applied to `vector` are also applied
+    /// Returns the internal list of `VectorBound`s contained in the `VectorBoundGroup` struct.
+    pub fn bounds(&self) -> &[VectorBound] {
+        &self.bounds
+    }
+
+    /// Constrains `vector` with `new_bound` while keeping the existing constraints and any constraints in
+    /// `temporary_bounds` satisfied. All projection transformations applied to `vector` are also applied
     /// to `tagalong` to allow two vectors to be transformed consistently with each other.
     pub fn apply_and_add_bound(
         &mut self,
         new_bound: VectorBound,
+        temporary_bounds: &[VectorBound],
         vector: &mut na::Vector3<f32>,
         tagalong: Option<&mut na::Vector3<f32>>,
     ) {
-        self.apply_bound(&new_bound, vector, tagalong);
+        self.apply_bound(&new_bound, temporary_bounds, vector, tagalong);
         self.bounds.push(new_bound);
     }
 
     /// Helper function to logically separate the "add" and the "apply" in `apply_and_add_bound` function.
     fn apply_bound(
         &self,
         new_bound: &VectorBound,
+        temporary_bounds: &[VectorBound],
         vector: &mut na::Vector3<f32>,
         mut tagalong: Option<&mut na::Vector3<f32>>,
     ) {
@@ -55,6 +62,9 @@ impl VectorBoundGroup {
         // bound that allows all bounds to be satisfied, and (3) zero out the vector if no such pairing works, as we
         // assume that we need to apply three linearly independent bounds.
 
+        // Combine existing bounds with temporary bounds into an iterator
+        let bounds_iter = self.bounds.iter().chain(temporary_bounds.iter());
+
         // Apply new_bound if necessary.
         if !new_bound.check_vector(vector, self.error_margin) {
             new_bound.constrain_vector(vector, self.error_margin);
@@ -65,13 +75,13 @@ impl VectorBoundGroup {
         }
 
         // Check if all constraints are satisfied
-        if (self.bounds.iter()).all(|b| b.check_vector(vector, self.error_margin)) {
+        if (bounds_iter.clone()).all(|b| b.check_vector(vector, self.error_margin)) {
             return;
         }
 
         // If not all constraints are satisfied, find the first constraint that if applied will satisfy
         // the remaining constriants
-        for bound in (self.bounds.iter()).filter(|b| !b.check_vector(vector, self.error_margin)) {
+        for bound in (bounds_iter.clone()).filter(|b| !b.check_vector(vector, self.error_margin)) {
             let Some(ortho_bound) = bound.get_self_constrained_with_bound(new_bound)
             else {
                 warn!("Unsatisfied existing bound is parallel to new bound. Is the character squeezed between two walls?");
@@ -81,7 +91,7 @@ impl VectorBoundGroup {
             let mut candidate = *vector;
             ortho_bound.constrain_vector(&mut candidate, self.error_margin);
 
-            if (self.bounds.iter()).all(|b| b.check_vector(&candidate, self.error_margin)) {
+            if (bounds_iter.clone()).all(|b| b.check_vector(&candidate, self.error_margin)) {
                 *vector = candidate;
                 if let Some(ref mut tagalong) = tagalong {
                     ortho_bound.constrain_vector(tagalong, 0.0);
@@ -124,6 +134,22 @@ impl VectorBound {
         }
     }
 
+    /// Creates a `VectorBound` that pulls vectors towards the plane given
+    /// by the normal in `projection_direction`. Even after applying such a bound to
+    /// a vector, its dot product with `normal` should still be positive even counting
+    /// floating point approximation limitations. This ensures that `new_push` and
+    /// `new_pull` don't conflict with each other even with equal parameters.
+    pub fn new_pull(
+        normal: na::UnitVector3<f32>,
+        projection_direction: na::UnitVector3<f32>,
+    ) -> Self {
+        VectorBound {
+            normal: na::UnitVector3::new_unchecked(-normal.as_ref()),
+            projection_direction,
+            error_margin_factor: -1.0,
+        }
+    }
+
     /// Updates `subject` with a projection transformation based on the constraint given by `self`.
     /// This function does not check whether such a constraint is needed.
     fn constrain_vector(&self, subject: &mut na::Vector3<f32>, error_margin: f32) {
@@ -183,6 +209,7 @@ mod tests {
         // Add a bunch of bounds that are achievable with nonzero vectors
         bounds.apply_and_add_bound(
             VectorBound::new_push(unit_vector(1.0, 3.0, 4.0), unit_vector(1.0, 2.0, 2.0)),
+            &[],
             &mut constrained_vector,
             None,
         );
@@ -192,6 +219,7 @@ mod tests {
 
         bounds.apply_and_add_bound(
             VectorBound::new_push(unit_vector(2.0, -3.0, -4.0), unit_vector(1.0, -2.0, -1.0)),
+            &[],
             &mut constrained_vector,
             None,
         );
@@ -201,6 +229,7 @@ mod tests {
 
         bounds.apply_and_add_bound(
             VectorBound::new_push(unit_vector(2.0, -3.0, -5.0), unit_vector(1.0, -2.0, -2.0)),
+            &[],
             &mut constrained_vector,
             None,
         );
@@ -211,6 +240,7 @@ mod tests {
         // Finally, add a bound that overconstrains the system
         bounds.apply_and_add_bound(
             VectorBound::new_push(unit_vector(-3.0, 3.0, -2.0), unit_vector(-3.0, 3.0, -2.0)),
+            &[],
             &mut constrained_vector,
             None,
         );
@@ -278,7 +308,7 @@ mod tests {
     }
 
     fn assert_bounds_achieved(bounds: &VectorBoundGroup, subject: &na::Vector3<f32>) {
-        for bound in &bounds.bounds {
+        for bound in bounds.bounds() {
             assert!(bound.check_vector(subject, bounds.error_margin));
         }
     }