Better collision detection

- Players cannot squeeze into each other, but feels better when controlling the player.
- Correct edge collision for counters and players

overcooked_simulator/overcooked_environment.py

@@ -583,6 +583,49 @@ class Environment:
         facing_counter = get_closest(player.facing_point, self.counters)
         return facing_counter
 
+    def get_counter_collisions(self, player_positions):
+        counter_diff_vecs = (
+            player_positions[:, np.newaxis, :]
+            - self.counter_positions[np.newaxis, :, :]
+        )
+        counter_distances = np.max((np.abs(counter_diff_vecs)), axis=2)
+        closest_counter_positions = self.counter_positions[
+            np.argmin(counter_distances, axis=1)
+        ]
+        nearest_counter_to_player = player_positions - closest_counter_positions
+        relevant_axes = np.abs(nearest_counter_to_player).argmax(axis=1)
+
+        distances = np.linalg.norm(
+            np.max(
+                [
+                    np.abs(counter_diff_vecs) - 0.5,
+                    np.zeros(counter_diff_vecs.shape),
+                ],
+                axis=0,
+            ),
+            axis=2,
+        )
+
+        collided = np.any(distances < self.player_radius, axis=1)
+
+        return collided, relevant_axes, nearest_counter_to_player
+
+    def get_player_push(self, player_positions):
+        distances_players_after_scipy = distance_matrix(
+            player_positions, player_positions
+        )
+
+        player_diff_vecs = -(
+            player_positions[:, np.newaxis, :] - player_positions[np.newaxis, :, :]
+        )
+        collisions = distances_players_after_scipy < (2 * self.player_radius)
+        eye_idxs = np.eye(len(player_positions), len(player_positions), dtype=bool)
+        collisions[eye_idxs] = False
+        player_diff_vecs[collisions == False] = 0
+        push_vectors = np.sum(player_diff_vecs, axis=0)
+        collisions = np.any(collisions, axis=1)
+        return collisions, push_vectors
+
     def perform_movement(self, duration: timedelta):
         """Moves a player in the direction specified in the action.action. If the player collides with a
         counter or other player through this movement, then they are not moved.
@@ -607,88 +650,62 @@ class Environment:
             ],
             dtype=float,
         )
-        number_players = len(player_positions)
 
         targeted_positions = player_positions + (
             player_movement_vectors * (self.player_movement_speed * d_time)
         )
 
         # Collisions player between player
-        distances_players_after_scipy = distance_matrix(
-            targeted_positions, targeted_positions
-        )
-
-        player_diff_vecs = -(
-            player_positions[:, np.newaxis, :] - player_positions[np.newaxis, :, :]
-        )
-        collision_idxs = distances_players_after_scipy < (2 * self.player_radius)
-        eye_idxs = np.eye(number_players, number_players, dtype=bool)
-        collision_idxs[eye_idxs] = False
-
-        # Player push players around
-        player_diff_vecs[collision_idxs == False] = 0
-        push_vectors = np.sum(player_diff_vecs, axis=0)
-
-        updated_movement = push_vectors + player_movement_vectors
-        new_positions = player_positions + (
+        force_factor = 1.2
+        _, push_vectors = self.get_player_push(targeted_positions)
+        updated_movement = (force_factor * push_vectors) + player_movement_vectors
+        new_targeted_positions = player_positions + (
             updated_movement * (self.player_movement_speed * d_time)
         )
-
-        # Collisions players counters
-        counter_diff_vecs = (
-            new_positions[:, np.newaxis, :] - self.counter_positions[np.newaxis, :, :]
+        # same again to prevent squeezing into other players
+        _, push_vectors2 = self.get_player_push(new_targeted_positions)
+        updated_movement = (force_factor * push_vectors2) + updated_movement
+        new_targeted_positions = player_positions + (
+            updated_movement * (self.player_movement_speed * d_time)
         )
-        counter_distances = np.max((np.abs(counter_diff_vecs)), axis=2)
-        # counter_distances = np.linalg.norm(counter_diff_vecs, axis=2)
-        closest_counter_positions = self.counter_positions[
-            np.argmin(counter_distances, axis=1)
-        ]
-
-        nearest_counter_to_player = closest_counter_positions - new_positions
 
-        collided = np.min(counter_distances, axis=1) < self.player_radius + 0.5
-        relevant_axes = np.abs(nearest_counter_to_player).argmax(axis=1)
+        # Check collisions with counters
+        (
+            collided,
+            relevant_axes,
+            nearest_counter_to_player,
+        ) = self.get_counter_collisions(new_targeted_positions)
 
+        # Check if sliding against counters is possible
         for idx, player in enumerate(player_positions):
             axis = relevant_axes[idx]
-
             if collided[idx]:
                 # collide with counter left or top
-                if nearest_counter_to_player[idx][axis] < 0:
-                    updated_movement[idx, axis] = max(updated_movement[idx, axis], 0)
-                # collide with counter right or bottom
                 if nearest_counter_to_player[idx][axis] > 0:
-                    updated_movement[idx, axis] = min(updated_movement[idx, axis], 0)
-
+                    updated_movement[idx, axis] = np.max(
+                        [updated_movement[idx, axis], 0]
+                    )
+                # collide with counter right or bottom
+                if nearest_counter_to_player[idx][axis] < 0:
+                    updated_movement[idx, axis] = np.min(
+                        [updated_movement[idx, axis], 0]
+                    )
         new_positions = player_positions + (
             updated_movement * (self.player_movement_speed * d_time)
         )
 
-        # Check if pushed players collide with counters or second closest is to close
-        counter_diff_vecs = (
-            new_positions[:, np.newaxis, :] - self.counter_positions[np.newaxis, :, :]
-        )
-        counter_distances = np.max((np.abs(counter_diff_vecs)), axis=2)
-        collided2 = np.min(counter_distances, axis=1) < self.player_radius + 0.5
-        # player do not move if they collide after pushing/sliding
-        new_positions[collided2] = player_positions[collided2]
-        # Players that pushed the player that can not be pushed do also no movement
-        # in the future these players could slide around the player?
-        for idx, collides in enumerate(collided2):
-            if collides:
-                new_positions[collision_idxs[idx]] = player_positions[
-                    collision_idxs[idx]
-                ]
+        # Check collisions with counters again, now absolute with no sliding possible
+        (
+            collided,
+            relevant_axes,
+            nearest_counter_to_player,
+        ) = self.get_counter_collisions(new_positions)
+        new_positions[collided] = player_positions[collided]
 
-        # Check if two moving players collide into each other: No movement (Future: slide?)
-        if PREVENT_SQUEEZING_INTO_OTHER_PLAYERS:
-            distances_players_after_scipy = distance_matrix(
-                new_positions, new_positions
-            )
-            collision_idxs = distances_players_after_scipy < (2 * self.player_radius)
-            collision_idxs[eye_idxs] = False
-            collision_idxs = np.any(collision_idxs, axis=1)
-            new_positions[collision_idxs] = player_positions[collision_idxs]
+        # Check player collisions a final time
+        # collided, _ = self.get_player_push(new_positions)
+        # if np.any(collided):
+        #     print(".", end="")
 
         # Collisions player world borders
         new_positions = np.clip(
@@ -698,9 +715,8 @@ class Environment:
         )
 
         for idx, p in enumerate(self.players.values()):
-            if not (new_positions[idx] == player_positions[idx]).all():
-                p.pos = new_positions[idx]
-                p.perform_interact_stop()
+            # if not (new_positions[idx] == player_positions[idx]).all():
+            p.pos = new_positions[idx]
 
             p.turn(player_movement_vectors[idx])
 
@@ -832,6 +848,9 @@ class Environment:
                 }
                 if self.player_view_restricted
                 else None,
+                "served_meals": [
+                    ("?", str(meal)) for (meal, time) in self.order_manager.served_meals
+                ],
                 "info_msg": [
                     (msg["msg"], msg["level"])
                     for msg in self.info_msgs_per_player[player_id]