From d347534ff28c9c12e5fc1f657a0c610c4e2bce49 Mon Sep 17 00:00:00 2001 From: Nguyen Quy Anh <23906-nguyenquy@users.noreply.gitlab.inf.ethz.ch> Date: Fri, 27 Mar 2026 17:20:55 +0100 Subject: [PATCH] Baseline --- .clang-format | 1 + .clangd | 2 + board.hpp | 9 ++++ forces.cpp | 120 +++++++++++++++++++++++++++++++++++++++++ forces.hpp | 50 +++++++++++++++++ main.cpp | 144 ++++++++++++++++++++++--------------------------- pedestrian.hpp | 85 +++++++++++++++++++++++++++++ tsc_x86.h | 94 ++++++++++++++++++++++++++++++++ vector2d.hpp | 24 +++++++++ 9 files changed, 450 insertions(+), 79 deletions(-) create mode 100644 .clang-format create mode 100644 .clangd create mode 100644 board.hpp create mode 100644 forces.cpp create mode 100644 forces.hpp create mode 100644 pedestrian.hpp create mode 100644 tsc_x86.h create mode 100644 vector2d.hpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..5b02c5e --- /dev/null +++ b/.clang-format @@ -0,0 +1 @@ +ColumnLimit: 1000 diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..edfd275 --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +CompileFlags: + Add: [-std=c++23] \ No newline at end of file diff --git a/board.hpp b/board.hpp new file mode 100644 index 0000000..6ee78b4 --- /dev/null +++ b/board.hpp @@ -0,0 +1,9 @@ +#pragma once +#include "vector2d.hpp" + +struct Board { + double length, width; + Vector2d *borders; // array containing the position of each obstacle + int n_borders; // number of obstacles + Board(double l, double w, int n, Vector2d *border) : length(l), width(w), n_borders(n), borders(border) {} +}; diff --git a/forces.cpp b/forces.cpp new file mode 100644 index 0000000..b48d0d0 --- /dev/null +++ b/forces.cpp @@ -0,0 +1,120 @@ +#include "forces.hpp" +#include + +// # Done +double w(Vector2d e, Vector2d f) { + if (e.dot(f) >= f.norm() * cos_phi) + return 1.0; + return c; +} + +// # Done +// +// $\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)$ +// +void compute_acceleration_term(Pedestrian *p) { + + // $v_{\alpha}^{0}\vec{e}_{\alpha}$ + Vector2d desired_velocity = p->e * p->desired_v0; + + // $\vec{v}_{\alpha}(t)$ + Vector2d actual_velocity = p->curr_vel; + + double tau_alpha_inv = 1 / tau_alpha; + + p->F_i += (desired_velocity - actual_velocity) * tau_alpha_inv; +} + +// # Done +// +// $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$ +// + +void compute_repulsive_effects_from_pedestrians(Pedestrian *pedestrians, Pedestrian *p, int N) { + + for (int i = 0; i < N; i++) { + + if (pedestrians + i == p) + continue; + + // # Optional: Do not consider far away pedestrians from current pedestrian + // # Might get better speed up, remove if not + // if ((pedestrians[i].curr_pos - p->curr_pos).norm() > DISTANCE) { + // continue; + // } + + // $\vec{r}_{\alpha\beta}$ + Vector2d r_ab = p->curr_pos - pedestrians[i].curr_pos; + + // $\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert$ + double r_ab_norm = r_ab.norm(); + + // $\vec{e}_{\beta}$ + Vector2d e_beta = pedestrians[i].e; + + // $v_\beta \Delta t$ + double v_beta_delta_t = pedestrians[i].desired_v0 * delta_t; + + // $\vec{r}_{\alpha\beta} - \vec{v}_{\beta}\varDelta_{t}\vec{e}_{\beta}$ + Vector2d r_ab_minus_v_beta_delta_t_e_beta = r_ab - (e_beta * v_beta_delta_t); + + // $\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert$ + double r_ab_minus_v_beta_delta_t_e_beta_norm = r_ab_minus_v_beta_delta_t_e_beta.norm(); + + // (4): + // + // $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 b = sqrt((r_ab_norm + r_ab_minus_v_beta_delta_t_e_beta_norm) * (r_ab_norm + r_ab_minus_v_beta_delta_t_e_beta_norm) - (v_beta_delta_t) * (v_beta_delta_t)) / 2; + + // (7): + // + // $\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})}{4b}$ + // + // + Vector2d delta_r_alpha_beta_b = (((r_ab / r_ab_norm) + (r_ab_minus_v_beta_delta_t_e_beta / r_ab_minus_v_beta_delta_t_e_beta_norm)) * (r_ab_norm + r_ab_minus_v_beta_delta_t_e_beta_norm)) / (4 * b); + + // (5): + // + // $\vec{f}_{\alpha \beta}(\vec{r}_{\alpha \beta}) = \frac{V^0_{\alpha \beta}}{\sigma}e^{-b / \sigma} \grad_{\vec{r}_{\alpha \beta}}b$ + // + // + Vector2d f_alpha_beta_r_alpha_beta = delta_r_alpha_beta_b * ((V0_AB / SIGMA) * (exp(-b / SIGMA))); + + // (11): + // + // $\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})$ + // + // Add repulsive effects from other pedestrians to current pedestrian's social force + p->F_i += f_alpha_beta_r_alpha_beta * w(p->e, f_alpha_beta_r_alpha_beta * -1); + } +} + +// # Done +void compute_repulsive_effects_from_borders(Board &B, Pedestrian *p) { + + Vector2d *borders = B.borders; + int n_borders = B.n_borders; + + for (int i = 0; i < n_borders; i++) { + + Vector2d border = borders[i]; + + // $\vec{r}_{\alpha B}$ + Vector2d r_alpha_B = p->curr_pos - border; + + // + // $\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 F_alpha_B = (r_alpha_B / r_alpha_B.norm()) * (U0_AB / R) * std::exp(-r_alpha_B.norm() / R); + + // (12): + // + // $\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})$ + // + + p->F_i += F_alpha_B * w(p->e, F_alpha_B * (-1)); + } +} diff --git a/forces.hpp b/forces.hpp new file mode 100644 index 0000000..27cc042 --- /dev/null +++ b/forces.hpp @@ -0,0 +1,50 @@ +#pragma once +#include "board.hpp" +#include "pedestrian.hpp" +#include + +// $\tau_{\alpha}$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double tau_alpha = 0.5; + +// $V^0_{\alpha \beta}$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double V0_AB = 2.1; + +// $\sigma$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double SIGMA = 0.3; + +// $U^0_{\alpha B}$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double U0_AB = 10.0; + +// $R$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double R = 0.2; + +// $c$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double c = 0.5; + +// $\varphi$ +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double phi = 100.0; + +const double cos_phi = cos(phi); + +// $\varDelta_{t}$ +// step size = 0.2 seconds +// https://arxiv.org/pdf/cond-mat/9805244, page 8 +const double delta_t = 2; + +// # Optional (forces.cpp: compute_repulsive_effects_from_pedestrians) +// const double DISTANCE = 2.0; + +double w(Vector2d e, Vector2d f); + +void compute_acceleration_term(Pedestrian *p); + +void compute_repulsive_effects_from_pedestrians(Pedestrian *P, Pedestrian *p, int N); + +void compute_repulsive_effects_from_borders(Board &B, Pedestrian *p); diff --git a/main.cpp b/main.cpp index 248d4f6..2e6035b 100644 --- a/main.cpp +++ b/main.cpp @@ -1,103 +1,89 @@ -#include -#include -#include +#include "board.hpp" +#include "forces.hpp" +#include "math.h" +#include "pedestrian.hpp" +#include "vector2d.hpp" + +double g(double v_max_alpha, double w_alpha_norm) { + if (w_alpha_norm <= v_max_alpha) { + return 1; + } + return v_max_alpha / w_alpha_norm; +} using namespace std; -struct Vector2d { - double x, y; - Vector2d(double a, double b) : x(a), y(b) {} - Vector2d() : x(.0), y(.0) {} - Vector2d operator+(Vector2d v) const { return Vector2d(v.x + x, v.y + y); } - Vector2d operator-(Vector2d v) const { return Vector2d(x - v.x, y - v.y); } - Vector2d operator-(double a) const { return Vector2d(x - a, y - a); } - Vector2d operator+(double a) const { return Vector2d(x + a, y + a); } - Vector2d operator/(double a) const { return Vector2d(x / a, y / a); } - Vector2d operator*(double a) const { return Vector2d(x * a, y * a); } - Vector2d operator+=(const Vector2d &v) { - *this = *this + v; - return *this; - } - double norm() { return sqrt(x * x + y * y); } - double dot(Vector2d v) { return x * v.x + y * v.y; } - void reset() { - x = 0; - y = 0; - } -}; - // Numer of pedestrians // $n_p$ -const int n_p = 100; +// const int n_p = 100; // Number of iterations // $n_{iter}$ -const int n_iter = 100; +// const int n_iter = 100; -// TODO: Change this -// $\tau$ -const double tau = 0.12345; - -void compute_acceleration_term(Vector2d *F_i) {} -void compute_repulsive_effects_from_pedestrians(Vector2d *F_i) {} -void compute_repulsive_effects_from_borders(Vector2d *F_i) {} - -// Return an array of doubles -double *w(Vector2d e, Vector2d f) { - - // TODO: - // $cos(60\degree)$ - // if (e * f > f.norm() * (0.5)) { - // } - // return 0; +Pedestrian *init_pedestrians(int n_p) { + Pedestrian *P = new Pedestrian[n_p]; + for (int i = 0; i < n_p; i++) { + P[i] = Pedestrian(); + } + return P; } -int main() { +Board init_board(int width, int length) { + Vector2d obs[] = {Vector2d(2.0, 1.0), Vector2d(4.0, 1.0), Vector2d(6.0, 2.0), Vector2d(6.0, 4.0)}; + return Board(width, length, 4, obs); +} - // ! DONT TOUCH +int main(int argc, const char* argv[]) { - // $\vec{v}^0_i$ - // Velocity at iteration $t$ for pedestrian $i$ - double v[n_iter][n_p]; + Board B = init_board(10, 15); - // $\vec{r}^0_i$ - // Intended destination at iteration $t$ for pedestrian $i$ - Vector2d r[n_iter][n_p]; + int n_p = atoi(argv[1]); + int n_iter = atoi(argv[2]); - // $\vec{e}_i$ - // Desired direction to the next way point at iteration $t$ for pedestrian $i$ - Vector2d e[n_iter][n_p]; - - // $v_i^{max}$ - // Maximum allowed speed for pedestrian $i$ - double v_max[n_p]; - - // $v_i^{0}$ - // Desired speed for pedestrian $i$ - double v_0[n_p]; - - // $\vec{F}_i$ - // Total social force for pedestrian $i$ - Vector2d F[n_p][n_p]; - - // $\vec{r}_0$ - // Desired destination of pedestrian $n_p$ - Vector2d r_0[n_p]; + Pedestrian *pedestrians = init_pedestrians(n_p); for (int t = 0; t < n_iter; t++) { + for (int i = 0; i < n_p; i++) { - for (int itr = 0; itr < n_p; itr++) { - F[i][itr].reset(); - } - compute_acceleration_term(F[i]); - compute_repulsive_effects_from_pedestrians(F[i]); - compute_repulsive_effects_from_borders(F[i]); - // TODO: Update $\vec{w}_i$ + Pedestrian &p = pedestrians[i]; - // TODO: Update $\vec{v}_i^{t+1}$ + p.F_i.reset(); - // TODO: pdate $\vec{r}_i^{t+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)$ + // + compute_acceleration_term(&p); + + // + // $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$ + // + compute_repulsive_effects_from_pedestrians(pedestrians, &p, n_p); + + // + // $\sum_{B} \vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$ + // + compute_repulsive_effects_from_borders(B, &p); + + // + // $\vec{w}_i = \vec{v}_i + \vec{F}_idt$ + // + p.w_alpha = p.curr_vel + p.F_i * delta_t; + + // + // $\vec{v}_i^{t+1} = \vec{w}_ig(\frac{v^{max}_i}{\Arrowvert \vec{w}_i \Arrowvert}) $ + // + p.next_vel = p.w_alpha * g(p.v_max, p.w_alpha.norm()); + + // + // $\vec{r}_i^{t+1} += \vec{r}^t_i + v^t_idt$ + // + p.curr_pos = p.curr_pos + p.curr_vel * delta_t; + } + + for (int i = 0; i < n_p; i++) { + pedestrians[i].update(); } } diff --git a/pedestrian.hpp b/pedestrian.hpp new file mode 100644 index 0000000..8cbef8b --- /dev/null +++ b/pedestrian.hpp @@ -0,0 +1,85 @@ +#pragma once +#include "vector2d.hpp" +#include "random" + + +struct Pedestrian { + + // TODO: Update initial values + Pedestrian() : + curr_vel(), + curr_pos(), + e(), + v_max(1.34 * 1.3), + desired_v0(1.34), + destination(), + F_i(), + next_pos(), + next_vel(), + w_alpha() + {} + + Pedestrian( + Vector2d c_v, + Vector2d c_p, + Vector2d e, + Vector2d d, + Vector2d f, + Vector2d n_p, + Vector2d n_v, + Vector2d w_alpha + ) : + curr_vel(c_v), + curr_pos(c_p), + e(e), + v_max(), + desired_v0(), + destination(d), + F_i(f), + next_pos(n_p), + next_vel(n_v), + w_alpha(w_alpha) + {} + + // $\vec{v}_{\alpha}(t)$ + Vector2d curr_vel; + + // $\vec{r}_\alpha(t)$ + Vector2d curr_pos; + + // $\vec{e}_{\alpha}(t)$ + Vector2d e; + + // $v_{\alpha}^{max} = 1.3 v^{0}_{\alpha}$ + double v_max; + + // $v_\alpha^0$ + double desired_v0; + + // # ============================================================= + + // $\vec{r}^0_\alpha$ + Vector2d destination; + + // social force + Vector2d F_i; + + // # ============================================================= + + // $\vec{r}_\alpha(t+1)$ + Vector2d next_pos; + + // $\vec{v}_{\alpha}(t + 1)$ + Vector2d next_vel; + + // $\vec{w}_{\alpha}$ + Vector2d w_alpha; + + // # ============================================================= + + void update() { + curr_pos = next_pos; + curr_vel = next_vel; + e = (destination - curr_pos) / (destination - curr_pos).norm(); + } +}; diff --git a/tsc_x86.h b/tsc_x86.h new file mode 100644 index 0000000..4e4baf4 --- /dev/null +++ b/tsc_x86.h @@ -0,0 +1,94 @@ +/* ==================== GNU C and possibly other UNIX compilers ===================== */ + #ifndef _WIN32 + + #if defined(__GNUC__) || defined(__linux__) + #define VOLATILE __volatile__ + #define ASM __asm__ + #else + /* if we're neither compiling with gcc or under linux, we can hope + * the following lines work, they probably won't */ + #define ASM asm + #define VOLATILE + #endif + + #define myInt64 unsigned long long + #define INT32 unsigned int + +/* ======================== WIN32 ======================= */ +#else + + #define myInt64 signed __int64 + #define INT32 unsigned __int32 + +#endif + +/* This is the RDTSC timer. + * RDTSC is an instruction on several Intel and compatible CPUs that Reads the + * Time Stamp Counter. The Intel manuals contain more information. + */ + + +#define COUNTER_LO(a) ((a).int32.lo) +#define COUNTER_HI(a) ((a).int32.hi) +#define COUNTER_VAL(a) ((a).int64) + +#define COUNTER(a) \ + ((unsigned long long)COUNTER_VAL(a)) + +#define COUNTER_DIFF(a,b) \ + (COUNTER(a)-COUNTER(b)) + +/* ==================== GNU C and possibly other UNIX compilers ===================== */ +#ifndef _WIN32 + + typedef union + { myInt64 int64; + struct {INT32 lo, hi;} int32; + } tsc_counter; + + #define RDTSC(cpu_c) \ + ASM VOLATILE ("rdtsc" : "=a" ((cpu_c).int32.lo), "=d"((cpu_c).int32.hi)) + #define CPUID() \ + ASM VOLATILE ("cpuid" : : "a" (0) : "bx", "cx", "dx" ) + +/* ======================== WIN32 ======================= */ +#else + + typedef union + { myInt64 int64; + struct {INT32 lo, hi;} int32; + } tsc_counter; + + #define RDTSC(cpu_c) \ + { __asm rdtsc \ + __asm mov (cpu_c).int32.lo,eax \ + __asm mov (cpu_c).int32.hi,edx \ + } + + #define CPUID() \ + { \ + __asm mov eax, 0 \ + __asm cpuid \ + } + +#endif + + +static void init_tsc() { + ; // no need to initialize anything for x86 +} + +static myInt64 start_tsc(void) { + tsc_counter start; + CPUID(); + RDTSC(start); + return COUNTER_VAL(start); +} + +static myInt64 stop_tsc(myInt64 start) { + tsc_counter end; + RDTSC(end); + CPUID(); + return COUNTER_VAL(end) - start; +} + diff --git a/vector2d.hpp b/vector2d.hpp new file mode 100644 index 0000000..b4eef41 --- /dev/null +++ b/vector2d.hpp @@ -0,0 +1,24 @@ +#pragma once +#include + +struct Vector2d { + double x, y; + Vector2d(double a, double b) : x(a), y(b) {} + Vector2d() : x(.0), y(.0) {} + Vector2d operator+(Vector2d v) const { return Vector2d(v.x + x, v.y + y); } + Vector2d operator-(Vector2d v) const { return Vector2d(x - v.x, y - v.y); } + Vector2d operator-(double a) const { return Vector2d(x - a, y - a); } + Vector2d operator+(double a) const { return Vector2d(x + a, y + a); } + Vector2d operator/(double a) const { return Vector2d(x / a, y / a); } + Vector2d operator*(double a) const { return Vector2d(x * a, y * a); } + Vector2d operator+=(const Vector2d &v) { + *this = *this + v; + return *this; + } + double norm() { return sqrt(x * x + y * y); } + double dot(Vector2d v) { return x * v.x + y * v.y; } + void reset() { + x = 0; + y = 0; + } +};