hell yea

main.cpp

@@ -1,125 +1,90 @@
-#include <cfloat>
-#include <iostream>
-#include <math.h>
-#include <vector2d.h>
-#include <pedestrian.h>
-#include <sidewalk.h>
-#include <forces.h>
+#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;