Add unit tests, refactor simulation code, improve Makefile, and add documentation

Makefile

@@ -1,7 +1,121 @@
-main: board.hpp forces.cpp forces.hpp main.cpp pedestrian.hpp vector2d.hpp constants.hpp
+# Makefile for the project.
-	g++ -O3 -march=native -std=c++23 -S main.cpp -o main.s
+# Provides targets for building, running, benchmarking, testing, and visualization.
-	g++ -O3 -march=native -std=c++23 -S forces.cpp -o force.s
+#
-	g++ -O3 -march=native -std=c++23 main.cpp forces.cpp -o main
+# Main commands:
+#   make run         run the simulation, producing a CSV file
+#                    containing pedestrian states at each iteration
+#   make video       generate a video of the simulation
+#   make run-bench   run benchmark measurements
+#   make test        run all unit tests
+#
+# Default parameters can be overridden from the shell, as follows:
+# (parameters can be provided selectively and in any order)
+#
+#   make run N_P=<num_ped> N_ITER=<num_iter> CSV=<out.csv>
+#   make video N_P=<num_ped> N_ITER=<num_iter> CSV=<out.csv> MP4=<video.mp4>
+#   make run-bench N_P=<num_ped> N_ITER=<num_iter>
 
-sizes: sizes.cpp
+
-	g++ -O3 -march=native -std=c++23 sizes.cpp -o sizes
+# Select the C++ compiler.
+# Prefer g++ when available, otherwise fall back to clang++.
+CXX := $(shell command -v g++ || command -v clang++)
+
+ifeq ($(CXX),)
+$(error No C++ compiler found. Please install g++ or clang++)
+endif
+
+ARCH := $(shell uname -m)
+
+# Prepend sudo on ARM machines when benchmarking.
+# This is needed to access the PMU performance counters.
+ifeq ($(ARCH),arm64)
+SUDO_BENCH := sudo
+else ifeq ($(ARCH),aarch64)
+SUDO_BENCH := sudo
+else
+SUDO_BENCH :=
+endif
+
+# Compiler flags:
+#   -O3            enable high-level optimization
+#   -march=native  optimize for the current machine
+#   -std=c++23     use the C++23 standard
+#   -Wall -Wextra  enable useful warnings
+#   -flto          enable link-time optimization across .cpp files, allowing
+#                  cross-file optimizations such as function inlining
+CXXFLAGS := -O3 -march=native -std=c++23 -Wall -Wextra -flto
+
+SRC := main.cpp forces.cpp social_force_baseline.cpp
+HEADERS := board.hpp forces.hpp pedestrian.hpp vector2d.hpp constants.hpp social_force.hpp
+
+TEST_SRC := test_vector2d.cpp test_board.cpp test_forces.cpp forces.cpp
+TEST_TARGET := run_tests
+TEST_LIBS := -lgtest -lgtest_main -pthread
+
+TARGET := main
+SIZES_TARGET := sizes
+
+# Default simulation parameters and output files.
+# These can be overridden from the make command line; see examples above.
+N_P ?= 60              # Default number of pedestrians.
+N_ITER ?= 4000         # Default number of simulation iterations.
+CSV ?= positions.csv   # Default CSV output file.
+MP4 ?= simulation.mp4  # Default visualization video output file.
+
+# Enable/disable non-reproducible random seeding.
+# Default: RANDOM_SEED=0 uses the fixed seed for reproducibility.
+# Use RANDOM_SEED=1 to seed from std::random_device.
+RANDOM_SEED ?= 0
+
+# Pass random-seeding mode to the C++ code.
+CXXFLAGS += -DRANDOM_SEED=$(RANDOM_SEED)
+
+.PHONY: all main run bench run-bench video sizes asm test clean clean-all
+
+all: main
+
+# make main : compile the main executable with benchmarking disabled.
+main: clean
+	$(CXX) $(CXXFLAGS) $(SRC) -o $(TARGET)
+
+# make run : compile the code and run the simulation without generating a video.
+run: main
+	./$(TARGET) $(N_P) $(N_ITER) $(CSV)
+
+# make bench : compile the main executable with benchmarking enabled.
+bench: CXXFLAGS += -DBENCHMARK=1
+bench: clean
+	$(SUDO_BENCH) $(CXX) $(CXXFLAGS) $(SRC) -o $(TARGET)
+
+# make run-bench : compile and run benchmark measurements.
+run-bench: bench
+	$(SUDO_BENCH) ./$(TARGET) $(N_P) $(N_ITER)
+
+# make video : compile the code, run the simulation, and generate the video.
+video: run
+	python visualization.py $(CSV) $(MP4)
+
+# make sizes : compile the size-check helper.
+sizes: clean
+	$(CXX) $(CXXFLAGS) sizes.cpp -o $(SIZES_TARGET)
+
+# make test : compile and run all unit tests.
+test: clean
+	$(CXX) $(CXXFLAGS) $(TEST_SRC) $(TEST_LIBS) -o $(TEST_TARGET)
+	./$(TEST_TARGET)
+
+# make asm : compile and generate assembly files.
+asm: clean main.s forces.s social_force_baseline.s
+
+%.s: %.cpp $(HEADERS)
+	$(CXX) -O3 -march=native -std=c++23 -Wall -Wextra -DRANDOM_SEED=$(RANDOM_SEED) -S $< -o $@
+
+# make clean : delete generated files from previous builds.
+clean:
+	rm -f $(TARGET) $(SIZES_TARGET) $(TEST_TARGET) *.o *.s
+
+# make clean-all : delete all generated files from previous builds and runs.
+# WARNING: this deletes all *.csv and *.mp4 files in this directory.
+# Move any output files you want to keep to another directory before running it.
+clean-all: clean
+	rm -f *.csv *.mp4

README.md

@@ -1,44 +1,456 @@
 # Advanced Systems Lab - Spring 2026
-This is the GitLab repository of group 70.
-Project topic: Social Force Model for Pedestrian Dynamics
 
-The goal of this project was to implement a fast version of Helbing's social force model. The final report includes an analysis and results of the work.
+This is the GitLab repository of **Group 70**.
 
-This repository contains all C++ and Python code that was used to generate and analyze the data.
+**Project topic:** Social Force Model for Pedestrian Dynamics
 
+The goal of this project was to implement and optimize Helbing's social force model for pedestrian dynamics. The final report includes the implementation details, performance analysis, and results of the work.
 
-## Compilation
+This repository contains the C++ simulation code and Python visualization code used to generate and analyze the data.
-Linux
 
-``
+---
 
-Windows
+## Requirements
 
-``
+The project uses a `Makefile` to simplify building, running, benchmarking, testing, and visualization.
 
-Apple
+Make sure `make` is installed:
 
-`clang++ -O3 -march=native -o main_test_visualisation main.cpp forces.cpp`
+```bash
+make --version
+```
 
-Compile the code with running the bash script 'src/main/run_simulation.sh'. After compilation was succesfull the user has 3 different option:
+You also need a C++ compiler. The Makefile automatically uses `g++` if available, otherwise it falls back to `clang++`.
-1. Benchmark
-2. Test
-3. Visualization
 
-The flags used are:
+Check your compiler with:
-`gcc -lm -O3 -mavx2 -mfma -ffast-math -g`
 
-## Storing coordinates in a csv
+```bash
-`./main_test_visualisation <number of pedestrians> <number of iterations> <csv file>`
+g++ --version
+clang++ --version
+```
 
+The visualization script requires Python and the following packages:
+
+```bash
+python3 -m pip install numpy pandas matplotlib
+```
+
+To generate MP4 videos, `ffmpeg` must also be installed.
+
+On Ubuntu/Debian:
+
+```bash
+sudo apt update
+sudo apt install make g++ clang python3 python3-pip ffmpeg libgtest-dev
+python3 -m pip install numpy pandas matplotlib
+```
+
+On macOS:
+
+```bash
+xcode-select --install
+brew install ffmpeg googletest
+python3 -m pip install numpy pandas matplotlib
+```
+
+---
+
+## Makefile commands
+
+The main commands are:
+
+```bash
+make run
+```
+
+Runs the simulation and writes pedestrian states to a CSV file.
+
+```bash
+make video
+```
+
+Runs the simulation and generates a visualization video.
+
+```bash
+make run-bench
+```
+
+Compiles and runs benchmark measurements.
+
+```bash
+make test
+```
+
+Compiles and runs all unit tests.
+
+```bash
+make asm
+```
+
+Generates assembly files.
+
+```bash
+make sizes
+```
+
+Compiles the helper executable used to inspect data-structure sizes.
+
+```bash
+make clean
+```
+
+Deletes generated build files.
+
+```bash
+make clean-all
+```
+
+Deletes generated build files and all `.csv` / `.mp4` outputs in the current directory.
+
+**Warning:** `make clean-all` removes all CSV and MP4 files in the directory. Move any output files you want to keep before running it.
+
+---
+
+## Default parameters
+
+The default simulation parameters are:
+
+```text
+N_P=60
+N_ITER=4000
+CSV=positions.csv
+MP4=simulation.mp4
+RANDOM_SEED=0
+```
+
+They can be overridden from the shell. Parameters can be provided selectively and in any order.
+
+Run the simulation with custom parameters:
+
+```bash
+make run N_P=100 N_ITER=5000 CSV=my_positions.csv
+```
+
+Generate a video with custom output names:
+
+```bash
+make video N_P=100 N_ITER=5000 CSV=my_positions.csv MP4=my_simulation.mp4
+```
+
+Run benchmark measurements with custom parameters:
+
+```bash
+make run-bench N_P=100 N_ITER=5000
+```
+
+Use non-reproducible random initialization:
+
+```bash
+make run RANDOM_SEED=1
+```
+
+or:
+
+```bash
+make video RANDOM_SEED=1
+```
+
+By default, `RANDOM_SEED=0` uses a fixed seed for reproducible simulations and benchmarks.
+
+---
+
+## Building
+
+To compile the main executable:
+
+```bash
+make main
+```
+
+This produces the executable:
+
+```text
+main
+```
+
+The Makefile uses the following compiler flags:
+
+```text
+-O3 -march=native -std=c++23 -Wall -Wextra -flto
+```
+
+where:
+
+- `-O3` enables high-level optimization.
+- `-march=native` optimizes for the current machine.
+- `-std=c++23` uses the C++23 standard.
+- `-Wall -Wextra` enables useful compiler warnings.
+- `-flto` enables link-time optimization across `.cpp` files, allowing cross-file optimizations such as function inlining.
+
+---
+
+## Running the simulation
+
+The recommended way to run the simulation is:
+
+```bash
+make run
+```
+
+This compiles the code and runs the simulation with the default parameters. It produces a CSV file containing pedestrian states at each saved time step.
+
+You can also run the executable manually after compiling:
+
+```bash
+./main <number_of_pedestrians> <number_of_iterations> [csv_output_path]
+```
+
+Example:
+
+```bash
+./main 60 4000 positions.csv
+```
+
+The third argument is optional. If it is provided, simulation states are written to the given CSV file.
+
+---
+
+## CSV output
+
+The CSV file contains the pedestrian states used for visualization.
+
+The output columns are:
+
+```text
+time,id,x,y,p.e.x,p.e.y,color
+```
+
+where:
+
+- `time` is the simulation time step.
+- `id` is the pedestrian index.
+- `x`, `y` are the pedestrian position coordinates.
+- `p.e.x`, `p.e.y` are the components of the desired direction.
+- `color` indicates the pedestrian group / walking direction.
+
+---
 
 ## Visualization
-`python3 visualization.py <csv file> <mp4 file>`
 
+To run the simulation and directly generate a video:
 
-## Benchmark
+```bash
+make video
+```
 
+This produces an MP4 file, by default:
 
+```text
+simulation.mp4
+```
 
-## Test
+To visualize an existing CSV file manually:
 
