From 6b8d5bc94c41a1dd68f5f7fc6ced44d03080857b Mon Sep 17 00:00:00 2001 From: lbaumberger Date: Tue, 28 Apr 2026 00:42:22 +0200 Subject: [PATCH] updated parameters and updated visualization function to better display the flow --- forces.hpp | 4 ++-- main.cpp | 49 ++++++++++++++++++++++++++++++++++++------------ visualization.py | 21 ++++++++++++++++++--- 3 files changed, 57 insertions(+), 17 deletions(-) diff --git a/forces.hpp b/forces.hpp index 6a24f9a..4fb0002 100644 --- a/forces.hpp +++ b/forces.hpp @@ -22,7 +22,7 @@ const double R = 0.2; const double c = 0.5; // $\varphi$ -const double phi = 100.0 / 180.0 * M_PI; +const double phi = 45.0 / 180.0 * M_PI; const double cos_phi = cos(phi); @@ -30,7 +30,7 @@ const double cos_phi = cos(phi); // step size = 0.2 seconds const double delta_t = 2; -const double dt = 0.001; +const double dt = 0.2; // # Optional (forces.cpp: compute_repulsive_effects_from_pedestrians) // const double DISTANCE = 2.0; diff --git a/main.cpp b/main.cpp index bf82ed9..d36aa31 100644 --- a/main.cpp +++ b/main.cpp @@ -28,12 +28,13 @@ Pedestrian *init_pedestrians(int n_p) { // std::mt19937 gen(rd()); // use rd instead of 42 as seed for real randomness std::mt19937 gen(42); // use fixed seed for reproducibility std::normal_distribution velocity_gen(1.34, 0.26); // 1.34 and 0.26 are the mean and stdrd deviation used in paper - std::uniform_real_distribution x_gen(2, BOARD_LENGTH - 2); - std::uniform_real_distribution y_gen(2, BOARD_WIDTH - 2); + std::uniform_real_distribution x_gen_left(2, BOARD_LENGTH / 2); + std::uniform_real_distribution x_gen_right(BOARD_LENGTH / 2, BOARD_LENGTH - 2); + std::uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); for (int i = 0; i < n_p / 2; i++) { double y = y_gen(gen); - Vector2d start_pos = Vector2d(x_gen(gen), y); - Vector2d destination = Vector2d(BOARD_LENGTH + 30, BOARD_WIDTH / 2); + Vector2d start_pos = Vector2d(x_gen_left(gen), y); + Vector2d destination = Vector2d(BOARD_LENGTH + 30, y); // double desired_v0 = velocity_gen(gen); double desired_v0; do { @@ -45,8 +46,8 @@ Pedestrian *init_pedestrians(int n_p) { for (int i = n_p / 2; i < n_p; i++) { double y = y_gen(gen); - Vector2d start_pos = Vector2d(x_gen(gen), y); - Vector2d destination = Vector2d(-30, BOARD_WIDTH / 2); + Vector2d start_pos = Vector2d(x_gen_right(gen), y); + Vector2d destination = Vector2d(-30, y); // double desired_v0 = velocity_gen(gen); double desired_v0; do { @@ -85,7 +86,7 @@ int main(int argc, const char *argv[]) { Board board = Board(BOARD_LENGTH, BOARD_WIDTH, 0, obs); // Logic to get/dump the simulation data - const int dump_interval = 50; // dump every 5 iterations -> 20 fps (since dt = 0.01) + const int dump_interval = 1; // dump every 5 iterations -> 20 fps (since dt = 0.01) const char *dump_path = (argc >= 4) ? argv[3] : nullptr; std::vector> samples; // t, id, x, y, e.x, e.y, color @@ -130,6 +131,11 @@ int main(int argc, const char *argv[]) { // # 4 flops = 2 ADDs, 2 MULTs // integrate preferred velocity: dw/dt = F (Eq. 10) p.w = p.w + p.f_i * dt; + + double w_norm = p.w.norm(); + if (w_norm > p.v_max) { + p.w = p.w * (p.v_max / w_norm); + } // Vector2d w_alpha = p.curr_vel + p.f_i * dt; // commented since this was using p.curr_vel instead of p.w (different from paper) // # 8 flops = 2 MULTs, 2 DIVs, 1 SQRT, 2 MULTs, 1 ADD @@ -142,7 +148,7 @@ int main(int argc, const char *argv[]) { p.curr_pos = p.curr_pos + p.curr_vel * dt; // # 10 flops = 5 ADDs, 2 DIVs, 1 SQRT, 2 MULTs - p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm(); + // p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm(); // how to handle if a pedestrian reached the end of our simulation/end of the hallway // The option we do here is to just "teleport" the person back to the beginning @@ -150,17 +156,36 @@ int main(int argc, const char *argv[]) { if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { // basically just keep the y position and reset the x p.curr_pos.x = 1.0; - // p.destination.y = p.curr_pos.y; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(p.desired_v0, 0); + p.curr_vel = p.w; } if (p.color == BLACK && p.curr_pos.x < 1.5) { p.curr_pos.x = BOARD_LENGTH - 1.0; - // p.destination.y = p.curr_pos.y; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(-p.desired_v0, 0); + p.curr_vel = p.w; } // # 10 flops = 5 ADDs, 2 DIVs, 1 SQRT, 2 MULTs - // p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm(); + p.e = (p.destination - p.curr_pos) / (p.destination - p.curr_pos).norm(); // p.e = (p.curr_vel - p.destination) / (p.curr_vel - p.destination).norm(); - p.e = (p.destination - (p.curr_vel / p.curr_vel.norm())) / (p.destination - (p.curr_vel / p.curr_vel.norm())).norm(); + // p.e = (p.destination + (p.curr_vel / p.curr_vel.norm())) / (p.destination + (p.curr_vel / p.curr_vel.norm())).norm(); + const double WALL_EPS = 1e-3; + if (p.curr_pos.y < 0.0) { + p.curr_pos.y = WALL_EPS; + if (p.curr_vel.y < 0) + p.curr_vel.y = 0; + if (p.w.y < 0) + p.w.y = 0; + } + if (p.curr_pos.y > BOARD_WIDTH) { + p.curr_pos.y = BOARD_WIDTH - WALL_EPS; + if (p.curr_vel.y > 0) + p.curr_vel.y = 0; + if (p.w.y > 0) + p.w.y = 0; + } // if we have a dump path/file we wanna save the data for visualisation if (dump_path && (t % dump_interval == 0)) { diff --git a/visualization.py b/visualization.py index b682c49..2d44439 100644 --- a/visualization.py +++ b/visualization.py @@ -1,4 +1,5 @@ import sys +import numpy as np import pandas as pd import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation @@ -9,6 +10,20 @@ if len(sys.argv) < 2: df = pd.read_csv(sys.argv[1]) frames = sorted(df["time"].unique()) +# Replace p.e with direction-of-next-hop: where the ped actually moves to next frame. +# Falls back to p.e for the last frame and for teleport jumps. +df = df.sort_values(["id", "time"]).reset_index(drop=True) +df["dx"] = df.groupby("id")["x"].shift(-1) - df["x"] +df["dy"] = df.groupby("id")["y"].shift(-1) - df["y"] +mag = np.sqrt(df["dx"]**2 + df["dy"]**2) +TELEPORT_THRESHOLD = 5.0 # any per-step jump larger than this is the teleport, not motion +fallback = (mag > TELEPORT_THRESHOLD) | mag.isna() | (mag < 1e-9) +df.loc[fallback, "dx"] = df.loc[fallback, "p.e.x"] +df.loc[fallback, "dy"] = df.loc[fallback, "p.e.y"] +mag = np.sqrt(df["dx"]**2 + df["dy"]**2).replace(0, 1.0) +df["dx"] = df["dx"] / mag +df["dy"] = df["dy"] / mag + fig, ax = plt.subplots(figsize=(10, 2.2)) ax.set_xlim(0, 50); ax.set_ylim(0, 10) ax.set_aspect("equal"); ax.set_facecolor("white") @@ -39,13 +54,13 @@ def update(t): # Draw fresh arrows each frame arrow_scale = 1.5 # Tune this to change arrow length visually if len(w): - q_white = ax.quiver(w["x"], w["y"], w["p.e.x"], w["p.e.y"], + q_white = ax.quiver(w["x"], w["y"], w["dx"], w["dy"], color="black", angles="xy", scale_units="xy", scale=1/arrow_scale, width=0.003, headwidth=4, headlength=5, zorder=3) quivers.append(q_white) if len(b): - q_black = ax.quiver(b["x"], b["y"], b["p.e.x"], b["p.e.y"], + q_black = ax.quiver(b["x"], b["y"], b["dx"], b["dy"], color="gray", angles="xy", scale_units="xy", scale=1/arrow_scale, width=0.003, headwidth=4, headlength=5, zorder=3) @@ -57,7 +72,7 @@ def update(t): anim = FuncAnimation(fig, update, frames=frames, interval=50, blit=False) if len(sys.argv) >= 3: - anim.save(sys.argv[2], writer="ffmpeg", fps=20) + anim.save(sys.argv[2], writer="ffmpeg", fps=5) print(f"Saved {sys.argv[2]}") else: plt.show() \ No newline at end of file