From b86c856e082168634645a5b5705686f75e638558 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=5BQuy=20Anh=5D=20=C2=ABElliot=C2=BB=20Nguyen?= <23906-nguyenquy@users.noreply.gitlab.inf.ethz.ch> Date: Tue, 19 May 2026 23:17:44 +0200 Subject: [PATCH] Inlining + Reorganization + Roofline --- Makefile | 22 +- OPTIMIZATIONS.md | 163 ---------- 9805244v1.pdf => docs/9805244v1.pdf | Bin docs/OPTIMIZATIONS.md | 196 ++++++++++++ .../Overview_Formulas_Variables.md | 0 board.hpp => headers/board.hpp | 0 constants.hpp => headers/constants.hpp | 5 + forces.hpp => headers/forces.hpp | 0 kperf.h => headers/kperf.h | 0 pedestrian.hpp => headers/pedestrian.hpp | 0 headers/pedestrian_reduced.hpp | 35 +++ social_force.hpp => headers/social_force.hpp | 2 +- test_helpers.hpp => headers/test_helpers.hpp | 0 tsc_x86.h => headers/tsc_x86.h | 0 util.hpp => headers/util.hpp | 0 vector2d.hpp => headers/vector2d.hpp | 0 main.cpp | 12 +- pseudocode.md | 26 -- .../disable-frequency-scaling.sh | 0 scripts/roofline.py | 159 ++++++++++ verify.py => scripts/verify.py | 0 visualization.py => scripts/visualization.py | 0 sizes.cpp | 4 +- test_board.cpp => tests/test_board.cpp | 4 +- test_forces.cpp => tests/test_forces.cpp | 12 +- test_vector2d.cpp => tests/test_vector2d.cpp | 4 +- .../variant_baseline.cpp | 12 +- variants/variant_inlining_1.cpp | 244 +++++++++++++++ variants/variant_inlining_2.cpp | 243 +++++++++++++++ variants/variant_inlining_3.cpp | 255 ++++++++++++++++ variants/variant_inlining_4.cpp | 282 +++++++++++++++++ variants/variant_inlining_5.cpp | 275 +++++++++++++++++ variants/variant_inlining_6.cpp | 286 ++++++++++++++++++ variants/variant_inlining_7.cpp | 283 +++++++++++++++++ 34 files changed, 2303 insertions(+), 221 deletions(-) delete mode 100644 OPTIMIZATIONS.md rename 9805244v1.pdf => docs/9805244v1.pdf (100%) create mode 100644 docs/OPTIMIZATIONS.md rename Overview_Formulas_Variables.md => docs/Overview_Formulas_Variables.md (100%) rename board.hpp => headers/board.hpp (100%) rename constants.hpp => headers/constants.hpp (95%) rename forces.hpp => headers/forces.hpp (100%) rename kperf.h => headers/kperf.h (100%) rename pedestrian.hpp => headers/pedestrian.hpp (100%) create mode 100644 headers/pedestrian_reduced.hpp rename social_force.hpp => headers/social_force.hpp (97%) rename test_helpers.hpp => headers/test_helpers.hpp (100%) rename tsc_x86.h => headers/tsc_x86.h (100%) rename util.hpp => headers/util.hpp (100%) rename vector2d.hpp => headers/vector2d.hpp (100%) delete mode 100644 pseudocode.md rename disable-frequency-scaling.sh => scripts/disable-frequency-scaling.sh (100%) create mode 100644 scripts/roofline.py rename verify.py => scripts/verify.py (100%) rename visualization.py => scripts/visualization.py (100%) rename test_board.cpp => tests/test_board.cpp (96%) rename test_forces.cpp => tests/test_forces.cpp (98%) rename test_vector2d.cpp => tests/test_vector2d.cpp (97%) rename variant_baseline.cpp => variants/variant_baseline.cpp (98%) create mode 100644 variants/variant_inlining_1.cpp create mode 100644 variants/variant_inlining_2.cpp create mode 100644 variants/variant_inlining_3.cpp create mode 100644 variants/variant_inlining_4.cpp create mode 100644 variants/variant_inlining_5.cpp create mode 100644 variants/variant_inlining_6.cpp create mode 100644 variants/variant_inlining_7.cpp diff --git a/Makefile b/Makefile index 8263b5d..b57f56c 100644 --- a/Makefile +++ b/Makefile @@ -47,12 +47,12 @@ CXXFLAGS := -O3 -march=native -std=c++23 -Wall -Wextra -flto VARIANT ?= baseline -VARIANT_SRC := variant_$(VARIANT).cpp +VARIANT_SRC := ./variants/variant_$(VARIANT).cpp SRC := main.cpp $(VARIANT_SRC) -HEADERS := board.hpp forces.hpp pedestrian.hpp vector2d.hpp constants.hpp social_force.hpp util.hpp +HEADERS := ./headers/board.hpp ./headers/pedestrian.hpp ./headers/vector2d.hpp ./headers/constants.hpp ./headers/util.hpp -TEST_SRC := test_vector2d.cpp test_board.cpp test_forces.cpp $(VARIANT_SRC) +TEST_SRC := ./tests/test_vector2d.cpp ./tests/test_board.cpp ./tests/test_forces.cpp $(VARIANT_SRC) TEST_TARGET := run_tests TEST_LIBS := -lgtest -lgtest_main -pthread @@ -109,7 +109,7 @@ run: main # make verify VARIANT=v1_cutoff # check a specific variant verify: main ./$(TARGET) $(REFERENCE_N_P) $(REFERENCE_N_ITER) $(VERIFY_CSV) - @if python3 verify.py $(REFERENCE_CSV) $(VERIFY_CSV); then \ + @if python3 ./scripts/verify.py $(REFERENCE_CSV) $(VERIFY_CSV); then \ echo "PASS: variant '$(VARIANT)' matches $(REFERENCE_CSV)"; \ rm -f $(VERIFY_CSV); \ else \ @@ -139,7 +139,7 @@ bench-suite: bench # make video : compile the code, run the simulation, and generate the video. video: run - python visualization.py $(CSV) $(MP4) + python ./scripts/visualization.py $(CSV) $(MP4) # make sizes : compile the size-check helper. sizes: clean @@ -151,7 +151,17 @@ test: clean ./$(TEST_TARGET) # make asm : compile and generate assembly files. -asm: clean main.s variant_$(VARIANT).s + +ASM_CPP_SRCS := main.cpp $(VARIANT_SRC) sizes.cpp +ASM_HPP_SRCS := ./headers/util.hpp +ASM_SRCS := $(ASM_CPP_SRCS) $(ASM_HPP_SRCS) + +ASM_TARGETS := $(patsubst %.cpp,%.s,$(patsubst %.hpp,%.s,$(ASM_SRCS))) + +asm: clean + $(foreach src,$(ASM_CPP_SRCS),$(CXX) -O3 -march=native -std=c++23 -Wall -Wextra -DRANDOM_SEED=$(RANDOM_SEED) -S $(src) -o $(src:.cpp=.s); ) + $(foreach src,$(ASM_HPP_SRCS),$(CXX) -O3 -march=native -std=c++23 -Wall -Wextra -DRANDOM_SEED=$(RANDOM_SEED) -x c++ -S $(src) -o $(src:.hpp=.s); ) + %.s: %.cpp $(HEADERS) $(CXX) -O3 -march=native -std=c++23 -Wall -Wextra -DRANDOM_SEED=$(RANDOM_SEED) -S $< -o $@ diff --git a/OPTIMIZATIONS.md b/OPTIMIZATIONS.md deleted file mode 100644 index 2e8c66a..0000000 --- a/OPTIMIZATIONS.md +++ /dev/null @@ -1,163 +0,0 @@ -# Optimizations of the Social Force Model - -## Table of Contents -1. [Performance Results](#performance-results) -2. [Baseline](#2-baseline) -3. [Optimization 1](#3-optimization-1) -4. [Optimization 2](#4-optimization-2) -5. [Optimization 3](#5-optimization-3) -6. [Optimization 4](#6-optimization-4) -7. [Optimization 5](#7-optimization-5) -8. [Optimization 6](#8-optimization-6) -9. [Optimization 7](#9-optimization-7) -10. [Optimization 8](#10-optimization-8) -11. [Optimization 9](#11-optimization-9) - ---- - -## Performance Results - -| # | Optimization | Cycle Count | Author | -|---|---|---|---| -| 0 | Baseline | - | - | -| 1 | | | | -| 2 | | | | -| 3 | | | | -| 4 | | | | -| 5 | | | | -| 6 | | | | -| 7 | | | | -| 8 | | | | -| 9 | | | | - ---- - -## 2. Baseline - -**Author**: [Name] - -**Description**: -Standard implementation without optimizations. - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 3. Optimization 1 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 4. Optimization 2 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 5. Optimization 3 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 6. Optimization 4 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 7. Optimization 5 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 8. Optimization 6 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 9. Optimization 7 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 10. Optimization 8 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- - -## 11. Optimization 9 - -**Author**: [Name] - -**Description**: -[Briefly describe the optimization technique] - -```cpp -// Paste your code here only the relevant part -``` - ---- \ No newline at end of file diff --git a/9805244v1.pdf b/docs/9805244v1.pdf similarity index 100% rename from 9805244v1.pdf rename to docs/9805244v1.pdf diff --git a/docs/OPTIMIZATIONS.md b/docs/OPTIMIZATIONS.md new file mode 100644 index 0000000..e1855f7 --- /dev/null +++ b/docs/OPTIMIZATIONS.md @@ -0,0 +1,196 @@ +# Optimizations of the Social Force Model + +## Table of Contents + +1. [Performance Results](#performance-results) +2. [Baseline](#2-baseline) +3. [Optimization 1](#3-optimization-1) +4. [Optimization 2](#4-optimization-2) +5. [Optimization 3](#5-optimization-3) +6. [Optimization 4](#6-optimization-4) +7. [Optimization 5](#7-optimization-5) +8. [Optimization 6](#8-optimization-6) +9. [Optimization 7](#9-optimization-7) +10. [Optimization 8](#10-optimization-8) +11. [Optimization 9](#11-optimization-9) + +--- + +## Performance Results + +### x86 + +**Machine**: + +``` +Processors: 16 × AMD Ryzen AI 7 350 w/ Radeon 860M +Memory: 32 GiB of RAM (30.6 GiB usable) +Graphics Processor: AMD Radeon 860M Graphics +Manufacturer: LENOVO +Product Name: 83HX +System Version: IdeaPad Slim 5 14AKP10 +``` + +**Frequency**: 2 GHz + +| # | Optimization | W(n) [Flops] | T(n) [Cycles] | Performance [Flops/Cycle] | Author | +| --- | ------------ | ---------------- | -------------- | ------------------------- | ------ | +| 0 | Baseline | 1'058'400'000.00 | 850'352'900.00 | 1.24 | | +| 1 | Inlining | 1'058'400'000.00 | 756'088'260.00 | 1.40 | Elliot | +| 2 | | | | | | +| 3 | | | | | | +| 4 | | | | | | +| 5 | | | | | | +| 6 | | | | | | +| 7 | | | | | | +| 8 | | | | | | +| 9 | | | | | | + +### ARM + +**Machine**: + +``` + +``` + +**Frequency**: ... + +| # | Optimization | W(n) [Flops] | T(n) [Cycles] | Performance [Flops/Cycle] | Author | +| --- | ------------ | ------------ | ------------- | ------------------------- | ------ | +| 0 | Baseline | | | | | +| 1 | | | | | | +| 2 | | | | | | +| 3 | | | | | | +| 4 | | | | | | +| 5 | | | | | | +| 6 | | | | | | +| 7 | | | | | | +| 8 | | | | | | +| 9 | | | | | | + +## 2. Baseline + +**Author**: [Name] + +**Description**: +Standard implementation without optimizations. + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 3. Optimization 1 + +**Author**: [Quy Anh] «Elliot» Nguyen + +**Description**: +Manually inlined functions, namely `w`, `acceleration_term_dt`, `pedestrian_repulsion_dt`, `obstacle_repulsion_dt`, `norm`, `dot`, and `find_nearest_obstacle_point`, all into `run_simulation`. Got rid of small number checks after verifying they do not affect results. Removed all references to `Vector2d` and rewrote `Pedestrian` into a struct of doubles. Reduced `sqrt()` calls using mathematical properties. + +**Relevant Files**: `variant_inlining_1.cpp`, `variant_inlining_2.cpp`, `variant_inlining_3.cpp`, `variant_inlining_4.cpp`, `variant_inlining_5.cpp`, `variant_inlining_6.cpp`, `variant_inlining_7.cpp`, `pedestrian_reduced.hpp` + +## 4. Optimization 2 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 5. Optimization 3 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 6. Optimization 4 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 7. Optimization 5 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 8. Optimization 6 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 9. Optimization 7 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 10. Optimization 8 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 11. Optimization 9 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- diff --git a/Overview_Formulas_Variables.md b/docs/Overview_Formulas_Variables.md similarity index 100% rename from Overview_Formulas_Variables.md rename to docs/Overview_Formulas_Variables.md diff --git a/board.hpp b/headers/board.hpp similarity index 100% rename from board.hpp rename to headers/board.hpp diff --git a/constants.hpp b/headers/constants.hpp similarity index 95% rename from constants.hpp rename to headers/constants.hpp index 3ed6f44..02beb3e 100644 --- a/constants.hpp +++ b/headers/constants.hpp @@ -47,6 +47,11 @@ const double phi = 100.0 / 180.0 * M_PI; // $cos(\varphi)$ const double cos_phi = cos(phi); +// +// $\frac{1}{cos(\varphi)}$ +// +const double cos_phi_inv = 1 / cos(phi); + // $$ const double v0_mean = 1.34; diff --git a/forces.hpp b/headers/forces.hpp similarity index 100% rename from forces.hpp rename to headers/forces.hpp diff --git a/kperf.h b/headers/kperf.h similarity index 100% rename from kperf.h rename to headers/kperf.h diff --git a/pedestrian.hpp b/headers/pedestrian.hpp similarity index 100% rename from pedestrian.hpp rename to headers/pedestrian.hpp diff --git a/headers/pedestrian_reduced.hpp b/headers/pedestrian_reduced.hpp new file mode 100644 index 0000000..cd91081 --- /dev/null +++ b/headers/pedestrian_reduced.hpp @@ -0,0 +1,35 @@ +#pragma once +#include "constants.hpp" + +struct Pedestrian { + + int color; + + double curr_vel_x; + double curr_vel_y; + + double w_x; + double w_y; + + double curr_pos_x; + double curr_pos_y; + + double e_x; + double e_y; + + double v_max; + + double desired_v0; + + double destination_x; + double destination_y; + + double s_b_sq; + double s_b_e_x; + double s_b_e_y; + + Pedestrian() : color(0), curr_vel_x(0.0), curr_vel_y(0), w_x(0.0), w_y(0.0), curr_pos_x(0.0), curr_pos_y(0.0), e_x(0.0), e_y(0.0), v_max(0.0), desired_v0(0.0), destination_x(0.0), destination_y(0.0), s_b_sq(0.0), s_b_e_x(0.0), s_b_e_y(0.0) {} + + Pedestrian(int clr, double c_v_x, double c_v_y, double c_p_x, double c_p_y, double e_x, double e_y, double v_max, double desired_v0, double d_x, double d_y) + : color(clr), curr_vel_x(c_v_x), curr_vel_y(c_v_y), w_x(c_v_x), w_y(c_v_y), curr_pos_x(c_p_x), curr_pos_y(c_p_y), e_x(e_x), e_y(e_y), v_max(v_max), desired_v0(desired_v0), destination_x(d_x), destination_y(d_y), s_b_sq((c_v_x * c_v_x + c_v_y * c_v_y) * delta_t_sq), s_b_e_x(e_x * (sqrt(c_v_x * c_v_x + c_v_y * c_v_y)) * delta_t), s_b_e_y(e_y * (sqrt(c_v_x * c_v_x + c_v_y * c_v_y)) * delta_t) {} +}; diff --git a/social_force.hpp b/headers/social_force.hpp similarity index 97% rename from social_force.hpp rename to headers/social_force.hpp index 3df1c5b..f0f35b5 100644 --- a/social_force.hpp +++ b/headers/social_force.hpp @@ -31,4 +31,4 @@ Board init_board_without_obstacles(); // @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); \ No newline at end of file +int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, const char *dump_path); diff --git a/test_helpers.hpp b/headers/test_helpers.hpp similarity index 100% rename from test_helpers.hpp rename to headers/test_helpers.hpp diff --git a/tsc_x86.h b/headers/tsc_x86.h similarity index 100% rename from tsc_x86.h rename to headers/tsc_x86.h diff --git a/util.hpp b/headers/util.hpp similarity index 100% rename from util.hpp rename to headers/util.hpp diff --git a/vector2d.hpp b/headers/vector2d.hpp similarity index 100% rename from vector2d.hpp rename to headers/vector2d.hpp diff --git a/main.cpp b/main.cpp index aaeb64e..9a185f5 100644 --- a/main.cpp +++ b/main.cpp @@ -1,7 +1,7 @@ -#include "board.hpp" -#include "constants.hpp" -#include "pedestrian.hpp" -#include "social_force.hpp" +#include "./headers/board.hpp" +#include "./headers/constants.hpp" +#include "./headers/pedestrian.hpp" +#include "./headers/social_force.hpp" #include #include @@ -14,11 +14,11 @@ #endif #ifdef __x86_64__ -#include "tsc_x86.h" +#include "./headers/tsc_x86.h" #endif #ifdef __aarch64__ -#include "kperf.h" +#include "./headers/kperf.h" #endif #define N_OBSTACLES 2 diff --git a/pseudocode.md b/pseudocode.md deleted file mode 100644 index 5ab5dc8..0000000 --- a/pseudocode.md +++ /dev/null @@ -1,26 +0,0 @@ -```c++ -for each time_step t: - for each pedestrian p: - - reset_social_force(p) - - // (1) - compute_acceleration_term(p) - - // (2) - compute_repulsive_effects_from_pedestrians(p, pedestrians) - - // (3) - compute_repulsive_effects_from_borders(p, board) - - for each pedestrian p: - // next pos, desired dir, desired v, curr v - update_state(p) -``` - - -(1) $\vec{F}_\alpha(\vec{v}_\alpha, v^0_\alpha \vec{e}_\alpha) \vec{e}_\alpha(t) = \frac{1}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$ - -(2) $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$ - -(3) $\sum_{B} \vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$ diff --git a/disable-frequency-scaling.sh b/scripts/disable-frequency-scaling.sh similarity index 100% rename from disable-frequency-scaling.sh rename to scripts/disable-frequency-scaling.sh diff --git a/scripts/roofline.py b/scripts/roofline.py new file mode 100644 index 0000000..9aaf50f --- /dev/null +++ b/scripts/roofline.py @@ -0,0 +1,159 @@ +import matplotlib.pyplot as plt +from matplotlib.ticker import FuncFormatter +import numpy as np + +ram_beta = 1.91 +l1d_l2_beta = 64 +l3_beta = 32 + +pi_scalar = 6.0 +pi_vector = 48.0 + +I_vals = np.logspace(-50, 8, 500, base=2) + +plt.figure(figsize=(10, 7)) + +plt.gca().set_facecolor('lightgrey') + +plt.plot(I_vals, ram_beta * I_vals, 'k-', label='_nolegend_') +plt.plot(I_vals, l1d_l2_beta * I_vals, 'k-', label='_nolegend_') +plt.plot(I_vals, l3_beta * I_vals, 'k-', label='_nolegend_') + +ram_bound_x_pos = 0.25 +ram_bound_y_pos = ram_beta * ram_bound_x_pos + 0.175 + +plt.text( + ram_bound_x_pos, + ram_bound_y_pos, + f'Read β mem. ({ram_beta} bytes/cycle)', + color='black', + va='center', + ha='center', + fontsize=8, + rotation=20.5, +) + +l1d_l2_bound_x_pos = 0.25 +l1d_l2_bound_y_pos = l1d_l2_beta * l1d_l2_bound_x_pos + 3.5 + +plt.text( + l1d_l2_bound_x_pos, + l1d_l2_bound_y_pos, + f'Read β L1D/L2 ({l1d_l2_beta} bytes/cycle)', + color='black', + va='center', + ha='center', + fontsize=8, + rotation=20.5, +) + +l3_bound_x_pos = 0.375 +l3_bound_y_pos = l3_beta * l3_bound_x_pos + 3.5 + +plt.text( + l3_bound_x_pos, + l3_bound_y_pos, + f'Read β L3 ({l3_beta} bytes/cycle)', + color='black', + va='center', + ha='center', + fontsize=8, + rotation=20.5, +) + +plt.plot(I_vals, [pi_scalar] * len(I_vals), 'k-', label='_nolegend_') +plt.text( + 85, + pi_scalar + 1.5, + f'Peak π seq. ({pi_scalar} flops/cycle)', + color='black', + va='center', + ha='center', + fontsize=8, +) + +plt.plot(I_vals, [pi_vector] * len(I_vals), 'k-', label='_nolegend_') +plt.text( + 85, + pi_vector + 12.5, + f'Peak π par. ({pi_vector} flops/cycle)', + color='black', + va='center', + ha='center', + fontsize=8, +) + + +def fraction_formatter(x, _pos): + if x == 0: + return '0' + log2_x = np.log2(x) + if abs(log2_x - round(log2_x)) < 1e-6: + exponent = int(round(log2_x)) + if exponent < 0: + denominator = 2**(-exponent) + return f'{1/denominator:.2f}' + elif exponent == 0: + return '1' + else: + return str(int(x)) + return f'{x:.2f}' + + +formatter = FuncFormatter(fraction_formatter) + +plt.xscale('log', base=2) +plt.yscale('log', base=2) +plt.gca().xaxis.set_major_formatter(formatter) +plt.gca().yaxis.set_major_formatter(formatter) + +plt.xlabel('Operational Intensity [flops/byte]', fontsize=10) +plt.title("""Roofline Plot for the Social Force Model +CPU: AMD Ryzen AI 7 350 w/ Radeon 860M @ 2 GHz +Compiler: g++ (Debian 15.2.0-16) 15.2.0 +Command: ./main 100 10000 + +Performance [flops/cycle]""", + loc='left', + size=10) +plt.scatter([53], [0.0936126], + c='black', + marker='o', + edgecolors='black', + label='baseline (-O0)') +plt.text( + 53, + 0.0936126 - 0.03, + f'0.09 flops/cycle', + color='black', + va='center', + ha='center', + fontsize=8, +) +plt.scatter([53], [0.627801], + c='black', + marker='s', + edgecolors='black', + label='baseline (-O3 -march=native)') +plt.text( + 53, + 0.627801 - 0.2, + f'0.63 flops/cycle', + color='black', + va='center', + ha='center', + fontsize=8, +) + +plt.legend(loc='lower right', + fontsize=11, + frameon=True, + facecolor='white', + edgecolor='black') +plt.grid(True, which="both", ls="-", color='white', alpha=0.8, linewidth=1) + +plt.xlim(0.05, 200) +plt.ylim(0.0005, 100) + +plt.tight_layout() +plt.show() diff --git a/verify.py b/scripts/verify.py similarity index 100% rename from verify.py rename to scripts/verify.py diff --git a/visualization.py b/scripts/visualization.py similarity index 100% rename from visualization.py rename to scripts/visualization.py diff --git a/sizes.cpp b/sizes.cpp index e497aac..e3b9420 100644 --- a/sizes.cpp +++ b/sizes.cpp @@ -1,5 +1,5 @@ -#include "pedestrian.hpp" -#include "board.hpp" +#include "./headers/pedestrian.hpp" +#include "./headers/board.hpp" #include #include diff --git a/test_board.cpp b/tests/test_board.cpp similarity index 96% rename from test_board.cpp rename to tests/test_board.cpp index 3ade780..4c639a4 100644 --- a/test_board.cpp +++ b/tests/test_board.cpp @@ -1,5 +1,5 @@ -#include "board.hpp" -#include "test_helpers.hpp" +#include "../headers/board.hpp" +#include "../headers/test_helpers.hpp" #include diff --git a/test_forces.cpp b/tests/test_forces.cpp similarity index 98% rename from test_forces.cpp rename to tests/test_forces.cpp index d125189..38da0ed 100644 --- a/test_forces.cpp +++ b/tests/test_forces.cpp @@ -2,12 +2,12 @@ // Tests for forces.hpp // ============================================================================= -#include "board.hpp" -#include "constants.hpp" -#include "forces.hpp" -#include "pedestrian.hpp" -#include "test_helpers.hpp" -#include "vector2d.hpp" +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/forces.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" +#include "../headers/test_helpers.hpp" #include #include diff --git a/test_vector2d.cpp b/tests/test_vector2d.cpp similarity index 97% rename from test_vector2d.cpp rename to tests/test_vector2d.cpp index b7480b6..f46921a 100644 --- a/test_vector2d.cpp +++ b/tests/test_vector2d.cpp @@ -1,7 +1,7 @@ -#include "test_helpers.hpp" -#include "vector2d.hpp" +#include "../headers/test_helpers.hpp" +#include "../headers/vector2d.hpp" #include #include diff --git a/variant_baseline.cpp b/variants/variant_baseline.cpp similarity index 98% rename from variant_baseline.cpp rename to variants/variant_baseline.cpp index a7dc228..3caa214 100644 --- a/variant_baseline.cpp +++ b/variants/variant_baseline.cpp @@ -3,12 +3,10 @@ // This should make it more easy in the future to implement optimizations // Our naming convention from now on is: variant_.cpp (Please make sure to keep that consistent, otherwise our Makefile wont work anymore) -#include "board.hpp" -#include "constants.hpp" -#include "forces.hpp" -#include "pedestrian.hpp" -#include "social_force.hpp" -#include "vector2d.hpp" +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" #include #include @@ -319,7 +317,7 @@ int run_simulation(Pedestrian *pedestrians, Board &board, int n_p, int n_iter, c // # 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. + 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); diff --git a/variants/variant_inlining_1.cpp b/variants/variant_inlining_1.cpp new file mode 100644 index 0000000..567e81c --- /dev/null +++ b/variants/variant_inlining_1.cpp @@ -0,0 +1,244 @@ +// # This file performs inlining for run_simulation. +// # Changes: Inlined w, acceleration_term_dt, pedestrian_repulsion_dt, and obstacle_repulsion_dt +// # Predecessor: variant_baseline.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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 + 30.0, 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(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination); + } + + 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.0, 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); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + Vector2d desired_velocity = p.e * p.desired_v0; + Vector2d actual_velocity = p.curr_vel; + Vector2d f_alpha_0_dt = (desired_velocity - actual_velocity) * dt_times_tau_alpha_inv; + Vector2d f_alpha_dt = f_alpha_0_dt; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + Vector2d r_ab = p.curr_pos - pedestrians[j].curr_pos; + double r_ab_norm = r_ab.norm(); + r_ab_norm = r_ab_norm > EPS ? r_ab_norm : EPS; + Vector2d s_b_e = pedestrians[j].s_b_e; + Vector2d r_ab2 = r_ab - s_b_e; + double r_ab2_norm = r_ab2.norm(); + r_ab2_norm = r_ab2_norm > EPS ? r_ab2_norm : EPS; + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + two_b_sq = two_b_sq > EPS_SQ ? two_b_sq : EPS_SQ; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + Vector2d f_alpha_beta_neg = r_ab * scalar_mult_r_ab + r_ab2 * scalar_mult_r_ab2; + double w_val; + if (p.e.dot(f_alpha_beta_neg) >= f_alpha_beta_neg.norm() * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + Vector2d f_alpha_beta_dt = f_alpha_beta_neg * w_val; + + f_alpha_dt += f_alpha_beta_dt; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + Vector2d r_aB = p.curr_pos - board.obstacles[k].find_nearest_obstacle_point(p.curr_pos); + double r_aB_norm = r_aB.norm(); + r_aB_norm = r_aB_norm > EPS ? r_aB_norm : EPS; + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + Vector2d f_alpha_B_neg = r_aB * scalar_mult; + double w_val; + if (p.e.dot(f_alpha_B_neg) >= f_alpha_B_neg.norm() * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + Vector2d f_alpha_B_dt = f_alpha_B_neg * w_val; + + f_alpha_dt += f_alpha_B_dt; + } + + p.w += f_alpha_dt; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = p.w.norm(); + + double w_norm2 = w_norm > EPS ? w_norm : EPS; + + double correction_factor = std::min(1.0, p.v_max / w_norm2); + + p.curr_vel = p.w * correction_factor; + + p.curr_pos = p.curr_pos + p.curr_vel * dt; + + if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { + p.curr_pos.x = 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(p.desired_v0, 0.0); + p.curr_vel = p.w; + } + if (p.color == BLACK && p.curr_pos.x < 1.5) { + p.curr_pos.x = BOARD_LENGTH - 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(-p.desired_v0, 0.0); + p.curr_vel = p.w; + } + + p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + Vector2d e = p.destination - p.curr_pos; + + double e_norm = e.norm(); + double e_norm2 = e_norm > EPS ? e_norm : EPS; + double e_norm_inv = 1.0 / e_norm2; + + p.e = e * e_norm_inv; + + double velocity_norm_sq = p.curr_vel.dot(p.curr_vel); + + double velocity_norm = std::sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e = p.e * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_2.cpp b/variants/variant_inlining_2.cpp new file mode 100644 index 0000000..4cb8a76 --- /dev/null +++ b/variants/variant_inlining_2.cpp @@ -0,0 +1,243 @@ +// # This file performs inlining for run_simulation. +// # Changes: Inlined norm() and dot() +// # Predecessor: variant_inlining_1.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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 + 30.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination); + } + + 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.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + Vector2d desired_velocity = p.e * p.desired_v0; + Vector2d actual_velocity = p.curr_vel; + Vector2d f_alpha_0_dt = (desired_velocity - actual_velocity) * dt_times_tau_alpha_inv; + Vector2d f_alpha_dt = f_alpha_0_dt; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + Vector2d r_ab = p.curr_pos - pedestrians[j].curr_pos; + double r_ab_norm = sqrt(r_ab.x * r_ab.x + r_ab.y * r_ab.y); + r_ab_norm = r_ab_norm > EPS ? r_ab_norm : EPS; + Vector2d s_b_e = pedestrians[j].s_b_e; + Vector2d r_ab2 = r_ab - s_b_e; + double r_ab2_norm = sqrt(r_ab2.x * r_ab2.x + r_ab2.y * r_ab2.y); + r_ab2_norm = r_ab2_norm > EPS ? r_ab2_norm : EPS; + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + two_b_sq = two_b_sq > EPS_SQ ? two_b_sq : EPS_SQ; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + Vector2d f_alpha_beta_neg = r_ab * scalar_mult_r_ab + r_ab2 * scalar_mult_r_ab2; + double w_val; + if ((p.e.x * f_alpha_beta_neg.x + p.e.y * f_alpha_beta_neg.y) >= sqrt(f_alpha_beta_neg.x * f_alpha_beta_neg.x + f_alpha_beta_neg.y * f_alpha_beta_neg.y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + Vector2d f_alpha_beta_dt = f_alpha_beta_neg * w_val; + + f_alpha_dt += f_alpha_beta_dt; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + Vector2d r_aB = p.curr_pos - board.obstacles[k].find_nearest_obstacle_point(p.curr_pos); + double r_aB_norm = sqrt(r_aB.x * r_aB.x + r_aB.y * r_aB.y); + r_aB_norm = r_aB_norm > EPS ? r_aB_norm : EPS; + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + Vector2d f_alpha_B_neg = r_aB * scalar_mult; + double w_val; + if ((p.e.x * f_alpha_B_neg.x + p.e.y * f_alpha_B_neg.y) >= sqrt(f_alpha_B_neg.x * f_alpha_B_neg.x + f_alpha_B_neg.y * f_alpha_B_neg.y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + Vector2d f_alpha_B_dt = f_alpha_B_neg * w_val; + + f_alpha_dt += f_alpha_B_dt; + } + + p.w += f_alpha_dt; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w.x * p.w.x + p.w.y * p.w.y); + + double w_norm2 = w_norm > EPS ? w_norm : EPS; + + double correction_factor = min(1.0, p.v_max / w_norm2); + + p.curr_vel = p.w * correction_factor; + + p.curr_pos = p.curr_pos + p.curr_vel * dt; + + if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { + p.curr_pos.x = 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(p.desired_v0, 0.0); + p.curr_vel = p.w; + } + if (p.color == BLACK && p.curr_pos.x < 1.5) { + p.curr_pos.x = BOARD_LENGTH - 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(-p.desired_v0, 0.0); + p.curr_vel = p.w; + } + + p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + Vector2d e = p.destination - p.curr_pos; + + double e_norm = sqrt(e.x * e.x + e.y * e.y); + double e_norm2 = e_norm > EPS ? e_norm : EPS; + double e_norm_inv = 1.0 / e_norm2; + + p.e = e * e_norm_inv; + + double velocity_norm_sq = p.curr_vel.x * p.curr_vel.x + p.curr_vel.y * p.curr_vel.y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e = p.e * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_3.cpp b/variants/variant_inlining_3.cpp new file mode 100644 index 0000000..a88307f --- /dev/null +++ b/variants/variant_inlining_3.cpp @@ -0,0 +1,255 @@ +// # This file performs in-lining for run_simulation. +// # Changes: Inlined find_nearest_obstacle_point() +// # Predecessor: variant_inlining_2.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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 + 30.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination); + } + + 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.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + Vector2d desired_velocity = p.e * p.desired_v0; + Vector2d actual_velocity = p.curr_vel; + Vector2d f_alpha_0_dt = (desired_velocity - actual_velocity) * dt_times_tau_alpha_inv; + Vector2d f_alpha_dt = f_alpha_0_dt; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + Vector2d r_ab = p.curr_pos - pedestrians[j].curr_pos; + + double r_ab_norm = sqrt(r_ab.x * r_ab.x + r_ab.y * r_ab.y); + r_ab_norm = r_ab_norm > EPS ? r_ab_norm : EPS; + Vector2d s_b_e = pedestrians[j].s_b_e; + Vector2d r_ab2 = r_ab - s_b_e; + double r_ab2_norm = sqrt(r_ab2.x * r_ab2.x + r_ab2.y * r_ab2.y); + r_ab2_norm = r_ab2_norm > EPS ? r_ab2_norm : EPS; + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + two_b_sq = two_b_sq > EPS_SQ ? two_b_sq : EPS_SQ; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + Vector2d f_alpha_beta_neg = r_ab * scalar_mult_r_ab + r_ab2 * scalar_mult_r_ab2; + double w_val; + if ((p.e.x * f_alpha_beta_neg.x + p.e.y * f_alpha_beta_neg.y) >= sqrt(f_alpha_beta_neg.x * f_alpha_beta_neg.x + f_alpha_beta_neg.y * f_alpha_beta_neg.y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + Vector2d f_alpha_beta_dt = f_alpha_beta_neg * w_val; + + f_alpha_dt += f_alpha_beta_dt; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + Vector2d v1 = p.curr_pos - board.obstacles[k].start; + Vector2d v2 = board.obstacles[k].obstacle_v; + + double projection = (v1.x * v2.x + v1.y * v2.y) * board.obstacles[k].length_sq_inv; + + if (projection < 0.0) + projection = 0.0; + else if (projection > 1.0) + projection = 1.0; + + Vector2d r_aB = p.curr_pos - (board.obstacles[k].start + board.obstacles[k].obstacle_v * projection); + + double r_aB_norm = sqrt(r_aB.x * r_aB.x + r_aB.y * r_aB.y); + r_aB_norm = r_aB_norm > EPS ? r_aB_norm : EPS; + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + Vector2d f_alpha_B_neg = r_aB * scalar_mult; + double w_val; + if ((p.e.x * f_alpha_B_neg.x + p.e.y * f_alpha_B_neg.y) >= sqrt(f_alpha_B_neg.x * f_alpha_B_neg.x + f_alpha_B_neg.y * f_alpha_B_neg.y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + Vector2d f_alpha_B_dt = f_alpha_B_neg * w_val; + + f_alpha_dt += f_alpha_B_dt; + } + + p.w += f_alpha_dt; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w.x * p.w.x + p.w.y * p.w.y); + + double w_norm2 = w_norm > EPS ? w_norm : EPS; + + double correction_factor = min(1.0, p.v_max / w_norm2); + + p.curr_vel = p.w * correction_factor; + + p.curr_pos = p.curr_pos + p.curr_vel * dt; + + if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { + p.curr_pos.x = 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(p.desired_v0, 0.0); + p.curr_vel = p.w; + } + if (p.color == BLACK && p.curr_pos.x < 1.5) { + p.curr_pos.x = BOARD_LENGTH - 1.0; + p.destination.y = p.curr_pos.y; + p.w = Vector2d(-p.desired_v0, 0.0); + p.curr_vel = p.w; + } + + p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + Vector2d e = p.destination - p.curr_pos; + + double e_norm = sqrt(e.x * e.x + e.y * e.y); + double e_norm2 = e_norm > EPS ? e_norm : EPS; + double e_norm_inv = 1.0 / e_norm2; + + p.e = e * e_norm_inv; + + double velocity_norm_sq = p.curr_vel.x * p.curr_vel.x + p.curr_vel.y * p.curr_vel.y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e = p.e * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_4.cpp b/variants/variant_inlining_4.cpp new file mode 100644 index 0000000..95547d8 --- /dev/null +++ b/variants/variant_inlining_4.cpp @@ -0,0 +1,282 @@ +// # This file performs in-lining for run_simulation. +// # Changes: Replace as many Vector2d declarations with doubles as possible +// # Predecessor: variant_inlining_3.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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 + 30.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination); + } + + 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.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double desired_velocity_x = p.e.x * p.desired_v0; + double desired_velocity_y = p.e.y * p.desired_v0; + + double f_alpha_dt_x = (desired_velocity_x - p.curr_vel.x) * dt_times_tau_alpha_inv; + double f_alpha_dt_y = (desired_velocity_y - p.curr_vel.y) * dt_times_tau_alpha_inv; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + double r_ab_x = p.curr_pos.x - pedestrians[j].curr_pos.x; + double r_ab_y = p.curr_pos.y - pedestrians[j].curr_pos.y; + + double r_ab_norm = sqrt(r_ab_x * r_ab_x + r_ab_y * r_ab_y); + r_ab_norm = r_ab_norm > EPS ? r_ab_norm : EPS; + + double r_ab2_x = r_ab_x - pedestrians[j].s_b_e.x; + double r_ab2_y = r_ab_y - pedestrians[j].s_b_e.y; + + double r_ab2_norm = sqrt(r_ab2_x * r_ab2_x + r_ab2_y * r_ab2_y); + r_ab2_norm = r_ab2_norm > EPS ? r_ab2_norm : EPS; + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + two_b_sq = two_b_sq > EPS_SQ ? two_b_sq : EPS_SQ; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + + double f_alpha_beta_neg_x = r_ab_x * scalar_mult_r_ab + r_ab2_x * scalar_mult_r_ab2; + double f_alpha_beta_neg_y = r_ab_y * scalar_mult_r_ab + r_ab2_y * scalar_mult_r_ab2; + + // TODO: Get rid of sqrt + double w_val; + double a = p.e.x * f_alpha_beta_neg_x + p.e.y * f_alpha_beta_neg_y; + double b = sqrt(f_alpha_beta_neg_x * f_alpha_beta_neg_x + f_alpha_beta_neg_y * f_alpha_beta_neg_y) * cos_phi; + + // TODO: Get rid of branching without impacting performance + if (a >= b) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + f_alpha_dt_x = f_alpha_dt_x + f_alpha_beta_neg_x * w_val; + f_alpha_dt_y = f_alpha_dt_y + f_alpha_beta_neg_y * w_val; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + double v1_x = p.curr_pos.x - board.obstacles[k].start.x; + double v1_y = p.curr_pos.y - board.obstacles[k].start.y; + + double projection = (v1_x * board.obstacles[k].obstacle_v.x + v1_y * board.obstacles[k].obstacle_v.y) * board.obstacles[k].length_sq_inv; + + // TODO: Get rid of branching without impacting performance + if (projection < 0.0) + projection = 0.0; + else if (projection > 1.0) + projection = 1.0; + + double r_aB_x = p.curr_pos.x - (board.obstacles[k].start.x + board.obstacles[k].obstacle_v.x * projection); + double r_aB_y = p.curr_pos.y - (board.obstacles[k].start.y + board.obstacles[k].obstacle_v.y * projection); + + double r_aB_norm = sqrt(r_aB_x * r_aB_x + r_aB_y * r_aB_y); + r_aB_norm = r_aB_norm > EPS ? r_aB_norm : EPS; + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + double f_alpha_B_neg_x = r_aB_x * scalar_mult; + double f_alpha_B_neg_y = r_aB_y * scalar_mult; + + // TODO: Get rid of sqrt + // TODO: Get rid of branching without impacting performance + double w_val; + if ((p.e.x * f_alpha_B_neg_x + p.e.y * f_alpha_B_neg_y) >= sqrt(f_alpha_B_neg_x * f_alpha_B_neg_x + f_alpha_B_neg_y * f_alpha_B_neg_y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + double f_alpha_B_dt_x = f_alpha_B_neg_x * w_val; + double f_alpha_B_dt_y = f_alpha_B_neg_y * w_val; + + f_alpha_dt_x += f_alpha_B_dt_x; + f_alpha_dt_y += f_alpha_B_dt_y; + } + + p.w.x += f_alpha_dt_x; + p.w.y += f_alpha_dt_y; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w.x * p.w.x + p.w.y * p.w.y); + + double w_norm2 = w_norm > EPS ? w_norm : EPS; + + double correction_factor = min(1.0, p.v_max / w_norm2); + + p.curr_vel = p.w * correction_factor; + + p.curr_pos = p.curr_pos + p.curr_vel * dt; + + if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { + p.curr_pos.x = 1.0; + p.destination.y = p.curr_pos.y; + p.w.x = p.desired_v0; + p.w.y = 0.0; + p.curr_vel.x = p.w.x; + p.curr_vel.y = p.w.y; + } + if (p.color == BLACK && p.curr_pos.x < 1.5) { + p.curr_pos.x = BOARD_LENGTH - 1.0; + p.destination.y = p.curr_pos.y; + p.w.x = -p.desired_v0; + p.w.y = 0.0; + p.curr_vel.x = p.w.x; + p.curr_vel.y = p.w.y; + } + + p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + double e_x = p.destination.x - p.curr_pos.x; + double e_y = p.destination.y - p.curr_pos.y; + + double e_norm = sqrt(e_x * e_x + e_y * e_y); + double e_norm2 = e_norm > EPS ? e_norm : EPS; + double e_norm_inv = 1.0 / e_norm2; + + p.e.x = e_x * e_norm_inv; + p.e.y = e_y * e_norm_inv; + + double velocity_norm_sq = p.curr_vel.x * p.curr_vel.x + p.curr_vel.y * p.curr_vel.y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e = p.e * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_5.cpp b/variants/variant_inlining_5.cpp new file mode 100644 index 0000000..32c8e04 --- /dev/null +++ b/variants/variant_inlining_5.cpp @@ -0,0 +1,275 @@ +// # This file performs in-lining for run_simulation. +// # Changes: Got rid of EPS checks +// # Predecessor: variant_inlining_4.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian.hpp" +#include "../headers/vector2d.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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 + 30.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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(WHITE, Vector2d(), start_pos, init_e_unit, v_max, desired_v0, destination); + } + + 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.0, y); + + Vector2d init_e = destination - start_pos; + double init_e_norm = sqrt(init_e.x * init_e.x + init_e.y * init_e.y); + 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); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double desired_velocity_x = p.e.x * p.desired_v0; + double desired_velocity_y = p.e.y * p.desired_v0; + + double f_alpha_dt_x = (desired_velocity_x - p.curr_vel.x) * dt_times_tau_alpha_inv; + double f_alpha_dt_y = (desired_velocity_y - p.curr_vel.y) * dt_times_tau_alpha_inv; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + double r_ab_x = p.curr_pos.x - pedestrians[j].curr_pos.x; + double r_ab_y = p.curr_pos.y - pedestrians[j].curr_pos.y; + + double r_ab_norm = sqrt(r_ab_x * r_ab_x + r_ab_y * r_ab_y); + + double r_ab2_x = r_ab_x - pedestrians[j].s_b_e.x; + double r_ab2_y = r_ab_y - pedestrians[j].s_b_e.y; + + double r_ab2_norm = sqrt(r_ab2_x * r_ab2_x + r_ab2_y * r_ab2_y); + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + + double f_alpha_beta_neg_x = r_ab_x * scalar_mult_r_ab + r_ab2_x * scalar_mult_r_ab2; + double f_alpha_beta_neg_y = r_ab_y * scalar_mult_r_ab + r_ab2_y * scalar_mult_r_ab2; + + // TODO: Get rid of sqrt + double w_val; + double a = p.e.x * f_alpha_beta_neg_x + p.e.y * f_alpha_beta_neg_y; + double b = sqrt(f_alpha_beta_neg_x * f_alpha_beta_neg_x + f_alpha_beta_neg_y * f_alpha_beta_neg_y) * cos_phi; + + // TODO: Get rid of branching without impacting performance + if (a >= b) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + f_alpha_dt_x = f_alpha_dt_x + f_alpha_beta_neg_x * w_val; + f_alpha_dt_y = f_alpha_dt_y + f_alpha_beta_neg_y * w_val; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + double v1_x = p.curr_pos.x - board.obstacles[k].start.x; + double v1_y = p.curr_pos.y - board.obstacles[k].start.y; + + double projection = (v1_x * board.obstacles[k].obstacle_v.x + v1_y * board.obstacles[k].obstacle_v.y) * board.obstacles[k].length_sq_inv; + + // TODO: Get rid of branching without impacting performance + if (projection < 0.0) + projection = 0.0; + else if (projection > 1.0) + projection = 1.0; + + double r_aB_x = p.curr_pos.x - (board.obstacles[k].start.x + board.obstacles[k].obstacle_v.x * projection); + double r_aB_y = p.curr_pos.y - (board.obstacles[k].start.y + board.obstacles[k].obstacle_v.y * projection); + + double r_aB_norm = sqrt(r_aB_x * r_aB_x + r_aB_y * r_aB_y); + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + double f_alpha_B_neg_x = r_aB_x * scalar_mult; + double f_alpha_B_neg_y = r_aB_y * scalar_mult; + + // TODO: Get rid of sqrt + // TODO: Get rid of branching without impacting performance + double w_val; + if ((p.e.x * f_alpha_B_neg_x + p.e.y * f_alpha_B_neg_y) >= sqrt(f_alpha_B_neg_x * f_alpha_B_neg_x + f_alpha_B_neg_y * f_alpha_B_neg_y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + double f_alpha_B_dt_x = f_alpha_B_neg_x * w_val; + double f_alpha_B_dt_y = f_alpha_B_neg_y * w_val; + + f_alpha_dt_x += f_alpha_B_dt_x; + f_alpha_dt_y += f_alpha_B_dt_y; + } + + p.w.x += f_alpha_dt_x; + p.w.y += f_alpha_dt_y; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w.x * p.w.x + p.w.y * p.w.y); + + double correction_factor = min(1.0, p.v_max / w_norm); + + p.curr_vel = p.w * correction_factor; + + p.curr_pos = p.curr_pos + p.curr_vel * dt; + + if (p.color == WHITE && p.curr_pos.x > (BOARD_LENGTH - 1.5)) { + p.curr_pos.x = 1.0; + p.destination.y = p.curr_pos.y; + p.w.x = p.desired_v0; + p.w.y = 0.0; + p.curr_vel.x = p.w.x; + p.curr_vel.y = p.w.y; + } + if (p.color == BLACK && p.curr_pos.x < 1.5) { + p.curr_pos.x = BOARD_LENGTH - 1.0; + p.destination.y = p.curr_pos.y; + p.w.x = -p.desired_v0; + p.w.y = 0.0; + p.curr_vel.x = p.w.x; + p.curr_vel.y = p.w.y; + } + + p.curr_pos.y = clamp(p.curr_pos.y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + double e_x = p.destination.x - p.curr_pos.x; + double e_y = p.destination.y - p.curr_pos.y; + + double e_norm = sqrt(e_x * e_x + e_y * e_y); + double e_norm_inv = 1.0 / e_norm; + + p.e.x = e_x * e_norm_inv; + p.e.y = e_y * e_norm_inv; + + double velocity_norm_sq = p.curr_vel.x * p.curr_vel.x + p.curr_vel.y * p.curr_vel.y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e = p.e * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_6.cpp b/variants/variant_inlining_6.cpp new file mode 100644 index 0000000..487d0cb --- /dev/null +++ b/variants/variant_inlining_6.cpp @@ -0,0 +1,286 @@ +// # This file performs in-lining for run_simulation. +// # Changes: Update Vector2d references to double references +// # Predecessor: variant_inlining_5.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian_reduced.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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); + + double start_pos_x = x; + double start_pos_y = y; + + double destination_x = BOARD_LENGTH + 30.0; + double destination_y = y; + + double init_e_x = destination_x - start_pos_x; + double init_e_y = destination_y - start_pos_y; + double init_e_norm = sqrt(init_e_x * init_e_x + init_e_y * init_e_y); + double init_e_unit_x = init_e_x / init_e_norm; + double init_e_unit_y = init_e_y / 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(0, 0, 0, start_pos_x, start_pos_y, init_e_unit_x, init_e_unit_y, v_max, desired_v0, destination_x, destination_y); + } + + for (int i = n_p_half; i < n_p; i++) { + double x = x_gen_right(gen); + double y = y_gen(gen); + + double start_pos_x = x; + double start_pos_y = y; + double destination_x = -30; + double destination_y = y; + + double init_e_x = destination_x - start_pos_x; + double init_e_y = destination_y - start_pos_y; + double init_e_norm = sqrt(init_e_x * init_e_x + init_e_y * init_e_y); + double init_e_unit_x = init_e_x / init_e_norm; + double init_e_unit_y = init_e_y / 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(1, 0, 0, start_pos_x, start_pos_y, init_e_unit_x, init_e_unit_y, v_max, desired_v0, destination_x, destination_y); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double desired_velocity_x = p.e_x * p.desired_v0; + double desired_velocity_y = p.e_y * p.desired_v0; + + double f_alpha_dt_x = (desired_velocity_x - p.curr_vel_x) * dt_times_tau_alpha_inv; + double f_alpha_dt_y = (desired_velocity_y - p.curr_vel_y) * dt_times_tau_alpha_inv; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + double r_ab_x = p.curr_pos_x - pedestrians[j].curr_pos_x; + double r_ab_y = p.curr_pos_y - pedestrians[j].curr_pos_y; + + double r_ab_norm = sqrt(r_ab_x * r_ab_x + r_ab_y * r_ab_y); + + double r_ab2_x = r_ab_x - pedestrians[j].s_b_e_x; + double r_ab2_y = r_ab_y - pedestrians[j].s_b_e_y; + + double r_ab2_norm = sqrt(r_ab2_x * r_ab2_x + r_ab2_y * r_ab2_y); + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + + double f_alpha_beta_neg_x = r_ab_x * scalar_mult_r_ab + r_ab2_x * scalar_mult_r_ab2; + double f_alpha_beta_neg_y = r_ab_y * scalar_mult_r_ab + r_ab2_y * scalar_mult_r_ab2; + + // TODO: Get rid of sqrt + double w_val; + double a = p.e_x * f_alpha_beta_neg_x + p.e_y * f_alpha_beta_neg_y; + double b = sqrt(f_alpha_beta_neg_x * f_alpha_beta_neg_x + f_alpha_beta_neg_y * f_alpha_beta_neg_y) * cos_phi; + + // TODO: Get rid of branching without impacting performance + if (a >= b) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + f_alpha_dt_x = f_alpha_dt_x + f_alpha_beta_neg_x * w_val; + f_alpha_dt_y = f_alpha_dt_y + f_alpha_beta_neg_y * w_val; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + double v1_x = p.curr_pos_x - board.obstacles[k].start.x; + double v1_y = p.curr_pos_y - board.obstacles[k].start.y; + + double projection = (v1_x * board.obstacles[k].obstacle_v.x + v1_y * board.obstacles[k].obstacle_v.y) * board.obstacles[k].length_sq_inv; + + // TODO: Get rid of branching without impacting performance + if (projection < 0.0) + projection = 0.0; + else if (projection > 1.0) + projection = 1.0; + + double r_aB_x = p.curr_pos_x - (board.obstacles[k].start.x + board.obstacles[k].obstacle_v.x * projection); + double r_aB_y = p.curr_pos_y - (board.obstacles[k].start.y + board.obstacles[k].obstacle_v.y * projection); + + double r_aB_norm = sqrt(r_aB_x * r_aB_x + r_aB_y * r_aB_y); + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + double f_alpha_B_neg_x = r_aB_x * scalar_mult; + double f_alpha_B_neg_y = r_aB_y * scalar_mult; + + // TODO: Get rid of sqrt + // TODO: Get rid of branching without impacting performance + double w_val; + if ((p.e_x * f_alpha_B_neg_x + p.e_y * f_alpha_B_neg_y) >= sqrt(f_alpha_B_neg_x * f_alpha_B_neg_x + f_alpha_B_neg_y * f_alpha_B_neg_y) * cos_phi) + w_val = dt_neg; + else + w_val = c_neg_times_dt; + + double f_alpha_B_dt_x = f_alpha_B_neg_x * w_val; + double f_alpha_B_dt_y = f_alpha_B_neg_y * w_val; + + f_alpha_dt_x += f_alpha_B_dt_x; + f_alpha_dt_y += f_alpha_B_dt_y; + } + + p.w_x += f_alpha_dt_x; + p.w_y += f_alpha_dt_y; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w_x * p.w_x + p.w_y * p.w_y); + + double correction_factor = min(1.0, p.v_max / w_norm); + + p.curr_vel_x = p.w_x * correction_factor; + p.curr_vel_y = p.w_y * correction_factor; + + p.curr_pos_x = p.curr_pos_x + p.curr_vel_x * dt; + p.curr_pos_y = p.curr_pos_y + p.curr_vel_y * dt; + + if (p.color == 0 && p.curr_pos_x > (BOARD_LENGTH - 1.5)) { + p.curr_pos_x = 1.0; + p.destination_y = p.curr_pos_y; + p.w_x = p.desired_v0; + p.w_y = 0.0; + p.curr_vel_x = p.w_x; + p.curr_vel_y = p.w_y; + } + if (p.color == 1 && p.curr_pos_x < 1.5) { + p.curr_pos_x = BOARD_LENGTH - 1.0; + p.destination_y = p.curr_pos_y; + p.w_x = -p.desired_v0; + p.w_y = 0.0; + p.curr_vel_x = p.w_x; + p.curr_vel_y = p.w_y; + } + + p.curr_pos_y = clamp(p.curr_pos_y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + double e_x = p.destination_x - p.curr_pos_x; + double e_y = p.destination_y - p.curr_pos_y; + + double e_norm = sqrt(e_x * e_x + e_y * e_y); + double e_norm_inv = 1.0 / e_norm; + + p.e_x = e_x * e_norm_inv; + p.e_y = e_y * e_norm_inv; + + double velocity_norm_sq = p.curr_vel_x * p.curr_vel_x + p.curr_vel_y * p.curr_vel_y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e_x = p.e_x * (velocity_norm * delta_t); + p.s_b_e_y = p.e_y * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +} diff --git a/variants/variant_inlining_7.cpp b/variants/variant_inlining_7.cpp new file mode 100644 index 0000000..92b2767 --- /dev/null +++ b/variants/variant_inlining_7.cpp @@ -0,0 +1,283 @@ +// # This file performs in-lining for run_simulation. +// # Changes: Removed 2 sqrt() calls from the loop bodies +// # Predecessor: variant_inlining_6.cpp + +#include "../headers/board.hpp" +#include "../headers/constants.hpp" +#include "../headers/pedestrian_reduced.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#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 +#define RANDOM_SEED 0 +#endif + +#ifndef BENCHMARK +#define BENCHMARK 0 +#endif + +using namespace std; + +Pedestrian *init_pedestrians(int n_p) { +#if RANDOM_SEED + random_device rd; + mt19937 gen(rd()); +#else + mt19937 gen(SEED); +#endif + + normal_distribution velocity_gen(v0_mean, v0_std_dev); + uniform_real_distribution y_gen(0.3, BOARD_WIDTH - 0.3); + uniform_real_distribution x_gen_left(2.0, BOARD_LENGTH / 2.0); + uniform_real_distribution x_gen_right(BOARD_LENGTH / 2.0, BOARD_LENGTH - 2.0); + + Pedestrian *P = new Pedestrian[n_p]; + + 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); + + double start_pos_x = x; + double start_pos_y = y; + + double destination_x = BOARD_LENGTH + 30.0; + double destination_y = y; + + double init_e_x = destination_x - start_pos_x; + double init_e_y = destination_y - start_pos_y; + double init_e_norm = sqrt(init_e_x * init_e_x + init_e_y * init_e_y); + double init_e_unit_x = init_e_x / init_e_norm; + double init_e_unit_y = init_e_y / 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(0, 0, 0, start_pos_x, start_pos_y, init_e_unit_x, init_e_unit_y, v_max, desired_v0, destination_x, destination_y); + } + + for (int i = n_p_half; i < n_p; i++) { + double x = x_gen_right(gen); + double y = y_gen(gen); + + double start_pos_x = x; + double start_pos_y = y; + double destination_x = -30; + double destination_y = y; + + double init_e_x = destination_x - start_pos_x; + double init_e_y = destination_y - start_pos_y; + double init_e_norm = sqrt(init_e_x * init_e_x + init_e_y * init_e_y); + double init_e_unit_x = init_e_x / init_e_norm; + double init_e_unit_y = init_e_y / 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(1, 0, 0, start_pos_x, start_pos_y, init_e_unit_x, init_e_unit_y, v_max, desired_v0, destination_x, destination_y); + } + + 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 + (void)dump_path; +#endif + +#if !BENCHMARK + const int dump_interval = 1; + + vector> samples; + + if (dump_path) { + samples.reserve(static_cast(n_iter / dump_interval + 1) * n_p); + + 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++) { + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double desired_velocity_x = p.e_x * p.desired_v0; + double desired_velocity_y = p.e_y * p.desired_v0; + + double f_alpha_dt_x = (desired_velocity_x - p.curr_vel_x) * dt_times_tau_alpha_inv; + double f_alpha_dt_y = (desired_velocity_y - p.curr_vel_y) * dt_times_tau_alpha_inv; + + for (int j = 0; j < n_p; j++) { + if (i == j) + continue; + + double r_ab_x = p.curr_pos_x - pedestrians[j].curr_pos_x; + double r_ab_y = p.curr_pos_y - pedestrians[j].curr_pos_y; + + double r_ab_norm = sqrt(r_ab_x * r_ab_x + r_ab_y * r_ab_y); + + double r_ab2_x = r_ab_x - pedestrians[j].s_b_e_x; + double r_ab2_y = r_ab_y - pedestrians[j].s_b_e_y; + + double r_ab2_norm = sqrt(r_ab2_x * r_ab2_x + r_ab2_y * r_ab2_y); + double r_ab_norm_sum = r_ab_norm + r_ab2_norm; + double two_b_sq = r_ab_norm_sum * r_ab_norm_sum - pedestrians[j].s_b_sq; + double two_b = sqrt(two_b_sq); + 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)); + double scalar_mult_r_ab = scalar_mult_common * r_ab2_norm; + double scalar_mult_r_ab2 = scalar_mult_common * r_ab_norm; + + double f_alpha_beta_neg_x = r_ab_x * scalar_mult_r_ab + r_ab2_x * scalar_mult_r_ab2; + double f_alpha_beta_neg_y = r_ab_y * scalar_mult_r_ab + r_ab2_y * scalar_mult_r_ab2; + + double w_val; + double a = (p.e_x * f_alpha_beta_neg_x + p.e_y * f_alpha_beta_neg_y) * cos_phi_inv; + double b_sq = f_alpha_beta_neg_x * f_alpha_beta_neg_x + f_alpha_beta_neg_y * f_alpha_beta_neg_y; + + w_val = dt_neg; + if (a >= 0 && (a * a > b_sq)) { + w_val = c_neg_times_dt; + } + + f_alpha_dt_x = f_alpha_dt_x + f_alpha_beta_neg_x * w_val; + f_alpha_dt_y = f_alpha_dt_y + f_alpha_beta_neg_y * w_val; + } + + for (int k = 0; k < board.n_obstacles; k++) { + + double v1_x = p.curr_pos_x - board.obstacles[k].start.x; + double v1_y = p.curr_pos_y - board.obstacles[k].start.y; + + double projection = (v1_x * board.obstacles[k].obstacle_v.x + v1_y * board.obstacles[k].obstacle_v.y) * board.obstacles[k].length_sq_inv; + + if (projection < 0.0) + projection = 0.0; + else if (projection > 1.0) + projection = 1.0; + + double r_aB_x = p.curr_pos_x - (board.obstacles[k].start.x + board.obstacles[k].obstacle_v.x * projection); + double r_aB_y = p.curr_pos_y - (board.obstacles[k].start.y + board.obstacles[k].obstacle_v.y * projection); + + double r_aB_norm = sqrt(r_aB_x * r_aB_x + r_aB_y * r_aB_y); + double scalar_mult = (NEG_U0_AB_div_R * exp(r_aB_norm * NEG_R_INV)) / r_aB_norm; + double f_alpha_B_neg_x = r_aB_x * scalar_mult; + double f_alpha_B_neg_y = r_aB_y * scalar_mult; + + double w_val; + double a = (p.e_x * f_alpha_B_neg_x + p.e_y * f_alpha_B_neg_y) * cos_phi_inv; + double b_sq = f_alpha_B_neg_x * f_alpha_B_neg_x + f_alpha_B_neg_y * f_alpha_B_neg_y; + + w_val = dt_neg; + if (a >= 0 && (a * a > b_sq)) { + w_val = c_neg_times_dt; + } + + double f_alpha_B_dt_x = f_alpha_B_neg_x * w_val; + double f_alpha_B_dt_y = f_alpha_B_neg_y * w_val; + + f_alpha_dt_x += f_alpha_B_dt_x; + f_alpha_dt_y += f_alpha_B_dt_y; + } + + p.w_x += f_alpha_dt_x; + p.w_y += f_alpha_dt_y; + } + + for (int i = 0; i < n_p; i++) { + Pedestrian &p = pedestrians[i]; + + double w_norm = sqrt(p.w_x * p.w_x + p.w_y * p.w_y); + + double correction_factor = min(1.0, p.v_max / w_norm); + + p.curr_vel_x = p.w_x * correction_factor; + p.curr_vel_y = p.w_y * correction_factor; + + p.curr_pos_x = p.curr_pos_x + p.curr_vel_x * dt; + p.curr_pos_y = p.curr_pos_y + p.curr_vel_y * dt; + + if (p.color == 0 && p.curr_pos_x > (BOARD_LENGTH - 1.5)) { + p.curr_pos_x = 1.0; + p.destination_y = p.curr_pos_y; + p.w_x = p.desired_v0; + p.w_y = 0.0; + p.curr_vel_x = p.w_x; + p.curr_vel_y = p.w_y; + } + if (p.color == 1 && p.curr_pos_x < 1.5) { + p.curr_pos_x = BOARD_LENGTH - 1.0; + p.destination_y = p.curr_pos_y; + p.w_x = -p.desired_v0; + p.w_y = 0.0; + p.curr_vel_x = p.w_x; + p.curr_vel_y = p.w_y; + } + + p.curr_pos_y = clamp(p.curr_pos_y, WALL_EPS, BOARD_WIDTH_MINUS_WALL_EPS); + + double e_x = p.destination_x - p.curr_pos_x; + double e_y = p.destination_y - p.curr_pos_y; + + double e_norm_inv = 1.0 / sqrt(e_x * e_x + e_y * e_y); + + p.e_x = e_x * e_norm_inv; + p.e_y = e_y * e_norm_inv; + + double velocity_norm_sq = p.curr_vel_x * p.curr_vel_x + p.curr_vel_y * p.curr_vel_y; + + double velocity_norm = sqrt(velocity_norm_sq); + + p.s_b_sq = velocity_norm_sq * delta_t_sq; + + p.s_b_e_x = p.e_x * (velocity_norm * delta_t); + p.s_b_e_y = p.e_y * (velocity_norm * delta_t); + +#if !BENCHMARK + 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 + 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; +}