smail alijagic
127 lines
3.0 KiB
C++
127 lines
3.0 KiB
C++
#include <cfloat>
|
|
#include <iostream>
|
|
#include <math.h>
|
|
#include <vector2d.h>
|
|
#include <pedestrian.h>
|
|
#include <sidewalk.h>
|
|
#include <forces.h>
|
|
|
|
using namespace std;
|
|
|
|
|
|
// Numer of pedestrians
|
|
// $n_p$
|
|
const int n_p = 100;
|
|
|
|
// Number of iterations
|
|
// $n_{iter}$
|
|
const int n_iter = 100;
|
|
|
|
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) {
|
|
|
|
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
|
|
|
|
// $\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
|
|
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);
|
|
|
|
|
|
// $\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$
|
|
|
|
|
|
// 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;
|
|
}
|