From b29e3a4182c0164998c9664e5331bc9405becee5 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Florian=20Schr=C3=B6der?=
 <fschroeder@techfak.uni-bielefeld.de>
Date: Mon, 5 Feb 2024 18:30:29 +0100
Subject: [PATCH] Improve collision detection and player control handling

This update refines the collision detection mechanism in the 'overcooked_environment' module. The changes ensure that players can't move into positions where they would collide with other elements. Player control handling in the 'overcooked_gui' module is also tweaked to provide smoother switching between players. Remaining modifications in 'arch_config.yml' deactivate the GUI in status manager and change the preference for IPAACARInfo under communication.
---
 .../game_content/agents/arch_config.yml       | 13 ++--
 .../gui_2d_vis/overcooked_gui.py              | 34 +++++++----
 .../overcooked_environment.py                 | 61 ++++++++-----------
 3 files changed, 56 insertions(+), 52 deletions(-)

diff --git a/overcooked_simulator/game_content/agents/arch_config.yml b/overcooked_simulator/game_content/agents/arch_config.yml
index 60d20c0d..e7108df0 100644
--- a/overcooked_simulator/game_content/agents/arch_config.yml
+++ b/overcooked_simulator/game_content/agents/arch_config.yml
@@ -2,7 +2,7 @@ concurrency: MultiProcessing
 
 communication:
   communication_prefs:
-   - !name:ipaacar_com_service.communications.ipaacar_com.IPAACARInfo
+    - !name:ipaacar_com_service.communications.ipaacar_com.IPAACARInfo
 
 modules:
   connection:
@@ -15,9 +15,10 @@ modules:
   action_execution:
     module_info: !name:cocosy_agent.modules.action_execution_module.ActionExecutionModule
     mean_frequency_step: 10  # 2: every 0.5 seconds
-  gui:
-    module_info: !name:aaambos.std.guis.pysimplegui.pysimplegui_window.PySimpleGUIWindowModule
-    window_title: Counting GUI
-    topics_to_show: [["SubtaskDecision", "cocosy_agent.conventions.communication.SubtaskDecision", ["task_type"]], ["ActionControl", "cocosy_agent.conventions.communication.ActionControl", ["action_type"]]]
+  #  gui:
+  #    module_info: !name:aaambos.std.guis.pysimplegui.pysimplegui_window.PySimpleGUIWindowModule
+  #    window_title: Counting GUI
+  #    topics_to_show: [["SubtaskDecision", "cocosy_agent.conventions.communication.SubtaskDecision", ["task_type"]], ["ActionControl", "cocosy_agent.conventions.communication.ActionControl", ["action_type"]]]
   status_manager:
-    module_info: !name:aaambos.std.modules.module_status_manager.ModuleStatusManager
\ No newline at end of file
+    module_info: !name:aaambos.std.modules.module_status_manager.ModuleStatusManager
+    gui: false
\ No newline at end of file
diff --git a/overcooked_simulator/gui_2d_vis/overcooked_gui.py b/overcooked_simulator/gui_2d_vis/overcooked_gui.py
index b5db493c..c8b65fe5 100644
--- a/overcooked_simulator/gui_2d_vis/overcooked_gui.py
+++ b/overcooked_simulator/gui_2d_vis/overcooked_gui.py
@@ -73,17 +73,23 @@ class PlayerKeySet:
         self.pickup_key: pygame.key = pickup_key
         self.switch_key: pygame.key = switch_key
         self.controlled_players: list[int] = players
-        self.current_player: int = players[0]
+        self.current_player: int = players[0] if players else 0
+        self.current_idx = 0
+        self.other_keyset: list[PlayerKeySet] = []
 
     def set_controlled_players(self, controlled_players: list[int]) -> None:
         self.controlled_players = controlled_players
         self.current_player = self.controlled_players[0]
+        self.current_idx = 0
 
     def next_player(self) -> None:
-        self.current_player += 1
-        if self.current_player > self.controlled_players[-1]:
-            self.current_player = self.controlled_players[0]
-        print(self.current_player, self.controlled_players)
+        self.current_idx = (self.current_idx + 1) % len(self.controlled_players)
+        if self.other_keyset:
+            for ok in self.other_keyset:
+                if ok.current_idx == self.current_idx:
+                    self.next_player()
+                    return
+        self.current_player = self.controlled_players[self.current_idx]
 
 
 class PyGameGUI:
@@ -183,28 +189,32 @@ class PyGameGUI:
 
     def setup_player_keys(self, n=1, disjunct=False):
         if n:
