update intermediate and next velocity

update next position

main.cpp

@@ -8,7 +8,6 @@
 
 using namespace std;
 
-
 // Numer of pedestrians
 // $n_p$
 const int n_p = 100;
@@ -17,23 +16,25 @@ const int n_p = 100;
 // $n_{iter}$
 const int n_iter = 100;
 
-Pedestrian* init_pedestrians() {
-  Pedestrian* P = new Pedestrian[n_p];
-  for (int i = 0; i < n_p; i++) {
+Pedestrian *init_pedestrians()
+{
+  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) {
+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() {
+int main()
+{
 
   // ! DONT TOUCH
 
@@ -68,16 +69,20 @@ int main() {
   // create sidewalk
   Sidewalk S = init_sidewalk(10, 15);
 
-
   // store Array of Structs, store all pedestrians
-  Pedestrian* P = init_pedestrians();
+  Pedestrian *P = init_pedestrians();
+
+  // Our time unit (how long a timestep goes)
+  double dt = 1;
 
   // for each time frame
-  for (int t = 0; t < n_iter; t++) {
+  for (int t = 0; t < n_iter; t++)
+  {
     // for each pedestrian
-    for (int i = 0; i < n_p; i++) {
+    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();
@@ -87,39 +92,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})]$
       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}$
       //
       compute_repulsive_effects_from_borders(S, &p);
 
-
       // TODO: Update $\vec{w}_i$
       // $\vectorarrow{w}_i += \vectorarrow{v}_i + \vectorarrow{F}_idt$
-
+      // Intermediate velocity: current_velocity + force impulse (Force * time) (Equation1)
+      Vector2d w_i = p.curr_vel + p.F_i * dt;
 
       // TODO: Update $\vec{v}_i^{t+1}$
       //
       // $\vec{v}_i^{t+1} += \vectorarrow{w}_ig(\frac{v^{max}_i}{\Arrowvert \vectorarrow{w}_i \Arrowvert}) $
-      //
-
+      // Limit velocity to vMax
+      double w_norm = w_i.norm();
+      double g = (w_norm < 1e-6) ? 1.0 : fmin(1.0, p.vMax / w_norm);
+      p.next_vel = w_i * g;
 
       // TODO: Update $\vec{r}_i^{t+1}$
       // $\vec{r}_i^{t+1} += \vectorarrow{r}^t_i + v^t_idt$
-
-
+      p.next_pos = p.curr_pos + p.curr_vel * dt;
     }
 
-    for (int i = 0; i < n_p; i++) {
+    for (int i = 0; i < n_p; i++)
+    {
       // move each pedestrian to their next position
       P[i].update();
     }
-
   }
 
   return 0;