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 +};