diff --git a/main.cpp b/main.cpp index 6e00475..bf8bca3 100644 --- a/main.cpp +++ b/main.cpp @@ -78,10 +78,12 @@ int main(int argc, const char *argv[]) { for (int i = 0; i < n_p; i++) { Pedestrian &p = pedestrians[i]; // # 4 flops = 2 ADDs, 2 MULTs - Vector2d w_alpha = p.curr_vel + p.f_i * dt; + // integrate preferred velocity: dw/dt = F (Eq. 10) + p.w = p.w + p.f_i * dt; + // Vector2d w_alpha = p.curr_vel + p.f_i * dt; // commented since this was using p.curr_vel instead of p.w (different from paper) // # 8 flops = 2 MULTs, 2 DIVs, 1 SQRT, 2 MULTs, 1 ADD - p.curr_vel = w_alpha * g(p.v_max, w_alpha.norm()); + p.curr_vel = p.w * g(p.v_max, p.w.norm()); // # 4 flops = 2 ADDs, 2 MULTs p.curr_pos = p.curr_pos + p.curr_vel * dt; diff --git a/pedestrian.hpp b/pedestrian.hpp index 54b3abe..cbe0009 100644 --- a/pedestrian.hpp +++ b/pedestrian.hpp @@ -7,7 +7,8 @@ enum Color { BLACK, WHITE }; struct Pedestrian { Color color; - Vector2d curr_vel; + Vector2d curr_vel; // v_alpha (actual, ≤ v_max) + Vector2d w; // w_alpha (preferred, integrates force) Vector2d curr_pos; Vector2d e; double v_max; @@ -17,5 +18,7 @@ struct Pedestrian { Pedestrian() : color(WHITE), curr_vel(0.0, 0.0), curr_pos(0.0, 0.0), e(0.0, 0.0), v_max(0.0), desired_v0(0.0), destination(0.0, 0.0), f_i(0.0, 0.0) {} - Pedestrian(Color clr, Vector2d c_v, Vector2d c_p, Vector2d e, double v_max, double desired_v0, Vector2d d, Vector2d f) : color(clr), curr_vel(c_v), curr_pos(c_p), e(e), v_max(v_max), desired_v0(desired_v0), destination(d), f_i(f) {} + Pedestrian(Color clr, Vector2d c_v, Vector2d c_p, Vector2d e, double v_max, double desired_v0, Vector2d d, Vector2d f) + : color(clr), curr_vel(c_v), w(c_v), // initialize w = v at t=0 + curr_pos(c_p), e(e), v_max(v_max), desired_v0(desired_v0), destination(d), f_i(f) {} };