missing repulsive_effects_from_borders, and last three updates. Restructured the whole project. TODO: check constants/parameters and implement missing functions.

forces.cpp

@@ -0,0 +1,117 @@
+#include <forces.h>
+#include <math.h>
+
+
+// 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);
+
+  }
+
+}
+

forces.h

@@ -0,0 +1,55 @@
+#pragma once
+#include <pedestrian.h>
+#include <sidewalk.h>
+
+// 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);
+
+

main.cpp

@@ -1,63 +1,13 @@
 #include <cfloat>
 #include <iostream>
 #include <math.h>
+#include <vector2d.h>
+#include <pedestrian.h>
+#include <sidewalk.h>
+#include <forces.h>
 
 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);
 
 
-
+      // $\vectorarrow{f}_{\alpha \beta}(\vectorarrow{r}_{\alpha \beta}) = -\gradient_{\vectorarrow{r}_{\alpha \beta}} V_{\alpha \beta}[b(\vectorarrow{r}_{\alpha \beta})]$
-      // TODO: define appropriate distance
+      compute_repulsive_effects_from_pedestrians(P, &p, n_p);
-      double distance = 0.0;
-      compute_repulsive_effects_from_pedestrians(&P, &p, distance);
 
 
-
+      // TODO: finalize this method
-      
+      //
-      compute_repulsive_effects_from_borders(F[i]);
+      // $\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;

pedestrian.h

@@ -0,0 +1,23 @@
+#pragma once
+#include <vector2d.h>
+
+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;
+  }
+};

sidewalk.h

@@ -0,0 +1,10 @@
+# pragma once
+# include <vector2d.h>
+
+// 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) {}
+};

vector2d.h

@@ -0,0 +1,24 @@
+#pragma once
+#include <math.h>
+
+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;
+  }
+};