lbaumberger
131 lines
3.4 KiB
C++
131 lines
3.4 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();
|
|
|
|
// Our time unit (how long a timestep goes)
|
|
double dt = 1;
|
|
|
|
// 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$
|
|
// 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++)
|
|
{
|
|
// move each pedestrian to their next position
|
|
P[i].update();
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|