+```bash
+python3 visualization.py <csv_file> <mp4_file>
+```
+
+Example:
+
+```bash
+python3 visualization.py positions.csv simulation.mp4
+```
+
+The generated video shows the pedestrian dynamics produced by the social force simulation.
+
+---
+
+## Benchmarking
+
+Run:
+
+```bash
+make run-bench
+```
+
+This compiles the executable with benchmarking enabled and measures the simulation performance.
+
+On ARM/AArch64 machines, the Makefile automatically prepends `sudo` for benchmark compilation and execution because access to PMU performance counters may require elevated privileges.
+
+For reproducible benchmark results, keep:
+
+```text
+RANDOM_SEED=0
+```
+
+which is the default.
+
+Example:
+
+```bash
+make run-bench N_P=100 N_ITER=5000
+```
+
+The benchmark output includes estimated work, operational intensity, measured cycles, and estimated performance in flops per cycle.
+
+---
+
+## Tests
+
+Run all unit tests with:
+
+```bash
+make test
+```
+
+The tests use GoogleTest, so `libgtest-dev` or an equivalent GoogleTest installation must be available.
+
+The test target compiles and runs the unit tests for the simulation components.
+
+---
+
+## Assembly generation
+
+To generate assembly files:
+
+```bash
+make asm
+```
+
+This generates assembly output for the main source files, such as:
+
+```text
+main.s
+forces.s
+social_force_baseline.s
+```
+
+This can be useful for inspecting compiler optimizations and generated instructions.
+
+---
+
+## Structure-size helper
+
+To compile the helper executable used to inspect data-structure sizes:
+
+```bash
+make sizes
+```
+
+This produces:
+
+```text
+sizes
+```
+
+Run it with:
+
+```bash
+./sizes
+```
+
+---
+
+## Cleaning generated files
+
+Remove generated build files:
+
+```bash
+make clean
+```
+
+This deletes files such as:
+
+```text
+main
+sizes
+run_tests
+*.o
+*.s
+```
+
+Remove build files and generated CSV/MP4 outputs:
+
+```bash
+make clean-all
+```
+
+This deletes:
+
+```text
+main
+sizes
+run_tests
+*.o
+*.s
+*.csv
+*.mp4
+```
+
+**Warning:** `make clean-all` deletes all `.csv` and `.mp4` files in the current directory. Move any files you want to keep before running it.
+
+---
+
+## Notes on reproducibility
+
+By default, the simulation uses a fixed random seed:
+
+```text
+SEED=42
+```
+
+This makes simulation runs reproducible.
+
+To use non-reproducible initialization, run:
+
+```bash
+make run RANDOM_SEED=1
+```
+
+or:
+
+```bash
+make video RANDOM_SEED=1
+```
+
+For benchmarking, reproducible initialization is recommended, so the default `RANDOM_SEED=0` should normally be kept.
+
+---
+
+## Example workflow
+
+Compile and run the simulation:
+
+```bash
+make run
+```
+
+Generate a video:
+
+```bash
+make video
+```
+
+Run tests:
+
+```bash
+make test
+```
+
+Run benchmark measurements:
+
+```bash
+make run-bench
+```
+
+Clean generated build files:
+
+```bash
+make clean
+```
+
+Clean everything generated, including CSV and MP4 outputs:
+
+```bash
+make clean-all
+```

board.hpp

