Chaotic debugging and testing

overcooked_simulator/game_content/environment_config.yaml

@@ -83,5 +83,5 @@ orders:
 
 player_config:
   radius: 0.4
-  player_speed_units_per_seconds: 8
+  player_speed_units_per_seconds: 6
   interaction_range: 1.6

overcooked_simulator/game_content/layouts/empty.layout

-______
-______
-______
-______
-______
-______
-_____P
\ No newline at end of file
+_______
+_______
+_______
+__#____
+__#P___
+_______
+_______
+_______

overcooked_simulator/overcooked_environment.py

@@ -171,8 +171,9 @@ class Environment:
             dtype=float,
         )
 
-
-        self.player_movement_speed = self.environment_config["player_config"]["player_speed_units_per_seconds"]
+        self.player_movement_speed = self.environment_config["player_config"][
+            "player_speed_units_per_seconds"
+        ]
         self.player_radius = self.environment_config["player_config"]["radius"]
 
         progress_counter_classes = list(
@@ -212,8 +213,12 @@ class Environment:
 
     def set_collision_arrays(self):
         number_players = len(self.players)
-        self.world_borders_lower = self.world_borders[np.newaxis, :, 0].repeat(number_players, axis=0)
-        self.world_borders_upper = self.world_borders[np.newaxis, :, 1].repeat(number_players, axis=0)
+        self.world_borders_lower = self.world_borders[np.newaxis, :, 0].repeat(
+            number_players, axis=0
+        )
+        self.world_borders_upper = self.world_borders[np.newaxis, :, 1].repeat(
+            number_players, axis=0
+        )
 
     def get_env_time(self):
         """the internal time of the environment. An environment starts always with the time from `create_init_env_time`.
@@ -403,37 +408,107 @@ class Environment:
         d_time = duration.total_seconds()
 
         player_positions = np.array([p.pos for p in self.players.values()], dtype=float)
-        player_movement_vectors = np.array([p.current_movement if self.env_time <= p.movement_until else [0, 0] for p in self.players.values()], dtype=float)
+        player_movement_vectors = np.array(
+            [
+                p.current_movement if self.env_time <= p.movement_until else [0, 0]
+                for p in self.players.values()
+            ],
+            dtype=float,
+        )
 
-        new_positions = player_positions + (player_movement_vectors * (self.player_movement_speed * d_time))
+        targeted_positions = player_positions + (
+            player_movement_vectors * (self.player_movement_speed * d_time)
+        )
 
         # Collisions player player
-        distances_players_after_scipy = distance_matrix(new_positions, new_positions)
-
-        player_diff_vecs = -(player_positions[:, np.newaxis, :] - player_positions[np.newaxis, :, :])
+        distances_players_after_scipy = distance_matrix(
+            targeted_positions, targeted_positions
+        )
 
-        collision_idxs = distances_players_after_scipy < (2*self.player_radius)
-        eye_idxs = np.eye(distances_players_after_scipy.shape[0], distances_players_after_scipy.shape[1], dtype=bool)
+        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(
+            distances_players_after_scipy.shape[0],
+            distances_players_after_scipy.shape[1],
+            dtype=bool,
+        )
         collision_idxs[eye_idxs] = False
-        # collisions_any = np.any(collision_idxs, axis=1)
 
         # Player push players around
-        player_diff_vecs[collision_idxs==False] = 0
+        player_diff_vecs[collision_idxs == False] = 0
         push_vectors = np.sum(player_diff_vecs, axis=0)
-        # new_positions[collisions_any] = player_positions[collisions_any]
-        new_positions += push_vectors * (self.player_movement_speed * d_time)
 
-        # Collisions player world borders
-        new_positions = np.max([new_positions, self.world_borders_lower+self.player_radius], axis=0)
-        new_positions = np.min([new_positions, self.world_borders_upper-self.player_radius], axis=0)
+        updated_movement = push_vectors + player_movement_vectors
+        new_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, :, :])
+        counter_diff_vecs = (
+            new_positions[:, np.newaxis, :] - self.counter_positions[np.newaxis, :, :]
+        )
         counter_distances = np.max((np.abs(counter_diff_vecs)), axis=2)
-        min_counter_distances = np.min(counter_distances, axis=1) - 0.5
-        new_positions[min_counter_distances < self.player_radius] = player_positions[min_counter_distances < self.player_radius]
+        # 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
+        print(nearest_counter_to_player)
+
+        collided = np.min(counter_distances, axis=1) - 0.5 < self.player_radius
+        # print("                     COLLIDED", collided)
 
-        counter_distances_axes = np.max((np.abs(counter_diff_vecs)), axis=1)
+        # print("CLOSEST_COUNTER", closest_counter_positions)
+        relevant_axes = nearest_counter_to_player.argmax(axis=1)
+        relevant_values = nearest_counter_to_player.max(axis=1)
+
+        new_positions = player_positions + (
+            updated_movement * (self.player_movement_speed * d_time)
+        )
+
+        for idx, player in enumerate(player_positions):
+            axis = relevant_axes[idx]
+
+            if collided[idx]:
+                # print("before", updated_movement)
+                if relevant_values[idx] - 0.5 > 0:
+                    # print("settings more")
+                    new_positions[idx, axis] = np.min(
+                        [
+                            player_positions[idx, axis],
+                            closest_counter_positions[idx, axis],
+                        ]
+                    )
+
+                    # updated_movement[idx, axis] = np.max(updated_movement[idx, axis], 0)
+                if relevant_values[idx] + 0.5 < 0:
+                    # print("settings less")
+                    new_positions[idx, axis] = np.max(
+                        [
+                            player_positions[idx, axis],
+                            closest_counter_positions[idx, axis],
+                        ]
+                    )
+
+                    # updated_movement[idx, axis] = np.min(updated_movement[idx, axis], 0)
+                # print("after", updated_movement)
+
+        # new_positions[collided] = player_positions[collided]
+
+        # 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)
+
+        # Collisions player world borders
+        new_positions = np.max(
+            [new_positions, self.world_borders_lower + self.player_radius], axis=0
+        )
+        new_positions = np.min(
+            [new_positions, self.world_borders_upper - self.player_radius], axis=0
+        )
 
         for idx, p in enumerate(self.players.values()):
             p.turn(player_movement_vectors[idx])