+            players = list(range(self.number_humans_to_be_added))
             key_set1 = PlayerKeySet(
                 move_keys=[pygame.K_a, pygame.K_d, pygame.K_w, pygame.K_s],
                 interact_key=pygame.K_f,
                 pickup_key=pygame.K_e,
                 switch_key=pygame.K_SPACE,
-                players=list(range(self.number_humans_to_be_added)),
+                players=players,
             )
             key_set2 = PlayerKeySet(
                 move_keys=[pygame.K_LEFT, pygame.K_RIGHT, pygame.K_UP, pygame.K_DOWN],
                 interact_key=pygame.K_i,
                 pickup_key=pygame.K_o,
                 switch_key=pygame.K_p,
-                players=list(range(self.number_humans_to_be_added)),
+                players=players,
             )
             key_sets = [key_set1, key_set2]
 
             if disjunct:
-                split_idx = int(np.ceil(self.number_humans_to_be_added / 2))
-                key_set1.set_controlled_players(list(range(0, split_idx)))
-                key_set2.set_controlled_players(
-                    list(range(split_idx, self.number_humans_to_be_added))
-                )
+                key_set1.set_controlled_players(players[::2])
+                key_set2.set_controlled_players(players[1::2])
+            elif n > 1:
+                key_set1.set_controlled_players(players)
+                key_set2.set_controlled_players(players)
+                key_set1.other_keyset = [key_set2]
+                key_set2.other_keyset = [key_set1]
+                key_set2.next_player()
             return key_sets[:n]
         else:
             return []
diff --git a/overcooked_simulator/overcooked_environment.py b/overcooked_simulator/overcooked_environment.py
index 7621761e..e3e9c2f5 100644
--- a/overcooked_simulator/overcooked_environment.py
+++ b/overcooked_simulator/overcooked_environment.py
@@ -402,7 +402,6 @@ class Environment:
         Detects collisions with other players and pushes them out of the way.
 
         Args:
-            player: The player to move.
             duration: The duration for how long the movement to perform.
         """
         d_time = duration.total_seconds()
@@ -415,13 +414,12 @@ class Environment:
             ],
             dtype=float,
         )
-        print(player_positions, ".")
 
         targeted_positions = player_positions + (
             player_movement_vectors * (self.player_movement_speed * d_time)
         )
 
-        # Collisions player player
+        # Collisions player between player
         distances_players_after_scipy = distance_matrix(
             targeted_positions, targeted_positions
         )
@@ -457,52 +455,46 @@ class Environment:
         ]
 
         nearest_counter_to_player = closest_counter_positions - new_positions
-        # print(nearest_counter_to_player)
 
         collided = np.min(counter_distances, axis=1) < self.player_radius + 0.5
-        # print("                     COLLIDED", collided)
-
-        # print("CLOSEST_COUNTER", closest_counter_positions)
         relevant_axes = np.abs(nearest_counter_to_player).argmax(axis=1)
-        relevant_values = nearest_counter_to_player.max(axis=1)
 
         for idx, player in enumerate(player_positions):
             axis = relevant_axes[idx]
 
             if collided[idx]:
-                # print("before", updated_movement)
+                # collide with counter left or top
                 if nearest_counter_to_player[idx][axis] < 0:
-                    # print("settings more")
-                    # new_positions[idx, axis] = np.min(
-                    #     [
-                    #         player_positions[idx, axis],
-                    #         closest_counter_positions[idx, axis],
-                    #     ]
-                    # )
-
                     updated_movement[idx, axis] = max(updated_movement[idx, axis], 0)
+                # collide with counter right or bottom
                 if nearest_counter_to_player[idx][axis] > 0:
-                    # print("settings less")
-                    # new_positions[idx, axis] = np.max(
-                    #     [
-                    #         player_positions[idx, axis],
-                    #         closest_counter_positions[idx, axis],
-                    #     ]
-                    # )
-
                     updated_movement[idx, axis] = min(updated_movement[idx, axis], 0)
-                # print("after", updated_movement)
 
-        # new_positions[collided] = player_positions[collided]
-        print(updated_movement, "-")
         new_positions = player_positions + (
             updated_movement * (self.player_movement_speed * d_time)
         )
-        print(new_positions, "<")
 
-        # new_positions[min_counter_distances < self.player_radius] = player_positions[min_counter_distances < self.player_radius]
-
-        # counter_distances_axes = np.max((np.abs(counter_diff_vecs)), axis=1)
+        # 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 if two moving players collide into each other: No movement (Future: slide?)
+        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]
 
         # Collisions player world borders
         new_positions = np.max(
@@ -513,8 +505,9 @@ class Environment:
         )
 
         for idx, p in enumerate(self.players.values()):
-            p.turn(player_movement_vectors[idx])
-            p.move_abs(new_positions[idx])
+            if not (new_positions[idx] == player_positions[idx]).all():
+                p.turn(player_movement_vectors[idx])
+                p.move_abs(new_positions[idx])
 
     def detect_collision(self, player: Player):
         """Detect collisions between the player and other players or counters.
-- 
GitLab