@@ -2,22 +2,44 @@
 #include "vector2d.hpp"
 
 struct Obstacle {
-  Vector2d start;
+  Vector2d start;       // First endpoint of the obstacle segment.
-  Vector2d end;
+  Vector2d end;         // Second endpoint of the obstacle segment.
-  Vector2d obstacle_v;
+  Vector2d obstacle_v;  // Direction vector of the segment: end - start.
-
+  double length_sq_inv; // Inverse squared length of the segment, used for projection.
-  double length_sq_inv;
 
+  // Default constructor: creates a degenerate obstacle at the origin.
+  // This is mainly used for array allocation before assigning real obstacles.
   Obstacle() : start(0, 0), end(0, 0), obstacle_v(0, 0), length_sq_inv(0.0) {}
+
+  // Constructs an obstacle as a finite line segment from s to e.
+  //
+  // The segment direction and inverse squared length are precomputed and cached because
+  // they are reused when projecting pedestrian positions onto the obstacle.
+  //
+  // @param s The first endpoint of the obstacle segment.
+  // @param e The second endpoint of the obstacle segment.
+  //
   Obstacle(Vector2d s, Vector2d e) : start(s), end(e) {
     obstacle_v = end - start;
     double length_sq = obstacle_v.dot(obstacle_v);
-    length_sq_inv = length_sq != 0 ? 1.0 / length_sq : 0.0;
+    length_sq_inv = length_sq != 0.0 ? 1.0 / length_sq : 0.0;
   }
 
-  // Find the point of the obstacle that is closest to p
+  // Flop count: 5 ADDs, 5 MULTs
-  // # 5 ADDs, 5 MULTs
+  //
-  Vector2d find_nearest_obstacle_point(Vector2d p) {
+  // Finds the point on this finite obstacle segment that is closest to p.
+  //
+  // The method projects the vector (p - start) onto the obstacle direction
+  // vector, then clamps the projection factor to [0, 1] so that the returned
+  // point lies on the finite segment.
+  //
+  // Note : If the obstacle is a single point, meaning start == end, length_sq_inv is zero
+  // and the method returns start.
+  //
+  // @param p The point for which the closest obstacle point is computed.
+  // @return The closest point on the obstacle segment to p.
+  //
+  Vector2d find_nearest_obstacle_point(const Vector2d &p) const {
 
     // # 3 ADDs, 3 MULTs
     double projection = (p - start).dot(obstacle_v) * length_sq_inv;
@@ -33,22 +55,43 @@ struct Obstacle {
 };
 
 struct Board {
-  double length, width;
+  double length;       // Board length.
-  Obstacle *obstacles; // Array of obstacles, represented as lines on the board
+  double width;        // Board width.
-  int n_obstacles;     // Number of obstacles
+  Obstacle *obstacles; // Array of obstacles contained in the board.
+  int n_obstacles;     // Total number of obstacles
+
+  // Constructs a rectangular board of size length x width.
+  //
+  // The input array o contains n user-defined obstacles. The constructor copies
+  // those obstacles and then appends the two horizontal borders of the board (bottom and top) as additional
+  // obstacle segments:
+  //
+  //   bottom: (0, 0) -> (length, 0)
+  //   top:    (0, width) -> (length, width)
+  //
+  // Note: only the horizontal borders are modeled as obstacles. The vertical
+  // borders are left open so pedestrians can enter and leave the walkway without
+  // being affected by repulsive forces from those borders. This is consistent
+  // with Fig. 2 in the paper, where the horizontal walkway boundaries are
+  // emphasized but the vertical ends are not shown as walls.
+  //
+  // @param l The board length.
+  // @param w The board width.
+  // @param n The number of user-defined obstacles in o.
+  // @param o Array of user-defined obstacles to copy into the board obstacles array.
+  //
+  Board(double l, double w, int n, Obstacle *o) : length(l), width(w), n_obstacles(n + 2) {
+    obstacles = new Obstacle[n + 2];
 
-  Board(double l, double w, int n, Obstacle *o) : length(l), width(w), n_obstacles(n + 4) {
-    obstacles = new Obstacle[n + 4];
     for (int i = 0; i < n; i++) {
       obstacles[i] = o[i];
     }
 
-    // borders of the board: bottom, right, top, left
+    // Board borders: bottom, top.
     obstacles[n] = Obstacle(Vector2d(0, 0), Vector2d(l, 0));
-    obstacles[n + 1] = Obstacle(Vector2d(l, 0), Vector2d(l, w));
+    obstacles[n + 1] = Obstacle(Vector2d(0, w), Vector2d(l, w));
-    obstacles[n + 2] = Obstacle(Vector2d(l, w), Vector2d(0, w));
-    obstacles[n + 3] = Obstacle(Vector2d(0, w), Vector2d(0, 0));
   }
 
+  // Releases the dynamically allocated obstacle array.
   ~Board() { delete[] obstacles; }
-};
+};

constants.hpp

@@ -1,8 +1,10 @@
 #pragma once
 #include <cmath>
 
-// $\tau_{\alpha}$
+//
-const double TAU_ALPHA = 0.5;
+// $\frac{1}{\tau_{\alpha}}$; $\tau_{\alpha} = 0.5s$
+//
+const double tau_alpha_inv = 2.0;
 
 // $V^0_{\alpha \beta}$
 const double V0_AB = 2.1;
@@ -10,79 +12,80 @@ const double V0_AB = 2.1;
 // $\sigma$
 const double SIGMA = 0.3;
 
+//
+// $\frac{-1}{2\sigma}$
+//
+const double NEG_TWO_SIGMA_INV = -1.0 / (2 * SIGMA);
+
+//
+// $\frac{-V^0_{\alpha\beta}}{2\sigma}$
+//
+const double NEG_V0_AB_div_TWO_SIGMA = V0_AB * NEG_TWO_SIGMA_INV;
+
 // $U^0_{\alpha B}$
 const double U0_AB = 10.0;
 
 // $R$
 const double R = 0.2;
 
-// $c$
-const double C = 0.5;
-
-// $\varphi$
-const double PHI = 100.0 / 180.0 * M_PI;
-
-// $<v^0>$
-const double V0_MEAN = 1.34;
-
-// $\sqrt{\theta}$
-const double V0_STD_DEV = 0.26;
-
-// $\varDelta_{t}$
-const double DELTA_T = 2;
-
-// $dt$
-const double DT = 0.02;
-
-// # ##############################################################################################
-
 //
-// $\frac{-1}{2\sigma}$
-//
-const double NEG_ONE_DIV_TWO_SIGMA = -1.0 / (2 * SIGMA);
-
-//
-// $\frac{-V^0_{\alpha\beta}}{2\sigma}$
-//
-const double NEG_V0_AB_DIV_TWO_SIGMA = -V0_AB / (2 * SIGMA);
-
-// $cos(\varphi)$
-const double COS_PHI = cos(PHI);
-
-// 
 // $\frac{-1}{R}$
-// 
+//
-const double NEG_R_INV = -1 / R;
+const double NEG_R_INV = -1.0 / R;
 
 //
 // $\frac{-U^0_{\alpha B}}{R}$
 //
-const double NEG_U0_AB_DIV_R = -U0_AB / R;
+const double NEG_U0_AB_div_R = -U0_AB / R;
 
-// $c * dt$
+// $-c$
-const double C_TIMES_DT = C * DT;
+const double c_neg = -0.5;
 
-// ${(\varDelta_{t})}^2$
+// $\varphi$
-const double DELTA_T_SQ = DELTA_T * DELTA_T;
+const double phi = 100.0 / 180.0 * M_PI;
+
+// $cos(\varphi)$
+const double cos_phi = cos(phi);
+
+// $<v^0>$
+const double v0_mean = 1.34;
+
+// $\sqrt{\theta}$
+const double v0_std_dev = 0.26;
+
+// $\varDelta_{t}$
+const double delta_t = 2;
+
+// $\varDelta_{t}^2$
+const double delta_t_sq = delta_t * delta_t;
+
+// $dt$
+const double dt = 0.02;
+
+// $-dt$
+const double dt_neg = -dt;
+
+// $-c \cdot dt$
+const double c_neg_times_dt = c_neg * dt;
 
 //
-// $\frac{1}{\tau_{\alpha}}$
+// $\frac{1}{\tau_{\alpha}} \cdot dt$
 //
-const double TAU_ALPHA_INV = 1 / TAU_ALPHA;
+const double dt_times_tau_alpha_inv = dt * tau_alpha_inv;
 
-//
+const double cost_add = 1;
-// $\frac{dt}{\tau_{\alpha}}$
+const double cost_mult = 1;
-//
+const double cost_div = 1;
-const double DT_TIMES_TAU_ALPHA_INV = DT * TAU_ALPHA_INV;
-
-// # ##############################################################################################
-
-const double COST_ADD = 1;
-const double COST_MULT = 1;
-const double COST_DIV = 1;
 
 // https://acl.inf.ethz.ch/teaching/fastcode/knowledgebase/#:~:text=How%20do%20exp(x)%2C%20sin(x)%2C%20etc.%20count%20in%20the%20flopcount%20of%20an%20algorithm%3F
-const double COST_EXP = 30;
+const double cost_exp = 30;
 
 // -O3 maps each sqrt() call to an instruction
-const double COST_SQRT = 1;
+const double cost_sqrt = 1;
+
+// Numerical tolerance used to avoid division by zero and unstable operations
+// when distances or norms are extremely small.
+const double EPS = 1e-9;
+
+// Squared numerical tolerance
+const double EPS_SQ = EPS * EPS;

forces.cpp

@@ -2,123 +2,139 @@
 #include "constants.hpp"
 #include <math.h>
 
-// # 2 ADDs, 5 MULTs, 1 SQRT
+// Flop count: 2 ADDs, 5 MULTs, 1 SQRT
 double w(Vector2d e, Vector2d f) {
-  if (e.dot(f) >= f.norm() * COS_PHI)
+  if (e.dot(f) >= f.norm() * cos_phi)
-    return DT;
+    return dt_neg;
   else
-    return C_TIMES_DT;
+    return c_neg_times_dt;
 }
 
-// # 4 ADDs, 4 MULTs
+// Flop count: 2 ADDs, 4 MULTs
-void add_acceleration_term(Pedestrian *p) {
+Vector2d acceleration_term_dt(const Pedestrian &alpha) {
 
   // # 2 MULTs
-  Vector2d desired_velocity = p->e * p->desired_v0;
+  Vector2d desired_velocity = alpha.e * alpha.desired_v0;
 
-  Vector2d actual_velocity = p->curr_vel;
+  Vector2d actual_velocity = alpha.curr_vel;
 
-  // # 4 ADDs, 2 MULTs
+  // # 2 ADDs, 2 MULTs
-  p->f_i += (desired_velocity - actual_velocity) * DT_TIMES_TAU_ALPHA_INV;
+  Vector2d f_alpha_0_dt = (desired_velocity - actual_velocity) * dt_times_tau_alpha_inv;
+
+  return f_alpha_0_dt;
 }
 
-// # (N - 1) * (14 ADDs, 23 MULTs, 1 DIV, 4 SQRTs, 1 EXP)
-//
-// $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$
-//
-void add_repulsive_effects_from_pedestrians(Pedestrian *pedestrians, Pedestrian *p, int N) {
 
-  for (int i = 0; i < N; i++) {
+// Flop count: 12 ADDs, 23 MULTs, 1 DIV, 4 SQRTs, 1 EXP
-
+Vector2d pedestrian_repulsion_dt(const Pedestrian &alpha, const Pedestrian &beta) {
-    if (pedestrians + i == p)
-      continue;
 
     // # 2 ADDs
     // $\vec{r}_{\alpha\beta}$
-    Vector2d r_ab = p->curr_pos - pedestrians[i].curr_pos;
+    Vector2d r_ab = alpha.curr_pos - beta.curr_pos;
 
     // # 1 ADD, 2 MULTs, 1 SQRT
     // $\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert$
     double r_ab_norm = r_ab.norm();
+    r_ab_norm = r_ab_norm > EPS ? r_ab_norm : EPS; // Avoid division by zero
+
+    // $v_\beta \Delta t \vec{e}_\beta$
+    Vector2d s_b_e = beta.s_b_e;
 
     // # 2 ADDs
     // $\vec{r}_{\alpha\beta} - \vec{v}_{\beta}\varDelta_{t}\vec{e}_{\beta}$
-    Vector2d r_ab2 = r_ab - pedestrians[i].s_b_e_b;
+    Vector2d r_ab2 = r_ab - s_b_e;
 
     // # 1 ADD, 2 MULTs, 1 SQRT
     // $\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert$
     double r_ab2_norm = r_ab2.norm();
+    r_ab2_norm = r_ab2_norm > EPS ? r_ab2_norm : EPS; // Avoid division by zero
 
     // # 1 ADD
     // $\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert$
     double r_ab_norm_sum = r_ab_norm + r_ab2_norm;
 
-    // # 1 ADD, 1 MULT, 1 SQRT
+    // # 1 ADD, 1 MULTs, 1 SQRT
     //
     // $2b = \sqrt{(\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert)^2 - (v_\beta \Delta t)^2}$
     //
-    double two_b = sqrt(r_ab_norm_sum * r_ab_norm_sum - pedestrians[i].s_b_sq);
+    double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - beta.s_b_sq;
+    two_b_sq = two_b_sq > EPS_SQ ? two_b_sq : EPS_SQ; // Avoid sqrt of negative and avoid division by zero
+    double two_b = sqrt(two_b_sq);
 
-    // # 3 MULTs, 1 DIV
+    // # 5 MULTs, 1 DIV, 1 EXP
     //
-    // $\frac{-V^0_{\alpha\beta}}{\sigma} \frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b \Arrowvert \vec{r}_{\alpha \beta} \Arrowvert \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}$
+    // $\frac{-V^0_{\alpha\beta}}{2\sigma}\,e^{-b/\sigma}\,\frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert\,\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}$
     //
-    double common_factor = NEG_V0_AB_DIV_TWO_SIGMA * r_ab_norm_sum / (two_b * r_ab_norm * r_ab2_norm);
+    double scalar_mult_common = (r_ab_norm_sum / (two_b * r_ab_norm * r_ab2_norm)) * (NEG_V0_AB_div_TWO_SIGMA * exp(two_b * NEG_TWO_SIGMA_INV));
+
+    // # 1 MULT
+    //
+    // $\frac{-V^0_{\alpha\beta}}{2\sigma}\,e^{-b/\sigma}\,\frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert}$
+    //
+    double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm;
+
+    // # 1 MULT
+    //
+    // $\frac{-V^0_{\alpha\beta}}{2\sigma}\,e^{-b/\sigma}\,\frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}$
+    //
+    double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm;
 
     // # 2 ADDs, 4 MULTs
-    // $2\grad_{\vec{r}_{\alpha \beta}}b$
+    //
-    // See: derivations.jpg
+    // $\vec{-f}_{\alpha \beta}(\vec{r}_{\alpha \beta}) = \frac{-V^0_{\alpha \beta}}{2\sigma}e^{-b / \sigma} 2\grad_{\vec{r}_{\alpha \beta}}b $
-    Vector2d A = (r_ab * r_ab2_norm + r_ab2 * r_ab_norm) * common_factor;
+    //
+    //
+    //          $= \frac{-V^0_{\alpha\beta}}{2\sigma}\,e^{-b/\sigma}\,\frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert}\,\vec{r}_{\alpha \beta} + \frac{-V^0_{\alpha\beta}}{2\sigma}\,e^{-b/\sigma}\,\frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}\,(\vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta)$
+    //
+    //
+    //
+    // $2\grad_{\vec{r}_{\alpha \beta}}b = \frac{(\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert)(\frac{\vec{r}_{\alpha \beta}}{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert} + \frac{\vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta}{\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert})}{2b}$
+    //
+    //
+    //       $= \frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert}\,\vec{r}_{\alpha \beta} + \frac{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}{2b\,\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert}\,(\vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta)$
+    //
+    //
+    Vector2d f_alpha_beta_neg = r_ab * scalar_mult_r_ab + r_ab2 * scalar_mult_r_ab2;
 
-    // # 4 MULTs, 1 EXP
+    // # 2 ADDs, 7 MULTs, 1 SQRT
-    //   $-\vec{f}_{\alpha \beta}(\vec{r}_{\alpha \beta})$
     //
-    // $= \frac{-V^0_{\alpha \beta}}{\sigma}e^{-b / \sigma} \grad_{\vec{r}_{\alpha \beta}}b$
+    // $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}).dt = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta})).\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}).dt$
     //
-    //
+    Vector2d f_alpha_beta_dt = f_alpha_beta_neg * w(alpha.e, f_alpha_beta_neg);
-    // $= \frac{-V^0_{\alpha \beta}}{2\sigma}e^{-b / \sigma} 2\grad_{\vec{r}_{\alpha \beta}}b$
-    //
-    Vector2d neg_f_ped = A * exp(two_b * NEG_ONE_DIV_TWO_SIGMA);
 
-    // # 4 ADDs, 7 MULTs, 1 SQRT
+    return f_alpha_beta_dt;
-    //
-    // $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta}) = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta})\vec{f}_{\alpha\beta}(\vec{r}_{\alpha} - \vec{r}_{\beta})$
-    //
-    p->f_i -= neg_f_ped * w(p->e, neg_f_ped);
-  }
 }
 
-// # board.n_obstacles * (12 ADDs, 18 MULTs, 1 DIV, 2 SQRTs, 1 EXP)
-//
-// $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$
-//
-void add_repulsive_effects_from_borders(Board &board, Pedestrian *p) {
 
-  Obstacle *obstacles = board.obstacles;
+// Flop count: 10 ADDs, 18 MULTs, 1 DIVs, 2 SQRTs, 1 EXP
-  int n_borders = board.n_obstacles;
+Vector2d obstacle_repulsion_dt(const Pedestrian &alpha, const Obstacle &B) {
-
-  for (int i = 0; i < n_borders; i++) {
-
-    Obstacle obstacle = obstacles[i];
 
     // # 7 ADDs, 5 MULTs
     // $\vec{r}_{\alpha B}$
-    Vector2d r_aB = p->curr_pos - obstacle.find_nearest_obstacle_point(p->curr_pos);
+    Vector2d r_aB = alpha.curr_pos - B.find_nearest_obstacle_point(alpha.curr_pos);
 
     // # 1 ADD, 2 MULTs, 1 SQRT
     // $\Arrowvert \vec{r}_{\alpha B} \Arrowvert$
     double r_aB_norm = r_aB.norm();
+    r_aB_norm = r_aB_norm > EPS ? r_aB_norm : EPS; // Avoid division by zero
 
-    // # 4 MULTs, 1 DIV, 1 EXP
+    // # 2 MULTs, 1 DIV, 1 EXP
+    //
+    // $\frac{-U^0_{\alpha B}}{R} e^{- \Arrowvert \vec{r}_{\alpha B} \Arrowvert / R}$
+    //
+    double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm;
+
+    // # 2 MULTs
     //
     // $-\vec{F}_{\alpha B}(\vec{r}_{\alpha B}) = \frac{-U^0_{\alpha B}}{R} e^{- \Arrowvert \vec{r}_{\alpha B} \Arrowvert / R} \frac{\vec{r}_{\alpha B}}{\Arrowvert \vec{r}_{\alpha B} \Arrowvert}$
     //
-    Vector2d neg_f_obst = r_aB * (NEG_U0_AB_DIV_R * exp(r_aB_norm * NEG_R_INV) / r_aB_norm);
+    Vector2d f_alpha_B_neg = r_aB * scalar_mult;
 
-    // # 4 ADDs, 7 MULTs, 1 SQRT
+    // # 2 ADDs, 7 MULTs, 1 SQRT
     //
-    // $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{ B}) = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B})\vec{F}_{\alpha B}(\vec{r}_{\alpha} - \vec{r}_{ B})$
+    // $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}).dt = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B})).\vec{F}_{\alpha B}(\vec{r}_{\alpha B}).dt$
     //
-    p->f_i -= neg_f_obst * w(p->e, neg_f_obst);
+    Vector2d f_alpha_B_dt = f_alpha_B_neg * w(alpha.e, f_alpha_B_neg);
+
+    return f_alpha_B_dt;
   }
-}

forces.hpp

@@ -1,13 +1,110 @@
 #pragma once
 #include "board.hpp"
 #include "pedestrian.hpp"
+#include "vector2d.hpp"
 
+// Note: force contributions are multiplied by dt inside each function because
+// this allows us to bake dt into precomputed constants and thus save two
+// multiplications in the main loop. Conceptually, each returned term is the
+// explicit-Euler approximation of the time integral of one force contribution
+// over a single simulation step.
+//
+// This is valid because integration is linear: the integral of a sum is the sum
+// of the integrals. Thus, summing F_i * dt for each contribution is equivalent
+// to computing dt * sum(F_i) after all force contributions have been accumulated.
+
+// Computes the direction-dependent perception weight from Eq. (7) in the paper:
+//
+//   $w(\vec{e}_{\alpha}, \vec{f}) = \begin{cases} 1.0, & \text{if } \vec{e}_{\alpha} \cdot \vec{f} \geq \|\vec{f}\| \cos(\varphi), \\ c, & \text{otherwise}. \end{cases}$
+//
+// where:
+//   - $\vec{e}_{\alpha}$ is the desired direction of motion of the pedestrian,
+//   - $\vec{f}$ is the force under consideration (e.g., the repulsive force from another pedestrian or an obstacle),
+//   - $\varphi$ is half of the effective angle of sight,
+//   - $c$ is the reduced weight factor for situations outside the field of view.
+//
+// The function returns:
+//
+//   $w(\vec{e}_{\alpha}, \vec{f}).(-dt)$
+//
+// Note: the reason we multiply the perception weight by -dt is:
+//     (1) the factor dt saves a multiplication by dt, in pedestrian_repulsion_dt and obstacle_repulsion_dt;
+//     (2) the minus sign is applied because the repulsion routines (pedestrian_repulsion_dt and obstacle_repulsion_dt) compute and
+//     pass the negated force direction to w(), matching the paper's w(e_alpha, -f) term. Returning -w() then converts that internally
+//     negated force back to the correct signed result vector without needing to perform any vector negation, in order to meet the paper's formula.
+//
+// @param e : The desired direction of motion of the pedestrian.
+// @param f : The force under consideration (e.g., the repulsive force from another pedestrian or an obstacle).
+// @return The direction-dependent perception weight multiplied by -dt.
+//
 double w(Vector2d e, Vector2d f);
 
-double g(double v_max_alpha, double w_alpha_norm);
+// Computes the acceleration term of the social force model
+// for pedestrian alpha (Eq. (2) in the paper):
+//
+//   $\vec{F}^0_\alpha = \frac{1}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
+//
+// where:
+//   - $v^0_\alpha$ is the desired speed,
+//   - $\vec{e}_\alpha$ is the desired direction,
+//   - $\vec{v}_\alpha$ is the current velocity,
+//   - $\tau_\alpha$ is the relaxation time.
+//
+// This function returns $\vec{F}^0_\alpha.dt$, i.e. the velocity increment caused by
+// the acceleration term during one simulation time step:
+//
+//   $\vec{F}^0_\alpha.dt = \frac{dt}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
+//
+// @param alpha : The pedestrian for which the acceleration contribution is computed.
+// @return The velocity increment caused by the acceleration term during one simulation time step.
+//
+Vector2d acceleration_term_dt(const Pedestrian &alpha);
 
-void add_acceleration_term(Pedestrian *p);
+// Computes the repulsive pedestrian-pedestrian effect
+// exerted by pedestrian beta on pedestrian alpha (Eq. (3) in the paper),
+// weighted by the direction-dependent perception factor from Eq. (7)-(8):
+//
+//   $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}) = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta})$
+//
+// where:
+//   - $\vec{r}_{\alpha \beta} = \vec{r}_\alpha - \vec{r}_\beta$ is the vector from beta's position to alpha's position,
+//   - $\vec{e}_\alpha$ is the desired direction of pedestrian alpha,
+//
+//   - $\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}) = -\nabla_{\vec{r}_{\alpha\beta}} V_{\alpha\beta}\left[b(\vec{r}_{\alpha\beta})\right]$ is the repulsive effect from beta on alpha (The expanded formula details are given in the code comments given inside the function's implementation in forces.cpp),
+//
+//   - $w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))$ the direction-dependent perception weighting function, for which the formula is detailed in the w() function above.
+//
+// This function returns $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}).dt$, i.e. the velocity increment caused by the repulsive pedestrian-pedestrian effect during one simulation time step:
+//
+//   $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}).dt = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}).dt$
+//
+// @param alpha : The pedestrian receiving the repulsive effect.
+// @param beta : The pedestrian producing the repulsive effect.
+// @return The velocity increment caused by the repulsive effect from pedestrian beta on alpha during one
+//         simulation time step.
+//
+Vector2d pedestrian_repulsion_dt(const Pedestrian &alpha, const Pedestrian &beta);
 
-void add_repulsive_effects_from_pedestrians(Pedestrian *P, Pedestrian *p, int N);
+// Computes the repulsive effect exerted by obstacle B
-
+// on pedestrian alpha (Eq. (5) in the paper), weighted by the direction-dependent perception factor from Eq. (7)-(8]:
-void add_repulsive_effects_from_borders(Board &B, Pedestrian *p);
+//
+//  $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}) = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))\vec{F}_{\alpha B}(\vec{r}_{\alpha B})$
+//
+// where:
+//  - $\vec{r}_{\alpha B} = \vec{r}_\alpha - \vec{r}^{\,\alpha}_B$ is the vector from the closest point on obstacle B (i.e., $\vec{r}^{\,\alpha}_B$) to pedestrian alpha's position,
+//  - $\vec{e}_\alpha$ is the desired direction of pedestrian alpha,
+//
+//  - $\vec{F}_{\alpha B}(\vec{r}_{\alpha B}) = -\nabla_{\vec{r}_{\alpha B}} U_{\alpha B}(||\vec{r}_{\alpha B}||)$ is the repulsive effect from obstacle B on alpha (The expanded formula details are given in the code comments given inside the function's implementation in forces.cpp),
+//
+//  - $w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))$ the direction-dependent perception weighting function, for which the formula is detailed in the w() function above.
+//
+// This function returns $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}).dt$, i.e. the velocity increment caused by the repulsive obstacle effect during one simulation time step:
+//
+//  $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}).dt = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))\vec{F}_{\alpha B}(\vec{r}_{\alpha B}).dt$
+//
+// @param alpha : The pedestrian receiving the obstacle repulsive effect.
+// @param B : The obstacle producing the repulsive effect.
+// @return The velocity increment caused by the repulsive effect from obstacle B on alpha during one
+//         simulation time step.
+//
+Vector2d obstacle_repulsion_dt(const Pedestrian &alpha, const Obstacle &B);

main.cpp

@@ -1,15 +1,18 @@
 #include "board.hpp"
 #include "constants.hpp"
-#include "forces.hpp"
-#include "math.h"
 #include "pedestrian.hpp"
-#include "vector2d.hpp"
+#include "social_force.hpp"
-#include <fstream>
+
-#include <iostream>
+#include <algorithm>
-#include <random>
+#include <cstdio>
-#include <tuple>
+#include <cstdlib>
 #include <vector>
 
+#ifndef BENCHMARK
+// Benchmarking is disabled by default.
+#define BENCHMARK 0
+#endif
+
 #ifdef __x86_64__
 #include "tsc_x86.h"
 #endif
@@ -18,98 +21,25 @@
 #include "kperf.h"
 #endif
 
-#define SEED 42
+#define N_OBSTACLES 2
-
+#define WARMUP_ITERS 5
-#define BOARD_WIDTH 10.0
+#define NUM_REPS 11
-#define BOARD_LENGTH 50.0
-
-#define BOARD_LENGTH_MINUS_ONE BOARD_LENGTH - 1
-#define BOARD_LENGTH_MINUS_ONE_POINT_FIVE BOARD_LENGTH - 1.5
-#define WALL_EPS 1e-3
-#define BOARD_WIDTH_MINUS_WALL_EPS BOARD_WIDTH - WALL_EPS
-#define BOARD_LENGTH_HALF BOARD_LENGTH / 2
-#define BOARD_LENGTH_MINUS_TWO BOARD_LENGTH - 2
-#define BOARD_WIDTH_MINUS_ZERO_POINT_THREE BOARD_WIDTH - 0.3
-#define BOARD_LENGTH_PLUS_THIRTY BOARD_LENGTH + 30
-
-using namespace std;
-
-Pedestrian *init_pedestrians(int n_p) {
-
-  Pedestrian *P = new Pedestrian[n_p];
-
-  // Platform-specific non-deterministic random number generator (if available)
-  // random_device rd;
-  // mt19937 gen(rd()); // Use rd for true randomness
-
-  mt19937 gen(SEED); // Use fixed seed for reproducibility
-
-  normal_distribution<double> velocity_gen(V0_MEAN, V0_STD_DEV);
-  uniform_real_distribution<double> x_gen_left(2, BOARD_LENGTH_HALF);
-  uniform_real_distribution<double> x_gen_right(BOARD_LENGTH_HALF, BOARD_LENGTH_MINUS_TWO);
-  uniform_real_distribution<double> y_gen(0.3, BOARD_WIDTH_MINUS_ZERO_POINT_THREE);
-
-  const int n_p_half = n_p / 2;
-
-  for (int i = 0; i < n_p_half; i++) {
-
-    double x = x_gen_left(gen);
-    double y = y_gen(gen);
-
-    Vector2d start_pos = Vector2d(x, y);
-    Vector2d destination = Vector2d(BOARD_LENGTH_PLUS_THIRTY, y);
-
-    Vector2d init_e = destination - start_pos;
-    double init_e_norm = init_e.norm();
-    Vector2d init_e_unit = init_e / init_e_norm;
-
-    double desired_v0;
-
-    // Avoid values are too small since they would make the pedestrians walk backwards
-    do {
-      desired_v0 = velocity_gen(gen);
-    } while (desired_v0 <= 0.1);
-
-    double v_max = 1.3 * desired_v0;
-
-    P[i] = Pedestrian(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination, Vector2d());
-  }
-
-  for (int i = n_p_half; i < n_p; i++) {
-
-    double x = x_gen_right(gen);
-    double y = y_gen(gen);
-
-    Vector2d start_pos = Vector2d(x, y);
-    Vector2d destination = Vector2d(-30, y);
-
-    Vector2d init_e = destination - start_pos;
-    double init_e_norm = init_e.norm();
-    Vector2d init_e_unit = init_e / init_e_norm;
-
-    double desired_v0;
-    do {
-      desired_v0 = velocity_gen(gen);
-    } while (desired_v0 <= 0.1);
-
-    double v_max = 1.3 * desired_v0;
-
-    P[i] = Pedestrian(BLACK, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination, Vector2d());
-  }
-
-  return P;
-}
 
 int main(int argc, const char *argv[]) {
-
+  // Check that the correct number of arguments has been passed.
-  // Ensure that arguments are passed
+  //
+  // Expected usage:
+  //
+  //   ./main <number_of_pedestrians> <number_of_iterations> [csv_output_path]
+  //
+  // The optional third argument enables CSV dumping for visualization.
   if (argc < 3) {
-    printf("Usage: ./sim <number_of_pedestrians> <number_of_iterations>\n");
+    printf("Usage: ./main <number_of_pedestrians> <number_of_iterations> [csv_output_path]\n");
-    printf("Each iteration takes %.2f seconds in the simulation.\n", DT);
+    printf("Each iteration takes %.2f seconds in the simulation.\n", dt);
     return 1;
   }
 
-  // Parse command-line arguments
+  // Parse command-line arguments.
   int n_p = atoi(argv[1]);
   int n_iter = atoi(argv[2]);
 
@@ -123,167 +53,118 @@ int main(int argc, const char *argv[]) {
     return 1;
   }
 
-  Obstacle obs[] = {};
+  // If provided, dump_path is used to write simulation states to CSV.
-  Board board = Board(BOARD_LENGTH, BOARD_WIDTH, 0, obs);
+  // In benchmark mode, this path is intentionally ignored to avoid measuring
-
+  // file I/O overhead.
-  // Logic for importing/exporting the simulation data
-  const int dump_interval = 1;
   const char *dump_path = (argc >= 4) ? argv[3] : nullptr;
 
-  vector<tuple<int, int, double, double, double, double, int>> samples; // t, id, x, y, e.x, e.y, color
+#if BENCHMARK
-  if (dump_path) {
-    samples.reserve((size_t)n_iter / dump_interval * n_p);
-  }
-
-  Pedestrian *pedestrians = init_pedestrians(n_p);
-
-#ifdef __x86_64__
-  uint64_t start = start_tsc();
-#endif
 
 #ifdef __aarch64__
+  // Initialize ARM performance counters.
   kperf_init();
   struct performance_counters startperf, endperf;
-  startperf = kperf_get_counters();
 #endif
 
-  for (int t = 0; t < n_iter; t++) {
+  // Warmup runs are not measured.
+  // Each run gets a fresh initialized state because run_simulation mutates the
+  // pedestrians and board state.
+  for (int i = 0; i < WARMUP_ITERS; i++) {
+    Pedestrian *P = init_pedestrians(n_p);
+    Board board = init_board_without_obstacles();
 
-    // Pass 1: Conmpute all the forces given the current state
+    run_simulation(P, board, n_p, n_iter, nullptr);
-    for (int i = 0; i < n_p; i++) {
 
-      Pedestrian &p = pedestrians[i];
+    delete[] P;
-
-      p.f_i.reset();
-
-      // # 4 ADDs, 4 MULTs
-      //
-      // $\vec{F}_\alpha(\vec{v}_\alpha, v^0_\alpha \vec{e}_\alpha) = \frac{1}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
-      //
-      add_acceleration_term(&p);
-
-      // # (n_p - 1) * (14 ADDs, 23 MULTs, 1 DIV, 4 SQRTs, 1 EXP)
-      //
-      // $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$
-      //
-      add_repulsive_effects_from_pedestrians(pedestrians, &p, n_p);
-
-      // # board.n_obstacles * (12 ADDs, 18 MULTs, 1 DIV, 2 SQRTs, 1 EXP)
-      //
-      // $\sum_{B} \vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$
-      //
-      add_repulsive_effects_from_borders(board, &p);
-    }
-
-    // Pass 2: Integrate the forces and update the current state
-    for (int i = 0; i < n_p; i++) {
-
-      Pedestrian &p = pedestrians[i];
-
-      // # 2 ADDs
-      //
-      // $\frac{d\vec{w_\alpha}}{dt} = \vec{F}_{\alpha}(t)$
-      //
-      //
-      p.w = p.w + p.f_i;
-
-      // # 1 ADD, 2 MULTs, 1 SQRT
-      double w_norm = p.w.norm();
-
-      p.curr_vel = p.w;
-
-      // # Assumption for calculating W(n): The "if" statement always evaluates to true.
-      // # This is not true in practice, but we assume the worst-case scenario for the flop count calculation.
-      if (w_norm > p.v_max) {
-
-        // # 2 MULTs, 1 DIV
-        p.curr_vel = p.curr_vel * (p.v_max / w_norm);
-      }
-
-      // # 2 ADDs, 2 MULTs
-      //
-      // $\vec{r}_i^{t+1} = \vec{r}^t_i + v^t_idt$
-      //
-      p.curr_pos = p.curr_pos + p.curr_vel * DT;
-
-      // When a pedestrian reaches the end of the hallway we simply "teleport" the person back to the beginning.
-      // This way, we have a constant flow of pedestrians, and it is easier to see lanes forming.
-      if (p.color == WHITE && p.curr_pos.x > BOARD_LENGTH_MINUS_ONE_POINT_FIVE) {
-
-        // keep y, reset x
-        p.curr_pos.x = 1.0;
-        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_MINUS_ONE;
-        p.destination.y = p.curr_pos.y;
-        p.w = Vector2d(-p.desired_v0, 0);
-        p.curr_vel = p.w;
-      }
-
-      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_MINUS_WALL_EPS;
-        if (p.curr_vel.y > 0)
-          p.curr_vel.y = 0;
-        if (p.w.y > 0)
-          p.w.y = 0;
-      }
-
-      // # 2 ADDs
-      Vector2d e = p.destination - p.curr_pos;
-
-      // # 1 ADD, 2 MULTs, 1 DIV, 1 SQRT
-      double e_norm_inv = 1.0 / e.norm();
-
-      // # 2 MULTs
-      p.e = e * e_norm_inv;
-
-      // # 1 ADD, 3 MULTs, 1 SQRT
-      double s_b = p.curr_vel.norm() * DELTA_T;
-
-      // # 2 MULTs
-      p.s_b_e_b = p.e * s_b;
-
-      // # 1 MULT
-      p.s_b_sq = s_b * s_b;
-
-      // Save the data for visualization
-      if (dump_path && (t % dump_interval == 0)) {
-        samples.emplace_back(t, i, p.curr_pos.x, p.curr_pos.y, p.e.x, p.e.y, p.color);
-      }
-    }
   }
 
+  // Store the cycle count of each measured repetition.
+  // The median is then used as the final timing result.
+  std::vector<long double> cycles_v;
+  cycles_v.reserve(NUM_REPS);
+
+  for (int i = 0; i < NUM_REPS; i++) {
+    Pedestrian *P = init_pedestrians(n_p);
+    Board board = init_board_without_obstacles();
+
 #ifdef __x86_64__
-  long double T = stop_tsc(start);
+    myInt64 start = start_tsc();
 #endif
 
 #ifdef __aarch64__
-  endperf = kperf_get_counters();
+    startperf = kperf_get_counters();
-  long double T = endperf.cycles - startperf.cycles;
 #endif
 
-  long long add_count_per_iter = n_p * (4 + 14 * (n_p - 1) + 12 * board.n_obstacles) + n_p * 9;
+    // Disable CSV dumping during benchmarking to avoid measuring file I/O
-  long long mult_count_per_iter = n_p * (4 + 23 * (n_p - 1) + 18 * board.n_obstacles) + n_p * 16;
+    // overhead. Initialization is also excluded from the measured region.
-  long long div_count_per_iter = n_p * (n_p - 1 + board.n_obstacles) + n_p * 2;
+    run_simulation(P, board, n_p, n_iter, nullptr);
-  long long sqrt_count_per_iter = n_p * (4 * (n_p - 1) + 2 * board.n_obstacles) + n_p * 3;
-  long long exp_count_per_iter = n_p * (n_p - 1 + board.n_obstacles);
 
-  long double W_per_iter = add_count_per_iter * COST_ADD + mult_count_per_iter * COST_MULT + div_count_per_iter * COST_DIV + sqrt_count_per_iter * COST_SQRT + exp_count_per_iter * COST_EXP;
+#ifdef __x86_64__
-  long long Q_per_iter = sizeof(Pedestrian) * n_p + sizeof(Obstacle) * board.n_obstacles; // TODO: Refine this
+    long double cycles = stop_tsc(start);
+#endif
+
+#ifdef __aarch64__
+    endperf = kperf_get_counters();
+    long double cycles = endperf.cycles - startperf.cycles;
+#endif
+
+    delete[] P;
+
+    cycles_v.push_back(cycles);
+  }
+
+  // Use the median cycle count to reduce the effect of noisy measurements.
+  std::sort(cycles_v.begin(), cycles_v.end());
+  long double T = cycles_v[NUM_REPS / 2];
+
+#else
+
+  // Normal execution mode. If dump_path is provided, the simulation writes a CSV
+  // file that can later be used to generate a video of the simulation.
+  Pedestrian *P = init_pedestrians(n_p);
+  Board board = init_board_without_obstacles();
+  run_simulation(P, board, n_p, n_iter, dump_path);
+
+  delete[] P;
+
+#endif
+
+#if BENCHMARK
+
+  // Flop-count per simulation iteration.
+  //
+  // The total work W(n) is estimated from the number of additions,
+  // multiplications, divisions, square roots, and exponentials performed by the
+  // simulation kernels.
+  long long add_count_per_iter = n_p * (4 + (n_p - 1) * 14 + N_OBSTACLES * 12) + n_p * 7;
+
+  long long mult_count_per_iter = n_p * (4 + (n_p - 1) * 23 + N_OBSTACLES * 18) + n_p * 16;
+
+  long long div_count_per_iter = n_p * ((n_p - 1) * 1 + N_OBSTACLES * 1) + n_p * 2;
+
+  long long sqrt_count_per_iter = n_p * ((n_p - 1) * 4 + N_OBSTACLES * 2) + n_p * 3;
+
+  long long exp_count_per_iter = n_p * ((n_p - 1) * 1 + N_OBSTACLES * 1);
+
+  long double W_per_iter = add_count_per_iter * cost_add + mult_count_per_iter * cost_mult + div_count_per_iter * cost_div + sqrt_count_per_iter * cost_sqrt + exp_count_per_iter * cost_exp;
+
+  // Estimated memory traffic per iteration.
+  // TODO: refine this model to account for actual memory accesses, cache reuse,
+  // and the full interaction loop.
+  long long Q_per_iter = sizeof(Pedestrian) * n_p + sizeof(Obstacle) * N_OBSTACLES;
+
+  // Operational intensity:
+  //
+  //   I(n) = W(n) / Q(n)
+  //
+  // where W is estimated work in flops and Q is estimated memory traffic in
+  // bytes.
   long double I = W_per_iter / Q_per_iter;
+
+  // Total estimated work over all iterations.
   long double W = W_per_iter * n_iter;
+
+  // Performance in estimated flops per cycle.
   long double P = W / T;
 
   printf("W(n) per iteration: %.2Lf flops\n", W_per_iter);
@@ -293,15 +174,7 @@ int main(int argc, const char *argv[]) {
   printf("T(n): %.2Lf cycles\n", T);
   printf("P(n): %.2Lf flops/cycle\n", P);
 
-  // Export the data to a .csv file
+#endif
-  if (dump_path) {
-    ofstream out(dump_path);
-    out << "time,id,x,y,p.e.x,p.e.y,color\n";
-    for (auto &[t, id, x, y, e_x, e_y, c] : samples) {
-      out << t << ',' << id << ',' << x << ',' << y << ',' << e_x << ',' << e_y << ',' << c << '\n';
-    }
-  }
 
-  delete[] pedestrians;
   return 0;
-}
+}

pedestrian.hpp

@@ -2,10 +2,10 @@
 #include "constants.hpp"
 #include "vector2d.hpp"
 
-// For visual simulation.
+// Needed for visual simulation, in order to distinguish two groups of pedestrians:
-// Black pedestrians move from left to right;
+//    - White pedestrians move from left to right
-// White pedestrians move from right to left.
+//    - Black pedestrians move from right to left
-enum Color { BLACK, WHITE };
+enum Color { WHITE, BLACK};
 
 struct Pedestrian {
 
@@ -31,17 +31,35 @@ struct Pedestrian {
 
   // $\vec{r}^0_\alpha$
   Vector2d destination;
-
-  // social force
-  Vector2d f_i;
-
   // $s_\beta^2 = (v_\beta \Delta t)^2$
   double s_b_sq;
+  // $s_\beta \vec{e}_\beta$
+  Vector2d s_b_e;
 
-  // $s_\beta \vec{e}_\beta = v_{\beta} \Delta t \vec{e}_{\beta}$
+  // Default constructor: initializes a pedestrian with color WHITE and all vector/scalar fields set to zero.
-  Vector2d s_b_e_b;
+  Pedestrian() : color(WHITE), curr_vel(0.0, 0.0), w(0.0, 0.0), curr_pos(0.0, 0.0), e(0.0, 0.0), v_max(0.0), desired_v0(0.0), destination(0.0, 0.0), s_b_sq(0.0), s_b_e(0.0, 0.0) {}
 
-  Pedestrian() : color(WHITE), curr_vel(0.0, 0.0), w(0.0, 0.0), curr_pos(0.0, 0.0), e(0.0, 0.0), v_max(0.0), desired_v0(0.0), destination(0.0, 0.0), f_i(0.0, 0.0), s_b_sq(0.0), s_b_e_b(0.0, 0.0) {}
+  // Constructs a pedestrian with the given initial state and precomputes the
-
+  // quantities used in the pedestrian-pedestrian repulsion term.
-  Pedestrian(Color clr, Vector2d c_v, Vector2d c_p, Vector2d e, double v_max, double desired_v0, Vector2d d, Vector2d f) : color(clr), curr_vel(c_v), w(c_v), curr_pos(c_p), e(e), v_max(v_max), desired_v0(desired_v0), destination(d), f_i(f), s_b_sq(c_v.dot(c_v) * DELTA_T_SQ), s_b_e_b(e * c_v.norm() * DELTA_T) {}
+  //
+  // The preferred velocity w is initialized to the current velocity c_v. The
+  // cached quantities s_b_sq and s_b_e correspond to the predicted step of this
+  // pedestrian over $\Delta t$:
+  //
+  //   s_b     = ||c_v|| * dt
+  //   s_b_sq  = s_b^2
+  //   s_b_e   = e * s_b
+  //
+  // These values are used when another pedestrian computes the elliptical
+  // interaction term involving this pedestrian.
+  //
+  // @param clr The display/color category of the pedestrian.
+  // @param c_v The initial current velocity.
+  // @param c_p The initial current position.
+  // @param e The initial desired direction of motion.
+  // @param v_max The maximum speed that the pedestrian can reach.
+  // @param desired_v0 The desired walking speed.
+  // @param d The pedestrian's target destination point.
+  //
+  Pedestrian(Color clr, Vector2d c_v, Vector2d c_p, Vector2d e, double v_max, double desired_v0, Vector2d d) : color(clr), curr_vel(c_v), w(c_v), curr_pos(c_p), e(e), v_max(v_max), desired_v0(desired_v0), destination(d), s_b_sq(c_v.dot(c_v) * delta_t_sq), s_b_e(e * c_v.norm() * delta_t) {}
 };

social_force.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include "board.hpp"
+#include "pedestrian.hpp"
+
+// Initializes the pedestrians used in the simulation.
+//
+// The returned array contains n_p pedestrians split into two groups:
+// one group walking from left to right and one group walking from right to left.
+// Pedestrian positions, desired speeds, destinations, and initial directions are
+// initialized according to the simulation setup.
+//
+// @param n_p : Number of pedestrians to initialize.
+// @return Pointer to a dynamically allocated array of pedestrians.
+Pedestrian *init_pedestrians(int n_p);
+
+// Initializes a simulation board with no obstacles inside.
+//
+// @return Initialized Board instance.
+Board init_board_without_obstacles();
+
+// Runs the social force model with n_p pedestrians for n_iter iterations,
+// i.e., n_iter * dt simulated seconds.
+// If dump_path is not nullptr, the simulation states are written to that CSV
+// file, which can be used to generate a visualization video. If dump_path is
+// nullptr, no output file is generated.
+//
+// @param pedestrians : Array of pedestrians to simulate. The array is updated in place.
+// @param board : Board containing the simulation dimensions and obstacles.
+// @param n_p : Number of pedestrians.
+// @param n_iter : Number of simulation iterations.
+// @param dump_path : Optional path to the CSV output file.
+// @return 0 on success.
+int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, const char *dump_path);

social_force_baseline.cpp

@@ -0,0 +1,253 @@
+#include "board.hpp"
+#include "constants.hpp"
+#include "forces.hpp"
+#include "pedestrian.hpp"
+#include "social_force.hpp"
+#include "vector2d.hpp"
+
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <random>
+#include <tuple>
+#include <vector>
+
+#define SEED 42
+
+#define BOARD_WIDTH 10.0
+#define BOARD_LENGTH 50.0
+#define WALL_EPS 1e-3
+#define BOARD_WIDTH_MINUS_WALL_EPS 9.999
+
+#ifndef RANDOM_SEED
+// Fixed seeding is used by default to make simulations reproducible.
+#define RANDOM_SEED 0
+#endif
+
+#ifndef BENCHMARK
+// Benchmarking is disabled by default.
+#define BENCHMARK 0
+#endif
+
+using namespace std;
+
+Pedestrian *init_pedestrians(int n_p) {
+#if RANDOM_SEED
+  random_device rd;  // Platform-provided random seed source.
+  mt19937 gen(rd()); // Non-reproducible initialization.
+#else
+  mt19937 gen(SEED); // Reproducible initialization.
+#endif
+
+  normal_distribution<double> velocity_gen(v0_mean, v0_std_dev);
+  uniform_real_distribution<double> y_gen(WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS);
+
+  Pedestrian *P = new Pedestrian[n_p];
+
+  int n_p_half = n_p / 2;
+
+  // Group of pedestrians walking from left to right.
+  for (int i = 0; i < n_p_half; i++) {
+    double x = 0.0;
+    double y = y_gen(gen);
+
+    Vector2d start_pos = Vector2d(x, y);
+    Vector2d destination = Vector2d(BOARD_LENGTH, y);
+
+    Vector2d init_e = destination - start_pos;
+    double init_e_norm = init_e.norm();
+    Vector2d init_e_unit = init_e / init_e_norm;
+
+    // Reject near-zero desired speeds to avoid pedestrians that barely move,
+    // and reject negative speeds because they would reverse the intended
+    // walking direction.
+    double desired_v0;
+    do {
+      desired_v0 = velocity_gen(gen);
+    } while (desired_v0 <= 0.1);
+
+    double v_max = 1.3 * desired_v0;
+
+    P[i] = Pedestrian(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination);
+  }
+
+  // Group of pedestrians walking from right to left.
+  for (int i = n_p_half; i < n_p; i++) {
+    double x = BOARD_LENGTH;
+    double y = y_gen(gen);
+
+    Vector2d start_pos = Vector2d(x, y);
+    Vector2d destination = Vector2d(0.0, y);
+
+    Vector2d init_e = destination - start_pos;
+    double init_e_norm = init_e.norm();
+    Vector2d init_e_unit = init_e / init_e_norm;
+
+    // Reject near-zero desired speeds to avoid pedestrians that barely move,
+    // and reject negative speeds because they would reverse the intended
+    // walking direction.
+    double desired_v0;
+    do {
+      desired_v0 = velocity_gen(gen);
+    } while (desired_v0 <= 0.1);
+
+    double v_max = 1.3 * desired_v0;
+
+    P[i] = Pedestrian(BLACK, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination);
+  }
+
+  return P;
+}
+
+Board init_board_without_obstacles() { return Board(BOARD_LENGTH, BOARD_WIDTH, 0, nullptr); }
+
+int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, const char *dump_path) {
+
+#if BENCHMARK
+  // Avoid compiler unused parameter warning
+  (void)dump_path;
+#endif
+
+#if !BENCHMARK
+  const int dump_interval = 1;
+
+  // Each sample stores: time step, pedestrian id, x, y, e.x, e.y, color.
+  vector<tuple<int, int, double, double, double, double, int>> samples;
+
+  if (dump_path) {
+    samples.reserve(static_cast<size_t>(n_iter / dump_interval + 1) * n_p);
+
+    // Store the initial state before the first simulation step.
+    for (int i = 0; i < n_p; i++) {
+      Pedestrian &p = pedestrians[i];
+      samples.emplace_back(0, i, p.curr_pos.x, p.curr_pos.y, p.e.x, p.e.y, p.color);
+    }
+  }
+#endif
+
+  for (int t = 0; t < n_iter; t++) {
+    // Pass 1: Compute all forces from the current state.
+    for (int i = 0; i < n_p; i++) {
+      Pedestrian &p = pedestrians[i];
+
+      // # 2 ADDs, 4 MULTs
+      //
+      // $\vec{F}_\alpha(\vec{v}_\alpha, v^0_\alpha \vec{e}_\alpha) = \frac{1}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
+      //
+      Vector2d f_alpha_dt = acceleration_term_dt(p);
+
+      // # (N-1) * (12 ADDs, 23 MULTs, 1 DIV, 4 SQRTs, 1 EXP)
+      //   + (N-1) * 2 ADDs
+      //
+      // $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha},\vec{r}_{\alpha} - \vec{r}_{\beta})$
+      //
+      for (int j = 0; j < n_p; j++) {
+        if (i == j)
+          continue;
+
+        f_alpha_dt += pedestrian_repulsion_dt(p, pedestrians[j]);
+      }
+
+      // # board.n_obstacles * (10 ADDs, 18 MULTs, 1 DIV, 2 SQRTs, 1 EXP)
+      //   + board.n_obstacles * 2 ADDs
+      //
+      // $\sum_{B} \vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$
+      //
+      for (int k = 0; k < board.n_obstacles; k++) {
+        f_alpha_dt += obstacle_repulsion_dt(p, board.obstacles[k]);
+      }
+
+      // # 2 ADDs
+      //
+      //
+      // $\frac{d\vec{w}_\alpha}{dt} = \vec{F}_{\alpha}(t)$
+      //
+      p.w += f_alpha_dt;
+    }
+
+    // Pass 2: Update pedestrian states after all forces have been computed.
+    // # (7 ADDs, 16 MULTs, 2 DIVs, 3 SQRTs) * n_p
+    for (int i = 0; i < n_p; i++) {
+      Pedestrian &p = pedestrians[i];
+
+      // # 1 ADD, 2 MULTs, 1 SQRT
+      double w_norm = p.w.norm();
+      double w_norm2 = w_norm > EPS ? w_norm : EPS; // Avoid division by zero.
+
+      // # 1 DIV
+      double correction_factor = std::min(1.0, p.v_max / w_norm2);
+
+      // # 2 MULTs
+      p.curr_vel = p.w * correction_factor;
+
+      // # 2 ADDs, 2 MULTs
+      //
+      // $\vec{r}_i^{t+1} = \vec{r}_i^t + \vec{v}_i^t dt$
+      //
+      p.curr_pos = p.curr_pos + p.curr_vel * dt;
+
+      // When a pedestrian reaches the end of the corridor, teleport them back
+      // to the opposite entrance while keeping the same y-coordinate. This
+      // maintains a constant pedestrian flow and makes lane formation easier to
+      // observe.
+      if (p.color == WHITE && p.curr_pos.x >= BOARD_LENGTH) {
+        p.curr_pos.x = 0.0;
+        p.w = Vector2d();
+        p.curr_vel = Vector2d();
+      }
+      if (p.color == BLACK && p.curr_pos.x <= 0.0) {
+        p.curr_pos.x = BOARD_LENGTH;
+        p.w = Vector2d();
+        p.curr_vel = Vector2d();
+      }
+
+      // Keep pedestrians inside the board boundaries.
+      p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS);
+
+      // # 2 ADDs
+      Vector2d e = p.destination - p.curr_pos;
+
+      // # 1 ADD, 2 MULTs, 1 DIV, 1 SQRT
+      double e_norm = e.norm();
+      double e_norm2 = e_norm > EPS ? e_norm : EPS; // Avoid division by zero.
+      double e_norm_inv = 1.0 / e_norm2;
+
+      // # 2 MULTs
+      p.e = e * e_norm_inv;
+
+      // # 1 ADD, 2 MULTs
+      double velocity_norm_sq = p.curr_vel.dot(p.curr_vel);
+
+      // # 1 SQRT
+      double velocity_norm = std::sqrt(velocity_norm_sq);
+
+      // # 1 MULT
+      p.s_b_sq = velocity_norm_sq * delta_t_sq;
+
+      // # 3 MULTs
+      p.s_b_e = p.e * (velocity_norm * delta_t);
+
+#if !BENCHMARK
+      // Store the current pedestrian state for visualization.
+      if (dump_path && (t % dump_interval == 0)) {
+        samples.emplace_back(t + 1, i, p.curr_pos.x, p.curr_pos.y, p.e.x, p.e.y, p.color);
+      }
+#endif
+    }
+  }
+
+#if !BENCHMARK
+  // Export recorded simulation states to CSV.
+  if (dump_path) {
+    ofstream out(dump_path);
+    out << "time,id,x,y,p.e.x,p.e.y,color\n";
+
+    for (auto &[t, id, x, y, e_x, e_y, c] : samples) {
+      out << t << ',' << id << ',' << x << ',' << y << ',' << e_x << ',' << e_y << ',' << c << '\n';
+    }
+  }
+#endif
+
+  return 0;
+}

test_board.cpp

@@ -0,0 +1,53 @@
+#include "board.hpp"
+#include "test_helpers.hpp"
+
+#include <cmath>
+
+TEST(ObstacleNearest, ProjectionInsideSegment) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
+  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(5.0, 3.0));
+  EXPECT_VEC2_NEAR(nearest, Vector2d(5.0, 0.0));
+}
+
+TEST(ObstacleNearest, ProjectionBeforeStartIsClampedToStart) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
+  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(-5.0, 3.0));
+  EXPECT_VEC2_NEAR(nearest, Vector2d(0.0, 0.0));
+}
+
+TEST(ObstacleNearest, ProjectionAfterEndIsClampedToEnd) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
+  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(15.0, 3.0));
+  EXPECT_VEC2_NEAR(nearest, Vector2d(10.0, 0.0));
+}
+
+TEST(ObstacleNearest, QueryEqualToStartReturnsStart) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
+  EXPECT_VEC2_NEAR(o.find_nearest_obstacle_point(Vector2d(0.0, 0.0)), Vector2d(0.0, 0.0));
+}
+
+TEST(ObstacleNearest, QueryEqualToEndReturnsEnd) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0));
+  EXPECT_VEC2_NEAR(o.find_nearest_obstacle_point(Vector2d(10.0, 0.0)), Vector2d(10.0, 0.0));
+}
+
+TEST(ObstacleNearest, DiagonalSegment) {
+  Obstacle o(Vector2d(0.0, 0.0), Vector2d(4.0, 4.0));
+  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(4.0, 0.0));
+  EXPECT_VEC2_NEAR(nearest, Vector2d(2.0, 2.0));
+}
+
+TEST(ObstacleNearest, PointOnSegmentReturnsItself) {
+  Obstacle o(Vector2d(1.0, 1.0), Vector2d(5.0, 9.0));
+  Vector2d on_segment(3.0, 5.0);
+  Vector2d nearest = o.find_nearest_obstacle_point(on_segment);
+  EXPECT_VEC2_NEAR(nearest, on_segment);
+}
+
+TEST(ObstacleNearest, PointOnSinglePointObstacleReturnsStart) {
+  Obstacle o(Vector2d(7.0, 8.0), Vector2d(7.0, 8.0));
+  Vector2d nearest = o.find_nearest_obstacle_point(Vector2d(100.0, -50.0));
+  EXPECT_VEC2_NEAR(nearest, Vector2d(7.0, 8.0));
+  EXPECT_TRUE(std::isfinite(nearest.x));
+  EXPECT_TRUE(std::isfinite(nearest.y));
+}

