[Quy Anh] «Elliot» Nguyen
115 lines
6.9 KiB
C++
115 lines
6.9 KiB
C++
#pragma once
|
|
#include "board.hpp"
|
|
#include "pedestrian.hpp"
|
|
#include "vector2d.hpp"
|
|
|
|
// Note: force contributions are multiplied by dt inside each function because
|
|
// this allows us to bake dt into precomputed constants and thus save two
|
|
// multiplications in the main loop. Conceptually, each returned term is the
|
|
// explicit-Euler approximation of the time integral of one force contribution
|
|
// over a single simulation step.
|
|
//
|
|
// This is valid because integration is linear: the integral of a sum is the sum
|
|
// of the integrals. Thus, summing F_i * dt for each contribution is equivalent
|
|
// to computing dt * sum(F_i) after all force contributions have been accumulated.
|
|
|
|
// Computes the direction-dependent perception weight from Eq. (7) in the paper:
|
|
//
|
|
// $w(\vec{e}_{\alpha}, \vec{f}) = \begin{cases} 1.0, & \text{if } \vec{e}_{\alpha} \cdot \vec{f} \geq \|\vec{f}\| \cos(\varphi), \\ c, & \text{otherwise}. \end{cases}$
|
|
//
|
|
// where:
|
|
// - $\vec{e}_{\alpha}$ is the desired direction of motion of the pedestrian,
|
|
// - $\vec{f}$ is the force under consideration (e.g., the repulsive force from another pedestrian or an obstacle),
|
|
// - $\varphi$ is half of the effective angle of sight,
|
|
// - $c$ is the reduced weight factor for situations outside the field of view.
|
|
//
|
|
// The function returns:
|
|
//
|
|
// $w(\vec{e}_{\alpha}, \vec{f}).(-dt)$
|
|
//
|
|
// Note: the reason we multiply the perception weight by -dt is:
|
|
// (1) the factor dt saves a multiplication by dt, in pedestrian_repulsion_dt and obstacle_repulsion_dt;
|
|
// (2) the minus sign is applied because the repulsion routines (pedestrian_repulsion_dt and obstacle_repulsion_dt) compute and
|
|
// pass the negated force direction to w(), matching the paper's w(e_alpha, -f) term. Returning -w() then converts that internally
|
|
// negated force back to the correct signed result vector without needing to perform any vector negation, in order to meet the paper's formula.
|
|
//
|
|
// @param e : The desired direction of motion of the pedestrian.
|
|
// @param f : The force under consideration (e.g., the repulsive force from another pedestrian or an obstacle).
|
|
// @return The direction-dependent perception weight multiplied by -dt.
|
|
//
|
|
// pedestrian-pedestrian weighting (full e dot f vs ||f||*cos(phi) check)
|
|
double w_ped(Vector2d e, Vector2d f);
|
|
|
|
// obstacle weighting (cheaper: obstacles are x-axis walls so only f.y matters)
|
|
double w_obstacle(Vector2d e, Vector2d f);
|
|
|
|
// Computes the acceleration term of the social force model
|
|
// for pedestrian alpha (Eq. (2) in the paper):
|
|
//
|
|
// $\vec{F}^0_\alpha = \frac{1}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
|
|
//
|
|
// where:
|
|
// - $v^0_\alpha$ is the desired speed,
|
|
// - $\vec{e}_\alpha$ is the desired direction,
|
|
// - $\vec{v}_\alpha$ is the current velocity,
|
|
// - $\tau_\alpha$ is the relaxation time.
|
|
//
|
|
// This function returns $\vec{F}^0_\alpha.dt$, i.e. the velocity increment caused by
|
|
// the acceleration term during one simulation time step:
|
|
//
|
|
// $\vec{F}^0_\alpha.dt = \frac{dt}{\tau_\alpha}(v^0_\alpha \vec{e}_\alpha - \vec{v}_\alpha)$
|
|
//
|
|
// @param alpha : The pedestrian for which the acceleration contribution is computed.
|
|
// @return The velocity increment caused by the acceleration term during one simulation time step.
|
|
//
|
|
Vector2d acceleration_term_dt(const Pedestrian &alpha);
|
|
|
|
// Computes the repulsive pedestrian-pedestrian effect
|
|
// exerted by pedestrian beta on pedestrian alpha (Eq. (3) in the paper),
|
|
// weighted by the direction-dependent perception factor from Eq. (7)-(8):
|
|
//
|
|
// $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}) = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta})$
|
|
//
|
|
// where:
|
|
// - $\vec{r}_{\alpha \beta} = \vec{r}_\alpha - \vec{r}_\beta$ is the vector from beta's position to alpha's position,
|
|
// - $\vec{e}_\alpha$ is the desired direction of pedestrian alpha,
|
|
//
|
|
// - $\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}) = -\nabla_{\vec{r}_{\alpha\beta}} V_{\alpha\beta}\left[b(\vec{r}_{\alpha\beta})\right]$ is the repulsive effect from beta on alpha (The expanded formula details are given in the code comments given inside the function's implementation in variants_baseline.cpp),
|
|
//
|
|
// - $w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))$ the direction-dependent perception weighting function, for which the formula is detailed in the w() function above.
|
|
//
|
|
// This function returns $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}).dt$, i.e. the velocity increment caused by the repulsive pedestrian-pedestrian effect during one simulation time step:
|
|
//
|
|
// $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha \beta}).dt = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}))\vec{f}_{\alpha\beta}(\vec{r}_{\alpha \beta}).dt$
|
|
//
|
|
// @param alpha : The pedestrian receiving the repulsive effect.
|
|
// @param beta : The pedestrian producing the repulsive effect.
|
|
// @return The velocity increment caused by the repulsive effect from pedestrian beta on alpha during one
|
|
// simulation time step.
|
|
//
|
|
Vector2d pedestrian_repulsion_dt(const Pedestrian &alpha, const Pedestrian &beta);
|
|
|
|
// Computes the repulsive effect exerted by obstacle B
|
|
// on pedestrian alpha (Eq. (5) in the paper), weighted by the direction-dependent perception factor from Eq. (7)-(8]:
|
|
//
|
|
// $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}) = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))\vec{F}_{\alpha B}(\vec{r}_{\alpha B})$
|
|
//
|
|
// where:
|
|
// - $\vec{r}_{\alpha B} = \vec{r}_\alpha - \vec{r}^{\,\alpha}_B$ is the vector from the closest point on obstacle B (i.e., $\vec{r}^{\,\alpha}_B$) to pedestrian alpha's position,
|
|
// - $\vec{e}_\alpha$ is the desired direction of pedestrian alpha,
|
|
//
|
|
// - $\vec{F}_{\alpha B}(\vec{r}_{\alpha B}) = -\nabla_{\vec{r}_{\alpha B}} U_{\alpha B}(||\vec{r}_{\alpha B}||)$ is the repulsive effect from obstacle B on alpha (The expanded formula details are given in the code comments given inside the function's implementation in variant_baseline.cpp),
|
|
//
|
|
// - $w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))$ the direction-dependent perception weighting function, for which the formula is detailed in the w() function above.
|
|
//
|
|
// This function returns $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}).dt$, i.e. the velocity increment caused by the repulsive obstacle effect during one simulation time step:
|
|
//
|
|
// $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha B}).dt = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B}(\vec{r}_{\alpha B}))\vec{F}_{\alpha B}(\vec{r}_{\alpha B}).dt$
|
|
//
|
|
// @param alpha : The pedestrian receiving the obstacle repulsive effect.
|
|
// @param B : The obstacle producing the repulsive effect.
|
|
// @return The velocity increment caused by the repulsive effect from obstacle B on alpha during one
|
|
// simulation time step.
|
|
//
|
|
Vector2d obstacle_repulsion_dt(const Pedestrian &alpha, const Obstacle &B);
|