From 0df466403adb47dff50266d1c84fcee63822e795 Mon Sep 17 00:00:00 2001
From: fheinrich <fheinrich@techfak.uni-bielefeld.de>
Date: Wed, 24 Jan 2024 16:59:17 +0100
Subject: [PATCH] Counters get an assigned orientation. They are oriented
 towards the next free tile, if more exist then the one is selected which
 looks to the kitchen center the most.

---
 overcooked_simulator/counters.py              |  8 +++
 .../game_content/layouts/basic.layout         | 18 +++---
 .../gui_2d_vis/overcooked_gui.py              | 29 +++++++++-
 .../overcooked_environment.py                 | 56 +++++++++++++++++++
 4 files changed, 100 insertions(+), 11 deletions(-)

diff --git a/overcooked_simulator/counters.py b/overcooked_simulator/counters.py
index 50d76412..b05ab74b 100644
--- a/overcooked_simulator/counters.py
+++ b/overcooked_simulator/counters.py
@@ -92,11 +92,19 @@ class Counter:
         """
         self.pos: npt.NDArray[float] = pos
         self.occupied_by: Optional[Item] = occupied_by
+        self.orientation: npt.NDArray[float] = np.zeros(2)
+        self.orientation[0] = 1
 
     @property
     def occupied(self):
         return self.occupied_by is not None
 
+    def set_orientation(self, orientation: npt.NDArray[float]) -> None:
+        if not np.isclose(np.linalg.norm(orientation), 1):
+            self.orientation = orientation / np.linalg.norm(orientation)
+        else:
+            self.orientation = orientation
+
     def pick_up(self, on_hands: bool = True) -> Item | None:
         """Gets called upon a player performing the pickup action. If the counter can give something to
         the player, it does so. In the standard counter this is when an item is on the counter.
diff --git a/overcooked_simulator/game_content/layouts/basic.layout b/overcooked_simulator/game_content/layouts/basic.layout
index 2e7395aa..e9639b31 100644
--- a/overcooked_simulator/game_content/layouts/basic.layout
+++ b/overcooked_simulator/game_content/layouts/basic.layout
@@ -1,9 +1,9 @@
-#QU#T###NLB#
-#__________M
-#__________#
-W___________
-#__A_____A__
-C___________
-C__________#
-#__________X
-#P#S+####S+#
\ No newline at end of file
+#QU#T###NLB##
+#__________M#
+#__________##
+W___________#
+#__A_____A__#
+C___________#
+C__________##
+#__________X#
+#P#S+####S+##
\ 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 ed86bfad..e62318bc 100644
--- a/overcooked_simulator/gui_2d_vis/overcooked_gui.py
+++ b/overcooked_simulator/gui_2d_vis/overcooked_gui.py
@@ -32,7 +32,7 @@ from overcooked_simulator.simulation_runner import Simulator
 
 USE_PLAYER_COOK_SPRITES = True
 SHOW_INTERACTION_RANGE = False
-SHOW_COUNTER_CENTERS = False
+SHOW_COUNTER_CENTERS = True
 
 
 class MenuStates(Enum):
@@ -557,10 +557,35 @@ class PyGameGUI:
         Args:
             state: The game state returned by the environment.
         """
+
+        pygame.draw.circle(
+            self.game_screen,
+            colors["red"],
+            np.array([self.game_width / 2, self.game_height / 2]),
+            3,
+        )
+
         for counter in state["counters"]:
             self.draw_counter(counter)
             if SHOW_COUNTER_CENTERS:
-                pygame.draw.circle(self.game_screen, colors["green1"], counter.pos, 3)
+                pos = counter.pos * self.grid_size
+                pygame.draw.circle(self.game_screen, colors["green1"], pos, 3)
+                facing = counter.orientation
+                pygame.draw.polygon(
+                    self.game_screen,
+                    colors["red"],
+                    (
+                        (
+                            pos[0] + (facing[1] * 0.1 * self.grid_size),
+                            pos[1] - (facing[0] * 0.1 * self.grid_size),
+                        ),
+                        (
+                            pos[0] - (facing[1] * 0.1 * self.grid_size),
+                            pos[1] + (facing[0] * 0.1 * self.grid_size),
+                        ),
+                        pos + (facing * 0.5 * self.grid_size),
+                    ),
+                )
 
     def update_score_label(self, state):
         score = state["score"]
diff --git a/overcooked_simulator/overcooked_environment.py b/overcooked_simulator/overcooked_environment.py
index b60ed369..7f13e036 100644
--- a/overcooked_simulator/overcooked_environment.py
+++ b/overcooked_simulator/overcooked_environment.py
@@ -342,9 +342,12 @@ class Environment:
             lines = layout_file.readlines()
             self.kitchen_height = len(lines)
 
+        grid = []
+
         for line in lines:
             line = line.replace("\n", "").replace(" ", "")  # remove newline char
             current_x = 0.5
+            grid_line = []
             for character in line:
                 character = character.capitalize()
                 pos = np.array([current_x, current_y])
@@ -352,7 +355,9 @@ class Environment:
                 if not isinstance(counter_class, str):
                     counter = counter_class(pos)
                     counters.append(counter)
+                    grid_line.append(1)
                 else:
+                    grid_line.append(0)
                     if counter_class == "Agent":
                         designated_player_positions.append(
                             np.array([current_x, current_y])
@@ -362,12 +367,63 @@ class Environment:
                 current_x += 1
                 if current_x > self.kitchen_width:
                     self.kitchen_width = current_x
+
+            grid.append(grid_line)
             current_y += 1
 
         self.kitchen_width -= 0.5
 
+        self.determine_counter_orientations(
+            counters, grid, np.array([self.kitchen_width / 2, self.kitchen_height / 2])
+        )
+
         return counters, designated_player_positions, free_positions
 
+    def determine_counter_orientations(self, counters, grid, kitchen_center):
+        grid = np.array(grid).T
+
+        grid_width = grid.shape[0]
+        grid_height = grid.shape[1]
+
+        for c in counters:
+            grid_idx = np.floor(c.pos).astype(int)
+            neighbour_offsets = np.array([[0, 1], [0, -1], [1, 0], [-1, 0]], dtype=int)
+
+            neighbours = []
+            for offset in neighbour_offsets:
+                neighbour_pos = grid_idx + offset
+                if (
+                    neighbour_pos[0] > (grid_width - 1)
+                    or neighbour_pos[0] < 0
+                    or neighbour_pos[1] > (grid_height - 1)
+                    or neighbour_pos[1] < 0
+                ):
+                    pass
+                else:
+                    if grid[neighbour_pos[0]][neighbour_pos[1]] == 0:
+                        neighbours.append(offset)
+            if len(neighbours) > 0:
+                vector_to_center = c.pos - kitchen_center
+                vector_to_center /= np.linalg.norm(vector_to_center)
+                n_idx = np.argmin(
+                    np.linalg.norm(vector_to_center - n) for n in neighbours
+                )
+                nearest_vec = neighbours[n_idx]
+                # print(nearest_vec, type(nearest_vec))
+                c.set_orientation(nearest_vec)
+
+            else:
+                print("HERE", c)
+                c.set_orientation(np.array([0, 1]))
+
+        # for c in counters:
+        #     near_counters = [
+        #         other
+        #         for other in counters
+        #         if np.isclose(np.linalg.norm(c.pos - other.pos), 1)
+        #     ]
+        #     # print(c.pos, len(near_counters))
+
     def perform_action(self, action: Action):
         """Performs an action of a player in the environment. Maps different types of action inputs to the
         correct execution of the players.
-- 
GitLab