test_forces.cpp

@@ -0,0 +1,419 @@
+// =============================================================================
+//  Tests for forces.cpp
+// =============================================================================
+
+#include "board.hpp"
+#include "constants.hpp"
+#include "forces.hpp"
+#include "pedestrian.hpp"
+#include "test_helpers.hpp"
+#include "vector2d.hpp"
+
+#include <algorithm>
+#include <cmath>
+
+namespace {
+
+// Build a pedestrian with default testing values so each test only has to override what it actually cares about.
+Pedestrian MakePed(Vector2d pos = Vector2d(0.0, 0.0), Vector2d vel = Vector2d(1.0, 0.0), Vector2d e = Vector2d(1.0, 0.0), double v_max = 1.5, double desired_v0 = 1.15, Vector2d dest = Vector2d(100.0, 0.0)) { return Pedestrian(WHITE, vel, pos, e, v_max, desired_v0, dest); }
+
+// One Euler step, mirroring social_force_baseline.cpp:
+//   w_new   = w + total_force_dt
+//   v_new   = w_new * min(1, v_max / ||w_new||)
+//   pos_new = pos + v_new * dt
+struct StepResult {
+  Vector2d w, v, pos;
+};
+
+StepResult IntegrateOneStep(const Pedestrian &p, Vector2d total_force_dt) {
+  Vector2d w_new = p.w + total_force_dt;
+  double w_norm = w_new.norm();
+  double cap = std::min(1.0, p.v_max / (w_norm > EPS ? w_norm : EPS));
+  Vector2d v_new = w_new * cap;
+  Vector2d pos_new = p.curr_pos + v_new * dt;
+  return {w_new, v_new, pos_new};
+}
+
+} // namespace
+
+// =============================================================================
+//  Tests for w(e, f)
+// =============================================================================
+
+TEST(WeightFn, ForceAlignedWithDirectionIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(1.0, 0.0)), dt_neg); }
+
+TEST(WeightFn, ForceOppositeToDirectionIsOutOfView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(-1.0, 0.0)), c_neg_times_dt); }
+
+TEST(WeightFn, PerpendicularForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 1.0), Vector2d(-1.0, 1.0)), dt_neg); }
+
+TEST(WeightFn, ZeroForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0)), dt_neg); }
+
+TEST(WeightFn, AngleSlightlyInsidePhiIsInView) {
+  Vector2d e(1.0, 0.0);
+  const double angle = 99.0 / 180.0 * M_PI;
+  Vector2d f(std::cos(angle), std::sin(angle));
+  EXPECT_DOUBLE_EQ(w(e, f), dt_neg);
+}
+
+TEST(WeightFn, AngleSlightlyOutsidePhiIsOutOfView) {
+  Vector2d e(1.0, 0.0);
+  const double angle = 101.0 / 180.0 * M_PI;
+  Vector2d f(std::cos(angle), std::sin(angle));
+  EXPECT_DOUBLE_EQ(w(e, f), c_neg_times_dt);
+}
+
+TEST(WeightFn, ScalingForceMagnitudeDoesNotChangeView) {
+  Vector2d e(1.0, 0.0);
+  Vector2d f(0.7, 0.7);
+  EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 1000.0));
+  EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 0.001));
+}
+
+// =============================================================================
+//  Tests for acceleration_term_dt(alpha)
+// =============================================================================
+
+TEST(AccelerationTerm, AtDesiredVelocityProducesNoForce) {
+  Vector2d e(1.0, 0.0);
+  double v0 = 1.5;
+  Pedestrian p = MakePed(Vector2d(), e * v0, e, 2.0, v0);
+  EXPECT_VEC2_NEAR_TOL(acceleration_term_dt(p), Vector2d(0.0, 0.0), 0.0);
+}
+
+TEST(AccelerationTerm, BelowDesiredVelocityAccelerates) {
+  Vector2d e(1.0, 0.0);
+  double v0 = 1.5;
+  Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, 0.0), e, 2.0, v0);
+  Vector2d expected = Vector2d(2.0, 0.0) * dt;
+  EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
+  EXPECT_GT(acceleration_term_dt(p).x, 0.0);
+}
+
+TEST(AccelerationTerm, AboveDesiredVelocityDecelerates) {
+  Vector2d e(1.0, 0.0);
+  double v0 = 1.5;
+  Pedestrian p = MakePed(Vector2d(), Vector2d(1.75, 0.0), e, 2.0, v0);
+  Vector2d expected = Vector2d(-0.5, 0.0) * dt;
+  EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
+  EXPECT_LT(acceleration_term_dt(p).x, 0.0);
+}
+
+TEST(AccelerationTerm, MovingBackwardProducesLargerForwardForceThanMovingForward) {
+  Vector2d e(1.0, 0.0);
+  Pedestrian forward = MakePed(Vector2d(3.0, 0.0), Vector2d(0.5, 0.0), e, 2.0, 1.5);
+  Pedestrian backward = MakePed(Vector2d(3.0, 0.0), Vector2d(-1.0, 0.0), e, 2.0, 1.5);
+  EXPECT_GT(acceleration_term_dt(backward).x, acceleration_term_dt(forward).x);
+}
+
+TEST(AccelerationTerm, DiagonalWithVelocity) {
+  Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, -0.25), Vector2d(1.0, 1.0), 5.0, 2.0);
+  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(3.0, 4.5) * dt);
+}
+
+TEST(AccelerationTerm, ZeroDesiredSpeed) {
+  Pedestrian p = MakePed(Vector2d(), Vector2d(0.7, -0.3), Vector2d(1.0, 0.0), 2.0, 0.0);
+  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(-1.4, 0.6) * dt);
+}
+
+TEST(AccelerationTerm, SidewaysVelocityProducesPerpendicularCorrection) {
+  Pedestrian p = MakePed(Vector2d(), Vector2d(1.0, 0.5), Vector2d(1, 0), 1.3, 1.0);
+  EXPECT_DOUBLE_EQ(acceleration_term_dt(p).x, 0.0);
+  EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(0.0, -1.0 * dt));
+}
+
+// =============================================================================
+//  Tests for pedestrian_repulsion_dt(alpha, beta)
+// =============================================================================
+
+TEST(PedRepulsion, DistantPedestriansProduceNegligibleForce) {
+  Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
+  Pedestrian b = MakePed(Vector2d(100.0, 0.0), Vector2d(-1.2, 0.0), Vector2d(-1.0, 0.0));
+  Vector2d f = pedestrian_repulsion_dt(a, b);
+  EXPECT_LT(f.norm(), 1e-30);
+}
+
+TEST(PedRepulsion, CloseIsStrongerThanFar) {
+  Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
+  Pedestrian close = MakePed(Vector2d(2.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
+  Pedestrian far = MakePed(Vector2d(8.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
+  EXPECT_GT(pedestrian_repulsion_dt(a, close).norm(), pedestrian_repulsion_dt(a, far).norm());
+}
+
+TEST(PedRepulsion, StationaryBetaInFront) {
+  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
+  Vector2d expected(-0.24971795 * dt, 0.0);
+  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
+}
+
+TEST(PedRepulsion, StationaryBetaToTheSide) {
+  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, -100));
+  Vector2d expected(0.0, -0.24971795 * dt);
+  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
+}
+
+TEST(PedRepulsion, StationaryBetaDiagonal) {
+  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, -100));
+  Vector2d expected(-0.04439144 * dt, -0.04439144 * dt);
+  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
+}
+
+TEST(PedRepulsion, HeadOnWithMovingBeta) {
+  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
+  EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), Vector2d((-0.02512853) * dt, 0.0));
+}
+
+TEST(PedRepulsion, OutOfViewAttenuatedByFactorC) {
+  Pedestrian a_front = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
+  Pedestrian a_back = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b_back = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
+
+  Vector2d f_in = pedestrian_repulsion_dt(a_front, b_front);
+  Vector2d f_out = pedestrian_repulsion_dt(a_back, b_back);
+
+  EXPECT_NEAR(f_in.norm() / f_out.norm(), 1.0 / -c_neg, tolerance);
+}
+
+TEST(PedRepulsion, ForceOnAlphaPointsAwayFromBeta) {
+  Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 1), 1.3, 1.0, Vector2d(100, 100));
+  Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
+  Vector2d f = pedestrian_repulsion_dt(a, b);
+  Vector2d to_b(1.0, 1.0);
+  EXPECT_LT(f.dot(to_b), 0.0);
+}
+
+TEST(PedRepulsion, OverlappingPedestriansProduceFiniteOutput) {
+  Pedestrian a = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian b = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, 0));
+  Vector2d f = pedestrian_repulsion_dt(a, b);
+  EXPECT_TRUE(std::isfinite(f.x));
+  EXPECT_TRUE(std::isfinite(f.y));
+}
+
+// =============================================================================
+//  Tests for obstacle_repulsion_dt(alpha, B)
+// =============================================================================
+
+TEST(ObstRepulsion, DistantObstacleProducesNegligibleForce) {
+  Pedestrian p = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
+  Obstacle o(Vector2d(100.0, -50.0), Vector2d(100.0, 50.0));
+  EXPECT_LT(obstacle_repulsion_dt(p, o).norm(), 1e-30);
+}
+
+TEST(ObstRepulsion, PerpendicularWallBelow) {
+  Pedestrian p = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));
+  Vector2d expected(0.0, 0.33689735 * dt);
+  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
+}
+
+TEST(ObstRepulsion, ClampedToEndpoint) {
+  Pedestrian p = MakePed(Vector2d(12, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(0, 0));
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+  Vector2d expected(0.0022699965 * dt, 0.0);
+  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
+}
+
+TEST(ObstRepulsion, ClampedToStart) {
+  Pedestrian p = MakePed(Vector2d(-3, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+  Vector2d expected(-0.0000152951 * dt, 0.0);
+  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
+}
+
+TEST(ObstRepulsion, OutOfViewAttenuatedByFactorC) {
+  Pedestrian p = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(0, 1), 1.3, 1.0, Vector2d(5, 100));
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+  Vector2d expected(0.0, 0.33689735 * dt * -c_neg);
+  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
+}
+
+TEST(ObstRepulsion, SinglePointObstacle) {
+  Pedestrian p = MakePed(Vector2d(2, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Obstacle pt(Vector2d(5, 0), Vector2d(5, 0));
+  Vector2d expected(-0.0000152951 * dt, 0.0);
+  EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, pt), expected);
+}
+
+TEST(ObstRepulsion, ForceOnAlphaPointsAwayFromObstacle) {
+  Pedestrian p = MakePed(Vector2d(5, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 2));
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+  Vector2d f = obstacle_repulsion_dt(p, wall);
+  EXPECT_GT(f.y, 0.0);
+  EXPECT_NEAR(f.x, 0.0, 1e-12);
+}
+
+TEST(ObstRepulsion, AlphaOnObstacleProducesFiniteOutput) {
+  Pedestrian p = MakePed(Vector2d(5, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+  Vector2d f = obstacle_repulsion_dt(p, wall);
+  EXPECT_TRUE(std::isfinite(f.x));
+  EXPECT_TRUE(std::isfinite(f.y));
+  EXPECT_VEC2_NEAR(f, Vector2d(0.0, 0.0));
+}
+
+// =============================================================================
+//  Integration tests
+// =============================================================================
+
+TEST(Integration, OnePedNoInteractions_AccelerationOnly) {
+  Pedestrian alpha = MakePed(Vector2d(2, 3), Vector2d(0.5, 0), Vector2d(1, 0), 1.3, 1.0);
+
+  Vector2d total = acceleration_term_dt(alpha);
+
+  Vector2d expected_total(dt, 0.0);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, Vector2d(0.52, 0.0));
+  EXPECT_VEC2_NEAR(step.v, Vector2d(0.52, 0.0));
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(2.0 + 0.52 * dt, 3.0));
+}
+
+TEST(Integration, OnePedOneStationaryBeta) {
+  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));
+
+  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);
+
+  double Fa0_x = dt_times_tau_alpha_inv;
+  double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
+  Vector2d expected_total(Fa0_x + Fab_x, 0.0);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, expected_total);
+  EXPECT_LT(step.w.norm(), alpha.v_max);
+  EXPECT_VEC2_NEAR(step.v, expected_total);
+  EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
+}
+
+TEST(Integration, OnePedOneWall) {
+  Pedestrian alpha = MakePed(Vector2d(0, 0.5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
+
+  Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, wall);
+
+  double Fa0_x = dt_times_tau_alpha_inv;
+  double FaB_y = (U0_AB / R) * std::exp(-0.5 / R) * dt;
+  Vector2d expected_total(Fa0_x, FaB_y);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, expected_total);
+  EXPECT_LT(step.w.norm(), alpha.v_max);
+  EXPECT_VEC2_NEAR(step.v, expected_total);
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(0, 0.5) + expected_total * dt);
+}
+
+TEST(Integration, AllThreeForcesCombined) {
+  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));
+  Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));
+
+  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta) + obstacle_repulsion_dt(alpha, wall);
+
+  double Fa0_x = dt_times_tau_alpha_inv;
+  double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
+  double FaB_y = (U0_AB / R) * std::exp(-1.0 / R) * dt;
+  Vector2d expected_total(Fa0_x + Fab_x, FaB_y);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, expected_total);
+  EXPECT_LT(step.w.norm(), alpha.v_max);
+  EXPECT_VEC2_NEAR(step.v, expected_total);
+  EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
+}
+
+TEST(Integration, SymmetricStationaryBetas_VerticalForcesCancel) {
+  Pedestrian alpha = MakePed(Vector2d(5, 5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 5));
+  Pedestrian b_north = MakePed(Vector2d(5, 6), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, 100));
+  Pedestrian b_south = MakePed(Vector2d(5, 4), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, -100));
+
+  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_north) + pedestrian_repulsion_dt(alpha, b_south);
+
+  Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, expected_total);
+  EXPECT_VEC2_NEAR(step.v, expected_total);
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 5) + expected_total * dt);
+}
+
+TEST(Integration, PedBetweenTwoParallelWalls_VerticalForcesCancel) {
+  Pedestrian alpha = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));
+  Obstacle bottom(Vector2d(0, 0), Vector2d(10, 0));
+  Obstacle top(Vector2d(0, 2), Vector2d(10, 2));
+
+  Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, bottom) + obstacle_repulsion_dt(alpha, top);
+
+  Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, expected_total);
+  EXPECT_VEC2_NEAR(step.v, expected_total);
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 1) + expected_total * dt);
+}
+
+TEST(Integration, HeadOnEncounter) {
+  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
+  Pedestrian beta = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
+
+  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);
+
+  double sqrt3 = std::sqrt(3.0);
+  double Fab_x = -(2.0 * V0_AB / SIGMA / sqrt3) * std::exp(-sqrt3 / SIGMA) * dt;
+  Vector2d expected_total(Fab_x, 0.0);
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  Vector2d expected_w(1.0 + Fab_x, 0.0);
+  EXPECT_VEC2_NEAR(step.w, expected_w);
+  EXPECT_LT(step.w.norm(), alpha.v_max);
+  EXPECT_VEC2_NEAR(step.v, expected_w);
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(0.0, 0.0) + expected_w * dt);
+}
+
+TEST(Integration, SpeedCapAppliesWhenPreferredExceedsVMax) {
+  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 100.0, Vector2d(1000, 0));
+
+  Vector2d total = acceleration_term_dt(alpha);
+  EXPECT_VEC2_NEAR(total, Vector2d(4.0, 0.0));
+
+  StepResult step = IntegrateOneStep(alpha, total);
+  EXPECT_VEC2_NEAR(step.w, Vector2d(4.0, 0.0));
+  EXPECT_VEC2_NEAR(step.v, Vector2d(alpha.v_max, 0.0));
+  EXPECT_VEC2_NEAR(step.pos, Vector2d(alpha.v_max * dt, 0.0));
+}
+
+TEST(Integration, FourPedestriansNoWalls_AsymmetricCrowd) {
+
+  Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Pedestrian b_behind = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
+  Pedestrian b_north = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));
+
+  Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_front) + pedestrian_repulsion_dt(alpha, b_behind) + pedestrian_repulsion_dt(alpha, b_north);
+  const double A = (V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
+  Vector2d expected_total(dt_times_tau_alpha_inv - A + -c_neg * A, -A);
+
+  EXPECT_VEC2_NEAR(total, expected_total);
+
+  StepResult step = IntegrateOneStep(alpha, total);
+
+  Vector2d expected_w = expected_total;
+  Vector2d expected_v = expected_w;
+  Vector2d expected_pos = alpha.curr_pos + expected_v * dt;
+
+  EXPECT_LT(expected_w.norm(), alpha.v_max);
+  EXPECT_VEC2_NEAR(step.w, expected_w);
+  EXPECT_VEC2_NEAR(step.v, expected_v);
+  EXPECT_VEC2_NEAR(step.pos, expected_pos);
+}

