diff --git a/overcooked_simulator/game_content/environment_config.yaml b/overcooked_simulator/game_content/environment_config.yaml
index c0de86d2afc35fad41a5c206b448a0f1eecdad6e..9e60cf36634c0c4010734c704066d73a93ea4d0a 100644
--- a/overcooked_simulator/game_content/environment_config.yaml
+++ b/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
diff --git a/overcooked_simulator/game_content/layouts/empty.layout b/overcooked_simulator/game_content/layouts/empty.layout
index 2fa1dd8c29c076e9d94e9a305843ff87bebb29f1..e0147e33f9259720edbc7d4bb380e7a512d728a1 100644
--- a/overcooked_simulator/game_content/layouts/empty.layout
+++ b/overcooked_simulator/game_content/layouts/empty.layout
@@ -1,7 +1,8 @@
-______
-______
-______
-______
-______
-______
-_____P
\ No newline at end of file
+_______
+_______
+_______
+__#____
+__#P___
+_______
+_______
+_______
diff --git a/overcooked_simulator/overcooked_environment.py b/overcooked_simulator/overcooked_environment.py
index 8fdb399dbc014f95c29b0abf9fa0a3d4e74ce5f1..c51de60a423afc030f7ebcc2ece6300c9932a388 100644
--- a/overcooked_simulator/overcooked_environment.py
+++ b/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])