Nguyen Quy Anh
92 lines
2.1 KiB
C++
92 lines
2.1 KiB
C++
#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;
|
|
|
|
// Number of iterations
|
|
// $n_{iter}$
|
|
// const int n_iter = 100;
|
|
|
|
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;
|
|
}
|
|
|
|
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 Board(width, length, 4, obs);
|
|
}
|
|
|
|
int main(int argc, const char* argv[]) {
|
|
|
|
Board B = init_board(10, 15);
|
|
|
|
int n_p = atoi(argv[1]);
|
|
int n_iter = atoi(argv[2]);
|
|
|
|
Pedestrian *pedestrians = init_pedestrians(n_p);
|
|
|
|
for (int t = 0; t < n_iter; t++) {
|
|
|
|
for (int i = 0; i < n_p; i++) {
|
|
|
|
Pedestrian &p = pedestrians[i];
|
|
|
|
p.F_i.reset();
|
|
|
|
//
|
|
// $\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);
|
|
|
|
//
|
|
// $\sum_{\beta} \vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$
|
|
//
|
|
compute_repulsive_effects_from_pedestrians(pedestrians, &p, n_p);
|
|
|
|
//
|
|
// $\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;
|
|
|
|
//
|
|
// $\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++) {
|
|
pedestrians[i].update();
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|