From cde82faa4ddc450e96f78dc6bb72bd162b016988 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:16:23 +0100 Subject: [PATCH] hell yea --- .clang-format | 1 + .clangd | 2 + board.hpp | 9 ++ forces.cpp | 185 +++++++++++++++++++------------------ forces.h | 55 ----------- forces.hpp | 50 ++++++++++ main.cpp | 125 +++++++++---------------- pedestrian.h | 23 ----- pedestrian.hpp | 85 +++++++++++++++++ sidewalk.h | 10 -- vector2d.h => vector2d.hpp | 2 +- 11 files changed, 287 insertions(+), 260 deletions(-) create mode 100644 .clang-format create mode 100644 .clangd create mode 100644 board.hpp delete mode 100644 forces.h create mode 100644 forces.hpp delete mode 100644 pedestrian.h create mode 100644 pedestrian.hpp delete mode 100644 sidewalk.h rename vector2d.h => vector2d.hpp (99%) 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 index a80f221..b48d0d0 100644 --- a/forces.cpp +++ b/forces.cpp @@ -1,117 +1,120 @@ -#include +#include "forces.hpp" #include - -// return the direction between curr position and destination -// $\vectorarrow{e}_\alpha(t)$ -Vector2d get_desired_direction(Pedestrian& p) { - Vector2d diff = p.desired_next_node - p.curr_pos; - double length = diff.norm(); - return (p.desired_next_node - p.curr_pos) / length; -} - -// return the direction between curr position and destination -// $\vectorarrow{e}_\alpha(t)$ -Vector2d get_actual_direction(Pedestrian& p) { - Vector2d diff = p.actual_next_node - p.curr_pos; - double length = diff.norm(); - return (p.actual_next_node - p.curr_pos) / length; -} - - - +// # Done double w(Vector2d e, Vector2d f) { - - if (e.dot(f) >= f.norm() * PHI_DEG) return 1.0; - return C; - + if (e.dot(f) >= f.norm() * cos_phi) + return 1.0; + return c; } - -// F_i is the current position resp. direction vector of pedestrian p in time i? +// # Done // -// $\vectorarrow{F}_\alpha(\vectorarrow{v}_\alpha, v^0_\alpha \vectorarrow{e}_\alpha) \vectorarrow{e}_\alpha(t) = \frac{1}{\tau_\alpha}(v^0_\alpha \vectorarrow{e}_\alpha - \vectorarrow{v}_\alpha)$ +// $\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) { - Vector2d desired_velocity = get_desired_direction(*p) * p->desired_v0; + // $v_{\alpha}^{0}\vec{e}_{\alpha}$ + Vector2d desired_velocity = p->e * p->desired_v0; - // TODO: How to get this number - // Probably needs to lookup the map and see if the desired position is reachable - Vector2d actual_velocity = get_actual_direction(*p) * p->actual_v0; + // $\vec{v}_{\alpha}(t)$ + Vector2d actual_velocity = p->curr_vel; - double tau_inv = 1 / TAU; - - p->F_i += (desired_velocity - actual_velocity) * tau_inv; + 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) { -// $\vectorarrow{f}_{\alpha \beta}(\vectorarrow{r}_{\alpha \beta}) = -\gradient_{\vectorarrow{r}_{\alpha \beta}} V_{\alpha \beta}[b(\vectorarrow{r}_{\alpha \beta})]$ -void compute_repulsive_effects_from_pedestrians(Pedestrian* P, Pedestrian* p, int N) { - double delta_t = 0.5; // assuming 0.5 seconds steps for (int i = 0; i < N; i++) { - // TODO: add check for $\vectorarrow{e} * \vectorarrow{f} >= \Arrowvert \vectorarrow{f} \Arrowvert \cos(\phi) $ - // only consider close pedestrians - if (P + i == p) continue; - if ((P[i].curr_pos - p->curr_pos).norm() < DISTANCE) { - // - // $2b = \sqrt{(\Arrowvert \vectorarrow{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vectorarrow{r}_{\alpha \beta} - v_\beta \Delta t \vectorarrow{e}_\beta \Arrowvert)^2 - (v_\beta \Delta t)^2}$ - // - // $\Arrowvert \vectorarrow{r}_{\alpha \beta} \Arrowvert$ - Vector2d r_ab = p->curr_pos - P[i].curr_pos; - double r_ab_norm = (p->curr_pos - P[i].curr_pos).norm(); - // $\Arrowvert \vectorarrow{r}_{\alpha \beta} - v_\beta \Delta t \vectorarrow{e}_\beta \Arrowvert$ - Vector2d e_beta = get_actual_direction(P[i]); - Vector2d r_ab_something = p->curr_pos - P[i].curr_pos - (e_beta * (P[i].actual_v0 * delta_t)); - double r_ab_something_norm = (p->curr_pos - P[i].curr_pos - (e_beta * (P[i].actual_v0 * delta_t))).norm(); + if (pedestrians + i == p) + continue; - // $v_\beta \Delta t$ - double v_beta_delta_t = P[i].actual_v0 * delta_t; + // # 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; + // } - double b = sqrt((r_ab_norm + r_ab_something_norm) * (r_ab_norm + r_ab_something_norm) - (v_beta_delta_t) * (v_beta_delta_t)) / 2; - - // - // $\vectorarrow{f}_{\alpha \beta}(\vectorarrow{r}_{\alpha \beta}) = \frac{V^0_{\alpha \beta}}{\sigma}e^{-b / \sigma} \grad_{\vectorarrow{r}_{\alpha \beta}}b$ - // - // TODO: what is $V^0_{\alpha \beta}$ probably some number? - // - // - // $\grad_{\vectorarrow{r}_{\alpha \beta}} = \frac{(\Arrowvert \vectorarrow{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vectorarrow{r}_{\alpha \beta} - v_\beta \Delta t \vectorarrow{e}_\beta \Arrowvert) (\frac{\vectorarrow{r}_{\alpha \beta}}{\Arrowvert \vectorarrow{r}_{\alpha \beta} \Arrowvert} + \frac{\vectorarrow{r}_{\alpha \beta} - v_\beta \Delta t \vectorarrow{e}_\beta}{\Arrowvert \vectorarrow{r}_{\alpha \beta} - v_\beta \Delta t \vectorarrow{e}_\beta \Arrowvert})}{4b}$ - // - Vector2d delta_r_alpha_beta = ( ( (r_ab / r_ab_norm) + (r_ab_something / r_ab_something_norm) ) * (r_ab_norm + r_ab_something_norm) ) / (4 * b); + // $\vec{r}_{\alpha\beta}$ + Vector2d r_ab = p->curr_pos - pedestrians[i].curr_pos; - // $\vectorarrow{f}_{\alpha \beta}$ - Vector2d f_alpha_beta_r_alpha_beta = delta_r_alpha_beta * ((V0_AB / SIGMA) * (exp(-b / SIGMA))); - - p->F_i += f_alpha_beta_r_alpha_beta * w(get_desired_direction(*p), f_alpha_beta_r_alpha_beta * -1); - } - } -} - -// TODO: missing -void compute_repulsive_effects_from_borders(Sidewalk& S, Pedestrian* p) { - - Vector2d* obstacles = S.obstacles; - int n_obs = S.n_obstacles; - - for (int i = 0; i < n_obs; i++) { - Vector2d obstacle = obstacles[i]; - // TODO: find out the variables and parameters - // - // $\vectorarrow{F}_{\alpha B}(\vectorarrow{r}_{\alpha B}) = \frac{U^0_{\alpha B}}{R} e^{- \Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert / R} \frac{\vectorarrow{r}_{\alpha B}}{\Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert}$ - // - double e = 2.718; - - // $\vectorarrow{r}_{\alpha B}$ - Vector2d r_ab = p->curr_pos - obstacle; - // $\Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert$ + // $\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert$ double r_ab_norm = r_ab.norm(); - p->F_i += * w(get_desired_direction(*p), * -1); + // $\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.h b/forces.h deleted file mode 100644 index f5e0556..0000000 --- a/forces.h +++ /dev/null @@ -1,55 +0,0 @@ -#pragma once -#include -#include - -// Params -// TODO: change this -// $\tau$ -const double TAU = 1.0; // time to reach desired speed in s - -// TODO: change this -// $V^0_{\alpha \beta}$ -const double V0_AB = 2.1; - -// TODO: change this -// $\sigma$ -const double SIGMA = 0.3; - -// TODO: change this -// $U^0_{\alpha B}$ -const double U0_AB = 10.0; - -// TODO: change this -// $R$ -const double R = 0.2; - -// TODO: change this -// $C$ -const double C = 0.2; - -// TODO: change this -// $\phi$ -const double PHI_DEG = 120.0; - -// TODO: change this -const double DISTANCE = 2.0; - - -// Helper functions - -Vector2d get_desired_direction(Pedestrian& p); - -Vector2d get_actual_direction(Pedestrian& p); - -double w(Vector2d e, Vector2d f); - - -// Force functions - -void compute_acceleration_term(Pedestrian* p); - -void compute_repulsive_effects_from_pedestrians(Pedestrian* P, Pedestrian *p, int N); - -void compute_repulsive_effects_from_borders(Sidewalk& S, Pedestrian* p); - - 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 1304e11..2e6035b 100644 --- a/main.cpp +++ b/main.cpp @@ -1,125 +1,90 @@ -#include -#include -#include -#include -#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; - // 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; -Pedestrian* init_pedestrians() { - Pedestrian* P = new Pedestrian[n_p]; +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; } -Sidewalk init_sidewalk(int width, int length) { - +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 Sidewalk(width, length, 4, obs); + return Board(width, length, 4, obs); } +int main(int argc, const char* argv[]) { + Board B = init_board(10, 15); -int main() { + int n_p = atoi(argv[1]); + int n_iter = atoi(argv[2]); - // ! DONT TOUCH + Pedestrian *pedestrians = init_pedestrians(n_p); - // $\vec{v}^0_i$ - // Velocity at iteration $t$ for pedestrian $i$ - double v[n_iter][n_p]; - - // $\vec{r}^0_i$ - // Intended destination at iteration $t$ for pedestrian $i$ - Vector2d r[n_iter][n_p]; - - // $\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]; - - // create sidewalk - Sidewalk S = init_sidewalk(10, 15); - - - // store Array of Structs, store all pedestrians - Pedestrian* P = init_pedestrians(); - - // for each time frame for (int t = 0; t < n_iter; t++) { - // for each pedestrian + for (int i = 0; i < n_p; i++) { - // update... - Pedestrian& p = P[i]; - - // reset total social force + + Pedestrian &p = pedestrians[i]; + p.F_i.reset(); // - // $\vectorarrow{F}_\alpha(\vectorarrow{v}_\alpha, v^0_\alpha \vectorarrow{e}_\alpha) \vectorarrow{e}_\alpha(t) = \frac{1}{\tau_\alpha}(v^0_\alpha \vectorarrow{e}_\alpha - \vectorarrow{v}_\alpha)$ + // $\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); - - // $\vectorarrow{f}_{\alpha \beta}(\vectorarrow{r}_{\alpha \beta}) = -\gradient_{\vectorarrow{r}_{\alpha \beta}} V_{\alpha \beta}[b(\vectorarrow{r}_{\alpha \beta})]$ - compute_repulsive_effects_from_pedestrians(P, &p, n_p); - - - // TODO: finalize this method // - // $\vectorarrow{F}_{\alpha B}(\vectorarrow{r}_{\alpha B}) = \frac{U^0_{\alpha B}}{R} e^{- \Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert / R} \frac{\vectorarrow{r}_{\alpha B}}{\Arrowvert \vectorarrow{r}_{\alpha B} \Arrowvert}$ + // $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$ // - compute_repulsive_effects_from_borders(S, &p); + compute_repulsive_effects_from_pedestrians(pedestrians, &p, n_p); - - // TODO: Update $\vec{w}_i$ - // $\vectorarrow{w}_i += \vectorarrow{v}_i + \vectorarrow{F}_idt$ - - - // TODO: Update $\vec{v}_i^{t+1}$ // - // $\vec{v}_i^{t+1} += \vectorarrow{w}_ig(\frac{v^{max}_i}{\Arrowvert \vectorarrow{w}_i \Arrowvert}) $ + // $\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; - // TODO: Update $\vec{r}_i^{t+1}$ - // $\vec{r}_i^{t+1} += \vectorarrow{r}^t_i + v^t_idt$ - + // + // $\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++) { - // move each pedestrian to their next position - P[i].update(); + pedestrians[i].update(); } - } return 0; diff --git a/pedestrian.h b/pedestrian.h deleted file mode 100644 index d95c426..0000000 --- a/pedestrian.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once -#include - -struct Pedestrian { - Pedestrian() : curr_pos(), next_pos(), curr_vel(), next_vel(), destination(), desired_next_node(), actual_next_node(), F_i(), desired_v0(1.34), actual_v0(1.34), vMax(1.34 * 1.3) {} - Pedestrian(Vector2d c_p, Vector2d n_p, Vector2d c_v, Vector2d n_v, Vector2d d, Vector2d d_n_n, Vector2d a_n_n, Vector2d f) : curr_pos(c_p), next_pos(n_p), curr_vel(c_v), next_vel(n_v), destination(d), desired_next_node(d_n_n), actual_next_node(a_n_n), F_i(f) {} - Vector2d curr_pos; // $\vectorarrow{r}_\alpha(t)$ - Vector2d next_pos; // $\vectorarrow{r}_\alpha(t+1)$ position, curr_pos is needed because when calculating the force on pedestrian j, we need the old pos, not the updated - Vector2d curr_vel; - Vector2d next_vel; // velocity - Vector2d destination; // $\vectorarrow{r}^0_\alpha$ - Vector2d desired_next_node; // $\vectorarrow{r}^k_\alpha$ - Vector2d actual_next_node; // $\vectorarrow{r}^k_\alpha$ should be the same as desired_next_node but with obstacles it changes - Vector2d F_i; // social force - double desired_v0; // $v_\alpha^0$ desired speed in m/s e.g. 1.34 - double actual_v0; - double vMax; // vMax = v0 * 1.3 - // direction Vector2d e = (dest - curr.position).norm() --> calculate on the fly or part of struct? - void update() { - curr_pos = next_pos; - curr_vel = next_vel; - } -}; \ No newline at end of file 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/sidewalk.h b/sidewalk.h deleted file mode 100644 index 7419191..0000000 --- a/sidewalk.h +++ /dev/null @@ -1,10 +0,0 @@ -# pragma once -# include - -// initialize map -struct Sidewalk{ - double length, width; - Vector2d* obstacles; // array containing the position of each obstacle - int n_obstacles; // number of obstacles - Sidewalk(double l, double w, int n, Vector2d* obs) : length(l), width(w), n_obstacles(n), obstacles(obs) {} -}; \ No newline at end of file diff --git a/vector2d.h b/vector2d.hpp similarity index 99% rename from vector2d.h rename to vector2d.hpp index d20bda1..b4eef41 100644 --- a/vector2d.h +++ b/vector2d.hpp @@ -21,4 +21,4 @@ struct Vector2d { x = 0; y = 0; } -}; \ No newline at end of file +};