implemented acceleration term

main.cpp

@@ -28,12 +28,17 @@ struct Vector2d {
 
 
 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 next_node; // $\vectorarrow{r}^k_\alpha$
-  double v0;  // speed in m/s e.g. 1.34
+  Vector2d desired_next_node; // $\vectorarrow{r}^k_\alpha$
+  Vector2d actual_next_node; // $\vectorarrow{r}^k_\alpha$
+  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; }
@@ -67,26 +72,20 @@ const int n_iter = 100;
 const double tau = 0.12345;
 
 // F_i is the current position resp. direction vector of pedestrian p in time i?
-void compute_acceleration_term(Vector2d *F_i, Vector2d curr, Vector2d next) {
-  // get curr pos --> probably no needed, can be read in F_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) {
 
-  // get next pos --> call shortest path --> how to encode map?
+  Vector2d desired_velocity = get_desired_direction(*P) * P->desired_v0;
 
-  // get desired velocity
-  // desired speed * desired direction
+  // 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;
 
-  // get actual velocity
-  // actual speed * actual direction
-
-  // TODO: how to get next node?
-
-  // acceleration * desired speed
-  double v_desired = 0.0;
-
-  double v_actual = 0.0;
-
-  double res = (1 / tau) * (v_desired - v_actual);
+  double tau_inv = 1 / tau;
 
+  P->F_i = (desired_velocity - actual_velocity) * tau_inv;
 
 }
 
@@ -97,10 +96,18 @@ Vector2d get_next_node(Pedestrian p, Vector2d map[]) {
 
 // return the direction between curr position and destination
 // $\vectorarrow{e}_\alpha(t)$
-Vector2d get_direction(Pedestrian p) {
-  Vector2d diff = p.next_node - p.curr_pos;
+Vector2d get_desired_direction(Pedestrian p) {
+  Vector2d diff = p.desired_next_node - p.curr_pos;
   double length = diff.norm();
-  return (p.next_node - p.curr_pos) / length;
+  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;
 }
 
 
@@ -122,6 +129,14 @@ double *w(Vector2d e, Vector2d f) {
   // return 0;
 }
 
+Pedestrian* init_pedestrians() {
+  Pedestrian* P = new Pedestrian[n_p];
+  for (int i = 0; i < n_p; i++) {
+    P[i] = Pedestrian();
+  }
+  return P;
+}
+
 int main() {
 
   // ! DONT TOUCH
@@ -154,19 +169,36 @@ int main() {
   // Desired destination of pedestrian $n_p$
   Vector2d r_0[n_p];
 
+
+  // 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
+      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)$
+      //
+      compute_acceleration_term(&p);
+
+
+
 
-      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$
 
       // TODO: Update $\vec{v}_i^{t+1}$