test_helpers.hpp

@@ -0,0 +1,18 @@
+#pragma once
+#include "vector2d.hpp"
+#include <gtest/gtest.h>
+
+// default tolerance value
+constexpr double tolerance = 1e-9;
+
+// Compare two Vector2d component-wise, with tolerance, and assert if difference between expected and obtained value is below tolerance
+inline ::testing::AssertionResult Vec2Near(const Vector2d &a, const Vector2d &b, double tol = tolerance) {
+  if (std::fabs(a.x - b.x) <= tol && std::fabs(a.y - b.y) <= tol) {
+    return ::testing::AssertionSuccess();
+  }
+  return ::testing::AssertionFailure() << "expected (" << b.x << ", " << b.y << "), got (" << a.x << ", " << a.y << "), diff (" << (a.x - b.x) << ", " << (a.y - b.y) << ")" << ", exceeds tolerance : " << tol;
+}
+
+#define EXPECT_VEC2_NEAR(actual, expected) EXPECT_TRUE(Vec2Near((actual), (expected)))
+
+#define EXPECT_VEC2_NEAR_TOL(actual, expected, tol) EXPECT_TRUE(Vec2Near((actual), (expected), (tol)))

test_vector2d.cpp

@@ -0,0 +1,146 @@
+
+
+#include "test_helpers.hpp"
+#include "vector2d.hpp"
+
+#include <cmath>
+#include <limits>
+
+TEST(Vector2dArith, VectorAddition) {
+  Vector2d a(1.0, 2.0), b(3.0, 4.0);
+  EXPECT_VEC2_NEAR(a + b, Vector2d(4.0, 6.0));
+}
+
+TEST(Vector2dArith, AdditionIsCommutative) {
+  Vector2d a(1.5, -3.2), b(-0.7, 4.1);
+  EXPECT_VEC2_NEAR(a + b, b + a);
+}
+
+TEST(Vector2dArith, AdditionDoesNotMutateOperands) {
+  Vector2d a(1.0, 2.0), b(3.0, 4.0);
+  (void)(a + b);
+  EXPECT_VEC2_NEAR(a, Vector2d(1.0, 2.0));
+  EXPECT_VEC2_NEAR(b, Vector2d(3.0, 4.0));
+}
+
+TEST(Vector2dArith, VectorSubtraction) {
+  Vector2d a(5.0, 7.0), b(2.0, 4.0);
+  EXPECT_VEC2_NEAR(a - b, Vector2d(3.0, 3.0));
+}
+
+TEST(Vector2dArith, SubtractionIsAntiCommutative) {
+  Vector2d a(5.0, 7.0), b(2.0, 4.0);
+  Vector2d ab = a - b;
+  Vector2d ba = b - a;
+  EXPECT_VEC2_NEAR(ab, Vector2d(-ba.x, -ba.y));
+}
+
+TEST(Vector2dArith, ScalarSubtractionAppliesToBothComponents) {
+  Vector2d a(5.0, 7.0);
+  EXPECT_VEC2_NEAR(a - 2.0, Vector2d(3.0, 5.0));
+}
+
+TEST(Vector2dArith, ScalarAdditionAppliesToBothComponents) {
+  Vector2d a(5.0, 7.0);
+  EXPECT_VEC2_NEAR(a + 2.0, Vector2d(7.0, 9.0));
+}
+
+TEST(Vector2dArith, ScalarMultiplication) {
+  Vector2d a(3.0, -4.0);
+  EXPECT_VEC2_NEAR(a * 2.0, Vector2d(6.0, -8.0));
+}
+
+TEST(Vector2dArith, ScalarMultiplicationByZero) {
+  Vector2d a(3.0, -4.0);
+  EXPECT_VEC2_NEAR(a * 0.0, Vector2d(0.0, 0.0));
+}
+
+TEST(Vector2dArith, ScalarMultiplicationByNegative) {
+  Vector2d a(3.0, -4.0);
+  EXPECT_VEC2_NEAR(a * -1.0, Vector2d(-3.0, 4.0));
+}
+
+TEST(Vector2dArith, ScalarDivision) {
+  Vector2d a(6.0, -8.0);
+  EXPECT_VEC2_NEAR(a / 2.0, Vector2d(3.0, -4.0));
+}
+
+TEST(Vector2dArith, CompoundAdditionMutatesAndReturns) {
+  Vector2d a(1.0, 2.0);
+  Vector2d b(3.0, 4.0);
+  Vector2d returned = (a += b);
+  EXPECT_VEC2_NEAR(a, Vector2d(4.0, 6.0));
+  EXPECT_VEC2_NEAR(returned, Vector2d(4.0, 6.0));
+  EXPECT_VEC2_NEAR(b, Vector2d(3.0, 4.0));
+}
+
+TEST(Vector2dArith, CompoundAdditionAccumulates) {
+  Vector2d a(0.0, 0.0);
+  for (int i = 0; i < 5; ++i)
+    a += Vector2d(1.0, 2.0);
+  EXPECT_VEC2_NEAR(a, Vector2d(5.0, 10.0));
+}
+
+TEST(Vector2dMutation, ReverseSignReturnsNegated) {
+  Vector2d a(3.0, -4.0);
+  Vector2d r = a.reverse_sign();
+  EXPECT_VEC2_NEAR(r, Vector2d(-3.0, 4.0));
+}
+
+TEST(Vector2dMutation, ReverseSignDoesNotMutateOriginal) {
+  Vector2d a(3.0, -4.0);
+  (void)a.reverse_sign();
+  EXPECT_VEC2_NEAR(a, Vector2d(3.0, -4.0));
+}
+
+TEST(Vector2dMutation, ResetZerosBothComponents) {
+  Vector2d a(3.0, -4.0);
+  a.reset();
+  EXPECT_DOUBLE_EQ(a.x, 0.0);
+  EXPECT_DOUBLE_EQ(a.y, 0.0);
+}
+
+TEST(Vector2dNorm, ZeroVectorHasZeroNorm) {
+  Vector2d v;
+  EXPECT_DOUBLE_EQ(v.norm(), 0.0);
+}
+
+TEST(Vector2dNorm, AxisAlignedNorm) {
+  EXPECT_DOUBLE_EQ(Vector2d(3.0, 0.0).norm(), 3.0);
+  EXPECT_DOUBLE_EQ(Vector2d(0.0, -4.0).norm(), 4.0);
+}
+
+TEST(Vector2dNorm, ThreeFourFiveTriangle) {
+  Vector2d v(3.0, 4.0);
+  EXPECT_NEAR(v.norm(), 5.0, tolerance);
+}
+
+TEST(Vector2dNorm, NormIsAlwaysNonNegative) {
+  EXPECT_GE(Vector2d(-1.0, -2.0).norm(), 0.0);
+  EXPECT_GE(Vector2d(1e-30, 1e-30).norm(), 0.0);
+}
+
+TEST(Vector2dDot, OrthogonalVectorsDotIsZero) {
+  Vector2d a(1.0, 0.0), b(0.0, 1.0);
+  EXPECT_DOUBLE_EQ(a.dot(b), 0.0);
+}
+
+TEST(Vector2dDot, ParallelVectorsDotIsProductOfNorms) {
+  Vector2d a(2.0, 0.0), b(3.0, 0.0);
+  EXPECT_DOUBLE_EQ(a.dot(b), 6.0);
+}
+
+TEST(Vector2dDot, AntiParallelDotIsNegative) {
+  Vector2d a(1.0, 1.0), b(-1.0, -1.0);
+  EXPECT_DOUBLE_EQ(a.dot(b), -2.0);
+}
+
+TEST(Vector2dDot, DotIsCommutative) {
+  Vector2d a(1.5, -2.5), b(3.0, 4.0);
+  EXPECT_DOUBLE_EQ(a.dot(b), b.dot(a));
+}
+
+TEST(Vector2dDot, DotWithSelfEqualsNormSquared) {
+  Vector2d a(3.0, 4.0);
+  EXPECT_NEAR(a.dot(a), a.norm() * a.norm(), tolerance);
+}

