diff --git a/forces.cpp b/forces.cpp new file mode 100644 index 0000000..a80f221 --- /dev/null +++ b/forces.cpp @@ -0,0 +1,117 @@ +#include +#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; +} + + + +double w(Vector2d e, Vector2d f) { + + if (e.dot(f) >= f.norm() * PHI_DEG) return 1.0; + return C; + +} + + +// F_i is the current position resp. direction vector of pedestrian p in time i? +// +// $\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)$ +// +void compute_acceleration_term(Pedestrian *p) { + + Vector2d desired_velocity = get_desired_direction(*p) * 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; + + double tau_inv = 1 / TAU; + + p->F_i += (desired_velocity - actual_velocity) * tau_inv; + +} + + +// $\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(); + + // $v_\beta \Delta t$ + double v_beta_delta_t = P[i].actual_v0 * delta_t; + + 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); + + // $\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$ + double r_ab_norm = r_ab.norm(); + + p->F_i += * w(get_desired_direction(*p), * -1); + + } + +} + diff --git a/forces.h b/forces.h new file mode 100644 index 0000000..f5e0556 --- /dev/null +++ b/forces.h @@ -0,0 +1,55 @@ +#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/main.cpp b/main.cpp index b8684bd..1304e11 100644 --- a/main.cpp +++ b/main.cpp @@ -1,63 +1,13 @@ #include #include #include +#include +#include +#include +#include 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; - } -}; - - -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, 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; } -}; - - -// initialize map -struct Sidewalk{ - int row, col; - double length, width; - Sidewalk(int r, int c, double l, double w) : row(r), col(c), length(l), width(w) {} - /* X = obstacle - | X | - | X | - | | - | X | - |X | - */ -}; // Numer of pedestrians // $n_p$ @@ -67,108 +17,6 @@ const int n_p = 100; // $n_{iter}$ const int n_iter = 100; -// TODO: Change this -// $\tau$ -const double tau = 0.12345; - -// F_i is the current position resp. direction vector of pedestrian p in time i? -// -// $\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)$ -// -void compute_acceleration_term(Pedestrian *P) { - - Vector2d desired_velocity = get_desired_direction(*P) * 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; - - double tau_inv = 1 / tau; - - P->F_i = (desired_velocity - actual_velocity) * tau_inv; - -} - -Vector2d get_next_node(Pedestrian p, Vector2d map[]) { - // dummy element - return Vector2d(p.curr_pos.x + 1.0, p.curr_pos.y + 1.0); -} - -// 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; -} - - -// $\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[n_p], Pedestrian* p, double distance) { - double delta_t = 0.5; // assuming 0.5 seconds steps - Vector2d F_i = Vector2d(); - for (int i = 0; i < n_p; i++) { - // only consider close pedestrians - 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(); - - // $v_\beta \Delta t$ - double v_beta_delta_t = P[i]->actual_v0 * delta_t; - - 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}$ - // - double v_null_alpha_beta = 0.0; - double sigma = 0.0; - double e = 2.718; - // - // $\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); - - Vector2d f_alpha_beta_r_alpha_beta = delta_r_alpha_beta * ((v_null_alpha_beta / sigma) * (exp(-b / sigma))); - F_i += f_alpha_beta_r_alpha_beta; - } - } - p->F_i += 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() { Pedestrian* P = new Pedestrian[n_p]; for (int i = 0; i < n_p; i++) { @@ -177,6 +25,14 @@ Pedestrian* init_pedestrians() { return P; } +Sidewalk init_sidewalk(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); +} + + + int main() { // ! DONT TOUCH @@ -209,6 +65,9 @@ int main() { // 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(); @@ -218,7 +77,7 @@ int main() { // for each pedestrian for (int i = 0; i < n_p; i++) { // update... - Pedestrian p = P[i]; + Pedestrian& p = P[i]; // reset total social force p.F_i.reset(); @@ -229,23 +88,38 @@ int main() { compute_acceleration_term(&p); - - // TODO: define appropriate distance - double distance = 0.0; - compute_repulsive_effects_from_pedestrians(&P, &p, distance); + // $\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); - - - compute_repulsive_effects_from_borders(F[i]); + // 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}$ + // + compute_repulsive_effects_from_borders(S, &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}) $ + // + // TODO: Update $\vec{r}_i^{t+1}$ + // $\vec{r}_i^{t+1} += \vectorarrow{r}^t_i + v^t_idt$ + + } + + for (int i = 0; i < n_p; i++) { + // move each pedestrian to their next position + P[i].update(); + } + } return 0; diff --git a/pedestrian.h b/pedestrian.h new file mode 100644 index 0000000..d95c426 --- /dev/null +++ b/pedestrian.h @@ -0,0 +1,23 @@ +#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/sidewalk.h b/sidewalk.h new file mode 100644 index 0000000..7419191 --- /dev/null +++ b/sidewalk.h @@ -0,0 +1,10 @@ +# 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.h new file mode 100644 index 0000000..d20bda1 --- /dev/null +++ b/vector2d.h @@ -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; + } +}; \ No newline at end of file