Nguyen Quy Anh
107 lines
7.7 KiB
C++
107 lines
7.7 KiB
C++
#include <cfloat>
|
|
#include <iostream>
|
|
#include <math.h>
|
|
|
|
using namespace std;
|
|
|
|
struct Vector2d {
|
|
double x, y;
|
|
Vector2d(double a, double b) : x(a), y(b) {}
|
|
Vector2d() : x(.0), y(.0) {}
|
|
Vector2d operator+(Vector2d v) const { return Vector2d(v.x + x, v.y + y); }
|
|
Vector2d operator-(Vector2d v) const { return Vector2d(x - v.x, y - v.y); }
|
|
Vector2d operator-(double a) const { return Vector2d(x - a, y - a); }
|
|
Vector2d operator+(double a) const { return Vector2d(x + a, y + a); }
|
|
Vector2d operator/(double a) const { return Vector2d(x / a, y / a); }
|
|
Vector2d operator*(double a) const { return Vector2d(x * a, y * a); }
|
|
Vector2d operator+=(const Vector2d &v) {
|
|
*this = *this + v;
|
|
return *this;
|
|
}
|
|
double norm() { return sqrt(x * x + y * y); }
|
|
double dot(Vector2d v) { return x * v.x + y * v.y; }
|
|
void reset() {
|
|
x = 0;
|
|
y = 0;
|
|
}
|
|
};
|
|
|
|
// Numer of pedestrians
|
|
// $n_p$
|
|
const int n_p = 100;
|
|
|
|
// Number of iterations
|
|
// $n_{iter}$
|
|
const int n_iter = 100;
|
|
|
|
// TODO: Change this
|
|
// $\tau$
|
|
const double tau = 0.12345;
|
|
|
|
void compute_acceleration_term(Vector2d *F_i) {}
|
|
void compute_repulsive_effects_from_pedestrians(Vector2d *F_i) {}
|
|
void compute_repulsive_effects_from_borders(Vector2d *F_i) {}
|
|
|
|
// Return an array of doubles
|
|
double *w(Vector2d e, Vector2d f) {
|
|
|
|
// TODO:
|
|
// $cos(60\degree)$
|
|
// if (e * f > f.norm() * (0.5)) {
|
|
// }
|
|
// return 0;
|
|
}
|
|
|
|
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];
|
|
|
|
for (int t = 0; t < n_iter; t++) {
|
|
for (int i = 0; i < n_p; i++) {
|
|
for (int itr = 0; itr < n_p; itr++) {
|
|
F[i][itr].reset();
|
|
}
|
|
compute_acceleration_term(F[i]);
|
|
compute_repulsive_effects_from_pedestrians(F[i]);
|
|
compute_repulsive_effects_from_borders(F[i]);
|
|
|
|
// TODO: Update $\vec{w}_i$
|
|
|
|
// TODO: Update $\vec{v}_i^{t+1}$
|
|
|
|
// TODO: pdate $\vec{r}_i^{t+1}$
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|