visualization.py

@@ -5,7 +5,9 @@ import matplotlib.pyplot as plt
 from matplotlib.animation import FuncAnimation
 
 if len(sys.argv) < 2:
-    sys.exit("Use it like the following: python visualization.py positions.csv (out.mp4)")
+    sys.exit(
+        "Use it like the following: python visualization.py positions.csv (out.mp4)"
+    )
 
 df = pd.read_csv(sys.argv[1])
 frames = sorted(df["time"].unique())
@@ -15,33 +17,39 @@ frames = sorted(df["time"].unique())
 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)
+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
+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)
+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_xlim(0, 50)
-ax.set_aspect("equal"); ax.set_facecolor("white")
+ax.set_ylim(0, 10)
-ax.set_xticks([]); ax.set_yticks([])
+ax.set_aspect("equal")
+ax.set_facecolor("white")
+ax.set_xticks([])
+ax.set_yticks([])
 for s in ("top", "right", "left", "bottom"):
     ax.spines[s].set_color("gray")
 
-s_white = ax.scatter([], [], s=30, facecolors="black", edgecolor="black", zorder=2)
+s_white = ax.scatter([], [], s=30, facecolors="gray", edgecolor="gray", zorder=2)
-s_black = ax.scatter([], [], s=30, facecolors="white", edgecolor="black", zorder=2)
+s_black = ax.scatter([], [], s=30, facecolors="black", edgecolor="black", zorder=2)
 title = ax.set_title("")
 
 # Keep a reference to quiver objects so we can remove them each frame
 quivers = []
 
+
 def update(t):
     sub = df[df["time"] == t]
-    w = sub[sub["color"] == 1]
+    w = sub[sub["color"] == 0]
-    b = sub[sub["color"] == 0]
+    b = sub[sub["color"] == 1]
 
     s_white.set_offsets(w[["x", "y"]].values)
     s_black.set_offsets(b[["x", "y"]].values)
@@ -54,25 +62,46 @@ 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["dx"], w["dy"],
+        q_white = ax.quiver(
-                            color="black", angles="xy", scale_units="xy",
+            w["x"],
-                            scale=1/arrow_scale, width=0.003,
+            w["y"],
-                            headwidth=4, headlength=5, zorder=3)
+            w["dx"],
+            w["dy"],
+            color="gray",
+            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["dx"], b["dy"],
+        q_black = ax.quiver(
-                            color="gray", angles="xy", scale_units="xy",
+            b["x"],
-                            scale=1/arrow_scale, width=0.003,
+            b["y"],
-                            headwidth=4, headlength=5, zorder=3)
+            b["dx"],
+            b["dy"],
+            color="black",
+            angles="xy",
+            scale_units="xy",
+            scale=1 / arrow_scale,
+            width=0.003,
+            headwidth=4,
+            headlength=5,
+            zorder=3,
+        )
         quivers.append(q_black)
 
     title.set_text(f"t = {t}")
     return s_white, s_black, title
 
+
 anim = FuncAnimation(fig, update, frames=frames, interval=50, blit=False)
 
 if len(sys.argv) >= 3:
-    anim.save(sys.argv[2], writer="ffmpeg", fps=5)
+    anim.save(sys.argv[2], writer="ffmpeg", fps=25)
     print(f"Saved {sys.argv[2]}")
 else:
-    plt.show()
+    plt.show()