diff --git a/derivations.jpg b/derivations.jpg deleted file mode 100644 index 65cdc3c..0000000 Binary files a/derivations.jpg and /dev/null differ diff --git a/forces.cpp b/forces.cpp deleted file mode 100644 index 0a74fea..0000000 --- a/forces.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "forces.hpp" -#include - -// # 2 ADDs, 5 MULTs, 1 SQRT -double w(Vector2d e, Vector2d f) { - if (e.dot(f) >= f.norm() * cos_phi) - return 1.0; - else - return c; -} - -// # 1 DIV -double g(double v_max_alpha, double w_alpha_norm) { - - // # Assumption for calculating W(n): The "if" statement always evaluates to false. - if (w_alpha_norm <= v_max_alpha) { - return 1; - } - - return v_max_alpha / w_alpha_norm; -} - -// # 6 ADDs, 2 DIVs -void add_acceleration_term(Pedestrian *p) { - - // # 2 ADDs - Vector2d desired_velocity = p->e * p->desired_v0; - - Vector2d actual_velocity = p->curr_vel; - - // # 4 ADDs, 2 DIVs - p->f_i += (desired_velocity - actual_velocity) / tau_alpha; -} - -// # 15 ADDs, 24 MULTs, 8 DIVs, 5 SQRTs, 1 EXP -// -// $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta})$ -// -void add_repulsive_effects_from_pedestrians(Pedestrian *pedestrians, Pedestrian *p, int N) { - - for (int i = 0; i < N; i++) { - - if (pedestrians + i == p) - continue; - - // # 2 ADDs - // $\vec{r}_{\alpha\beta}$ - Vector2d r_ab = p->curr_pos - pedestrians[i].curr_pos; - - // # 1 ADD, 2 MULTs, 1 SQRT - // $\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert$ - double r_ab_norm = r_ab.norm(); - - // $\vec{e}_{\beta}$ - Vector2d e_b = pedestrians[i].e; - - // # 1 ADD, 3 MULTs, 1 SQRT - // $v_\beta \Delta t$ - double s_b = pedestrians[i].curr_vel.norm() * delta_t; - - // # 2 ADDs, 2 MULTs - // $\vec{r}_{\alpha\beta} - \vec{v}_{\beta}\varDelta_{t}\vec{e}_{\beta}$ - Vector2d r_ab2 = r_ab - (e_b * s_b); - - // # 1 ADD, 2 MULTs, 1 SQRT - // $\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert$ - double r_ab2_norm = r_ab2.norm(); - - // # 1 ADD - double r_ab_norm_sum = r_ab_norm + r_ab2_norm; - - // # 1 ADD, 2 MULTs, 1 DIV, 1 SQRT - // - // $2b = \sqrt{(\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert)^2 - (v_\beta \Delta t)^2}$ - // - double b = sqrt(r_ab_norm_sum * r_ab_norm_sum - s_b * s_b) / 2; - - // # 2 ADDs, 3 MULTs, 6 DIVs - // - // $\grad_{\vec{r}_{\alpha \beta}}b = \frac{(\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert + \Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert) (\frac{\vec{r}_{\alpha \beta}}{\Arrowvert \vec{r}_{\alpha \beta} \Arrowvert} + \frac{\vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta}{\Arrowvert \vec{r}_{\alpha \beta} - v_\beta \Delta t \vec{e}_\beta \Arrowvert})}{4b}$ - // - Vector2d b_grad = ((r_ab / r_ab_norm + r_ab2 / r_ab2_norm) * r_ab_norm_sum) / (4 * b); - - // # 3 MULTs, 1 DIV, 1 EXP - // - // $\vec{f}_{\alpha \beta}(\vec{r}_{\alpha \beta}) = \frac{V^0_{\alpha \beta}}{\sigma}e^{-b / \sigma} \grad_{\vec{r}_{\alpha \beta}}b$ - // - Vector2d f_ped = b_grad * (V0_AB_div_SIGMA * exp(-b / SIGMA)); - - // # 4 ADDs, 7 MULTs, 1 SQRT - // - // $\vec{F}_{\alpha\beta}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{\beta}) = w(\vec{e}_{\alpha}, -\vec{f}_{\alpha\beta})\vec{f}_{\alpha\beta}(\vec{r}_{\alpha} - \vec{r}_{\beta})$ - // - p->f_i += f_ped * w(p->e, f_ped.reverse_sign()); - } -} - -// # board.n_obstacles * (13 ADDs, 19 MULTs, 4 DIVs, 2 SQRTs, 1 EXP) -// -// $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{B})$ -// -void add_repulsive_effects_from_borders(Board &board, Pedestrian *p) { - - Obstacle *obstacles = board.obstacles; - int n_borders = board.n_obstacles; - - for (int i = 0; i < n_borders; i++) { - - Obstacle obstacle = obstacles[i]; - - // # 8 ADDs, 6 MULTs, 1 DIV - // $\vec{r}_{\alpha B}$ - Vector2d r_aB = p->curr_pos - obstacle.find_nearest_obstacle_point(p->curr_pos); - - // # 1 ADD, 2 MULTs, 1 SQRT - double r_aB_norm = r_aB.norm(); - - // # 4 MULTs, 3 DIVs, 1 EXP - // - // $\vec{F}_{\alpha B}(\vec{r}_{\alpha B}) = \frac{U^0_{\alpha B}}{R} e^{- \Arrowvert \vec{r}_{\alpha B} \Arrowvert / R} \frac{\vec{r}_{\alpha B}}{\Arrowvert \vec{r}_{\alpha B} \Arrowvert}$ - // - Vector2d f_obst = (r_aB / r_aB_norm) * U0_AB_div_R * exp(-r_aB_norm / R); - - // # 4 ADDS, 7 MULTs, 1 SQRT - // - // $\vec{F}_{\alpha B}(\vec{e}_{\alpha}, \vec{r}_{\alpha} - \vec{r}_{ B}) = w(\vec{e}_{\alpha}, -\vec{F}_{\alpha B})\vec{F}_{\alpha B}(\vec{r}_{\alpha} - \vec{r}_{ B})$ - // - p->f_i += f_obst * w(p->e, f_obst.reverse_sign()); - } -} diff --git a/tests.cpp b/tests.cpp deleted file mode 100644 index a12f968..0000000 --- a/tests.cpp +++ /dev/null @@ -1,442 +0,0 @@ -// Unit tests for the Social Force Model. -// -// Build (from this directory): -// g++ -std=c++17 -O2 tests.cpp forces.cpp -lgtest -lgtest_main -lpthread -o tests -// -// Run: -// ./tests - -#include "board.hpp" -#include "forces.hpp" -#include "pedestrian.hpp" -#include "vector2d.hpp" - -#include -#include - -// --------------------------------------------------------------------------- -// Helpers -// --------------------------------------------------------------------------- - -namespace { - -// Tolerance for floating-point comparisons. The model uses exp/sqrt/division -// and chains several of them, so we leave some headroom over machine epsilon. -constexpr double kEps = 1e-9; -constexpr double kEpsLoose = 1e-6; - -::testing::AssertionResult Vec2Near(const Vector2d& a, const Vector2d& b, - double eps = kEps) { - if (std::fabs(a.x - b.x) <= eps && std::fabs(a.y - b.y) <= eps) { - return ::testing::AssertionSuccess(); - } - return ::testing::AssertionFailure() - << "vectors differ: got (" << a.x << ", " << a.y << "), expected (" - << b.x << ", " << b.y << "), eps=" << eps; -} - -// Build a pedestrian at `pos` heading toward +x with the given desired speed. -Pedestrian MakePedestrian(Vector2d pos, Vector2d vel, Vector2d dir, - double v0 = 1.34) { - Pedestrian p; - p.curr_pos = pos; - p.curr_vel = vel; - p.w = vel; - p.e = dir; - p.v_max = 1.3 * v0; - p.desired_v0 = v0; - p.destination = Vector2d(100.0, pos.y); - p.f_i = Vector2d(0.0, 0.0); - return p; -} - -} // namespace - -// =========================================================================== -// Vector2d -// =========================================================================== - -TEST(Vector2dTest, DefaultConstructorIsZero) { - Vector2d v; - EXPECT_DOUBLE_EQ(v.x, 0.0); - EXPECT_DOUBLE_EQ(v.y, 0.0); -} - -TEST(Vector2dTest, ValueConstructor) { - Vector2d v(3.0, -4.0); - EXPECT_DOUBLE_EQ(v.x, 3.0); - EXPECT_DOUBLE_EQ(v.y, -4.0); -} - -TEST(Vector2dTest, AddAndSubtract) { - Vector2d a(1.0, 2.0); - Vector2d b(3.0, 5.0); - EXPECT_TRUE(Vec2Near(a + b, Vector2d(4.0, 7.0))); - EXPECT_TRUE(Vec2Near(a - b, Vector2d(-2.0, -3.0))); -} - -TEST(Vector2dTest, ScalarMulAndDiv) { - Vector2d a(2.0, -3.0); - EXPECT_TRUE(Vec2Near(a * 2.5, Vector2d(5.0, -7.5))); - EXPECT_TRUE(Vec2Near(a / 2.0, Vector2d(1.0, -1.5))); -} - -TEST(Vector2dTest, ScalarAddAndSub) { - Vector2d a(2.0, -3.0); - EXPECT_TRUE(Vec2Near(a + 1.0, Vector2d(3.0, -2.0))); - EXPECT_TRUE(Vec2Near(a - 1.0, Vector2d(1.0, -4.0))); -} - -TEST(Vector2dTest, CompoundAssignment) { - Vector2d a(1.0, 2.0); - a += Vector2d(0.5, -1.0); - EXPECT_TRUE(Vec2Near(a, Vector2d(1.5, 1.0))); -} - -TEST(Vector2dTest, ReverseSign) { - Vector2d a(1.5, -2.5); - EXPECT_TRUE(Vec2Near(a.reverse_sign(), Vector2d(-1.5, 2.5))); - Vector2d zero(0.0, 0.0); - EXPECT_TRUE(Vec2Near(zero.reverse_sign(), Vector2d(0.0, 0.0))); -} - -TEST(Vector2dTest, NormBasic) { - EXPECT_NEAR(Vector2d(3.0, 4.0).norm(), 5.0, kEps); - EXPECT_NEAR(Vector2d(0.0, 0.0).norm(), 0.0, kEps); - EXPECT_NEAR(Vector2d(-3.0, -4.0).norm(), 5.0, kEps); -} - -TEST(Vector2dTest, Dot) { - Vector2d a(1.0, 2.0); - Vector2d b(3.0, 4.0); - EXPECT_NEAR(a.dot(b), 11.0, kEps); - // orthogonal - EXPECT_NEAR(Vector2d(1.0, 0.0).dot(Vector2d(0.0, 1.0)), 0.0, kEps); - // anti-parallel - EXPECT_NEAR(Vector2d(1.0, 0.0).dot(Vector2d(-1.0, 0.0)), -1.0, kEps); -} - -TEST(Vector2dTest, Reset) { - Vector2d a(7.0, -8.0); - a.reset(); - EXPECT_TRUE(Vec2Near(a, Vector2d(0.0, 0.0))); -} - -// =========================================================================== -// Obstacle::find_nearest_obstacle_point -// =========================================================================== - -TEST(ObstacleTest, ProjectionOntoMiddleOfSegment) { - // Horizontal segment from (0,0) to (10,0); point above the middle. - Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0)); - Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(5.0, 3.0)); - EXPECT_TRUE(Vec2Near(nearest, Vector2d(5.0, 0.0))); -} - -TEST(ObstacleTest, ProjectionClampedToStart) { - // Point lies "before" the segment start -> nearest is start. - Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0)); - Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(-5.0, 2.0)); - EXPECT_TRUE(Vec2Near(nearest, Vector2d(0.0, 0.0))); -} - -TEST(ObstacleTest, ProjectionClampedToEnd) { - // Point lies "after" the segment end -> nearest is end. - Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0)); - Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(20.0, 2.0)); - EXPECT_TRUE(Vec2Near(nearest, Vector2d(10.0, 0.0))); -} - -TEST(ObstacleTest, ProjectionOnDiagonalSegment) { - // Diagonal from (0,0) to (4,4); query (4,0) projects onto (2,2). - Obstacle obs(Vector2d(0.0, 0.0), Vector2d(4.0, 4.0)); - Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(4.0, 0.0)); - EXPECT_TRUE(Vec2Near(nearest, Vector2d(2.0, 2.0))); -} - -TEST(ObstacleTest, PointOnSegmentItself) { - Obstacle obs(Vector2d(0.0, 0.0), Vector2d(10.0, 0.0)); - Vector2d nearest = obs.find_nearest_obstacle_point(Vector2d(7.0, 0.0)); - EXPECT_TRUE(Vec2Near(nearest, Vector2d(7.0, 0.0))); -} - -// =========================================================================== -// Board -// =========================================================================== - -TEST(BoardTest, AddsFourBorderObstacles) { - Obstacle inner[1] = {Obstacle(Vector2d(2.0, 2.0), Vector2d(3.0, 3.0))}; - Board b(10.0, 5.0, 1, inner); - - ASSERT_EQ(b.n_obstacles, 5); - // First entry is the user-supplied obstacle. - EXPECT_TRUE(Vec2Near(b.obstacles[0].start, Vector2d(2.0, 2.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[0].end, Vector2d(3.0, 3.0))); - - // Borders: bottom, right, top, left. - EXPECT_TRUE(Vec2Near(b.obstacles[1].start, Vector2d(0.0, 0.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[1].end, Vector2d(10.0, 0.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[2].start, Vector2d(10.0, 0.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[2].end, Vector2d(10.0, 5.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[3].start, Vector2d(10.0, 5.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[3].end, Vector2d(0.0, 5.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[4].start, Vector2d(0.0, 5.0))); - EXPECT_TRUE(Vec2Near(b.obstacles[4].end, Vector2d(0.0, 0.0))); -} - -TEST(BoardTest, ZeroInnerObstaclesYieldsFourBorders) { - Board b(8.0, 4.0, 0, nullptr); - EXPECT_EQ(b.n_obstacles, 4); -} - -// =========================================================================== -// w(e, f) -- field-of-view weighting -// =========================================================================== - -TEST(WeightingTest, FullWeightWhenForceIsInForwardCone) { - // e points along +x; force along +x is dead-ahead -> w = 1. - Vector2d e(1.0, 0.0); - Vector2d f(2.0, 0.0); - EXPECT_NEAR(w(e, f), 1.0, kEps); -} - -TEST(WeightingTest, ReducedWeightWhenForceIsBehind) { - // Force points behind the pedestrian -> w = c. - Vector2d e(1.0, 0.0); - Vector2d f(-2.0, 0.0); - EXPECT_NEAR(w(e, f), c, kEps); -} - -TEST(WeightingTest, BoundaryAtPhi) { - // Vector exactly at +phi (45 deg) from e: dot product equals |f|*cos(phi), - // so the condition `>= |f|*cos(phi)` is true and weight is 1. - Vector2d e(1.0, 0.0); - double angle = phi; - Vector2d f(std::cos(angle), std::sin(angle)); - EXPECT_NEAR(w(e, f), 1.0, kEps); -} - -TEST(WeightingTest, JustOutsideConeReturnsC) { - // Slightly beyond phi -> outside cone -> weight is c. - Vector2d e(1.0, 0.0); - double angle = phi + 1e-3; - Vector2d f(std::cos(angle), std::sin(angle)); - EXPECT_NEAR(w(e, f), c, kEps); -} - -// =========================================================================== -// g(v_max, |w|) -// =========================================================================== - -TEST(GFunctionTest, ReturnsOneWhenWithinMaxSpeed) { - EXPECT_NEAR(g(2.0, 1.5), 1.0, kEps); - EXPECT_NEAR(g(2.0, 2.0), 1.0, kEps); // boundary -} - -TEST(GFunctionTest, ReturnsRatioWhenAboveMaxSpeed) { - EXPECT_NEAR(g(2.0, 4.0), 0.5, kEps); - EXPECT_NEAR(g(1.0, 5.0), 0.2, kEps); -} - -// =========================================================================== -// add_acceleration_term -// =========================================================================== - -TEST(AccelerationTest, ZeroWhenAtDesiredVelocity) { - // Desired velocity = e * v0; if curr_vel matches, the term is zero. - Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(1.34, 0.0), - Vector2d(1.0, 0.0), 1.34); - add_acceleration_term(&p); - EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(0.0, 0.0))); -} - -TEST(AccelerationTest, PointsTowardDesiredDirectionFromRest) { - // At rest, the term should be (e*v0 - 0)/tau = (v0/tau, 0). - Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0), 1.34); - add_acceleration_term(&p); - EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(1.34 / tau_alpha, 0.0))); -} - -TEST(AccelerationTest, AccumulatesIntoExistingForce) { - // Pre-load f_i; verify that the function adds (does not overwrite). - Pedestrian p = MakePedestrian(Vector2d(0, 0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0), 1.34); - p.f_i = Vector2d(0.5, -0.25); - add_acceleration_term(&p); - EXPECT_TRUE(Vec2Near(p.f_i, Vector2d(0.5 + 1.34 / tau_alpha, -0.25))); -} - -// =========================================================================== -// add_repulsive_effects_from_pedestrians -// =========================================================================== - -TEST(PedestrianRepulsionTest, NoSelfInteraction) { - // Single pedestrian, N=1: loop body must skip self and leave f_i untouched. - Pedestrian peds[1]; - peds[0] = MakePedestrian(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0)); - peds[0].f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_pedestrians(peds, &peds[0], 1); - EXPECT_TRUE(Vec2Near(peds[0].f_i, Vector2d(0.0, 0.0))); -} - -TEST(PedestrianRepulsionTest, ForceOnAlphaPushesAwayFromBeta) { - // Beta is to the right of alpha -> force on alpha should push it left. - Pedestrian peds[2]; - peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(-1.0, 0.0)); - peds[0].f_i = Vector2d(0.0, 0.0); - - add_repulsive_effects_from_pedestrians(peds, &peds[0], 2); - - // Repulsion should push alpha in the -x direction. - EXPECT_LT(peds[0].f_i.x, 0.0); - // By symmetry of the configuration (both stationary, beta directly in front - // along x), the y-component should be ~0. - EXPECT_NEAR(peds[0].f_i.y, 0.0, kEpsLoose); -} - -TEST(PedestrianRepulsionTest, ForceMagnitudeDecreasesWithDistance) { - // Same setup, two distances; the closer pair should produce a larger |f_i|. - auto force_magnitude_at = [](double distance) { - Pedestrian peds[2]; - peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds[1] = MakePedestrian(Vector2d(distance, 0.0), Vector2d(0.0, 0.0), - Vector2d(-1.0, 0.0)); - peds[0].f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_pedestrians(peds, &peds[0], 2); - return peds[0].f_i.norm(); - }; - - EXPECT_GT(force_magnitude_at(0.5), force_magnitude_at(2.0)); - EXPECT_GT(force_magnitude_at(2.0), force_magnitude_at(5.0)); -} - -TEST(PedestrianRepulsionTest, SuperpositionMirrorSymmetryOnPerpendicularAxis) { - // Two betas placed symmetrically on +y and -y of alpha, with alpha facing +x. - // The configuration is mirror-symmetric across the x-axis (alpha's facing - // axis) so the y-components must cancel exactly. The x-components stack - // (both betas contribute equally and lie in alpha's forward field of view) - // and should each push alpha in -x. - Pedestrian peds[3]; - peds[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds[1] = MakePedestrian(Vector2d(0.0, 1.0), Vector2d(0.0, 0.0), - Vector2d(0.0, -1.0)); - peds[2] = MakePedestrian(Vector2d(0.0, -1.0), Vector2d(0.0, 0.0), - Vector2d(0.0, 1.0)); - peds[0].f_i = Vector2d(0.0, 0.0); - - add_repulsive_effects_from_pedestrians(peds, &peds[0], 3); - - // y-components cancel by mirror symmetry across alpha's facing axis. - EXPECT_NEAR(peds[0].f_i.y, 0.0, kEpsLoose); - // No net x-push: r_ab is purely along ±y for both betas, and beta velocities - // are zero, so the pedestrian repulsion has no x-component. - EXPECT_NEAR(peds[0].f_i.x, 0.0, kEpsLoose); -} - -TEST(PedestrianRepulsionTest, FieldOfViewAttenuatesBackwardNeighbor) { - // Two betas symmetric on ±x of alpha, who is facing +x. The front beta is - // in the FOV cone (w=1); the back beta is outside (w=c). The forces do NOT - // cancel: alpha gets pushed backward (in -x, i.e. away from the front beta, - // attenuated repulsion from behind). - Pedestrian peds_front_only[2]; - peds_front_only[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds_front_only[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(-1.0, 0.0)); - peds_front_only[0].f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_pedestrians(peds_front_only, &peds_front_only[0], - 2); - double f_front = peds_front_only[0].f_i.x; // negative - - Pedestrian peds_both[3]; - peds_both[0] = MakePedestrian(Vector2d(0.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds_both[1] = MakePedestrian(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(-1.0, 0.0)); - peds_both[2] = MakePedestrian(Vector2d(-1.0, 0.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - peds_both[0].f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_pedestrians(peds_both, &peds_both[0], 3); - double f_both = peds_both[0].f_i.x; - - // Adding the rear beta (with weight c) pushes alpha forward by exactly - // c * |contribution of one beta at distance 1|. The combined x-force should - // therefore equal f_front * (1 - c). - EXPECT_NEAR(f_both, f_front * (1.0 - c), kEpsLoose); - // Sanity: with c < 1, the net push is still in -x but smaller in magnitude. - EXPECT_LT(f_both, 0.0); - EXPECT_GT(f_both, f_front); -} - -// =========================================================================== -// add_repulsive_effects_from_borders -// =========================================================================== - -TEST(BorderRepulsionTest, ForcePushesAwayFromNearestBorder) { - // Empty inner-obstacle board so only the four walls act on the pedestrian. - Board board(10.0, 10.0, 0, nullptr); - Pedestrian p = MakePedestrian(Vector2d(0.5, 5.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - p.f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_borders(board, &p); - - // Pedestrian is closest to the left wall, so the dominant component of the - // border force should push in +x. - EXPECT_GT(p.f_i.x, 0.0); -} - -TEST(BorderRepulsionTest, SymmetryAtCenterCancelsOut) { - // At the exact center of a square board, the four walls should produce - // equal and opposite forces -> net force is ~zero. - Board board(10.0, 10.0, 0, nullptr); - Pedestrian p = MakePedestrian(Vector2d(5.0, 5.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - p.f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_borders(board, &p); - - EXPECT_NEAR(p.f_i.x, 0.0, kEpsLoose); - EXPECT_NEAR(p.f_i.y, 0.0, kEpsLoose); -} - -TEST(BorderRepulsionTest, ForceDecaysWithDistance) { - // Force on a pedestrian closer to the left wall should be larger than - // the force on one farther from it. - auto x_force_at = [](double x) { - Board board(20.0, 10.0, 0, nullptr); - Pedestrian p = MakePedestrian(Vector2d(x, 5.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - p.f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_borders(board, &p); - return p.f_i.x; // expect positive (pushed right) - }; - - // x = 0.3 is closer to the left wall than x = 1.0; the +x push should be - // larger and clearly above what the right wall contributes back at x = 0.3. - EXPECT_GT(x_force_at(0.3), x_force_at(1.0)); -} - -TEST(BorderRepulsionTest, InnerObstacleContributesForce) { - // A wall placed right next to the pedestrian should produce a much larger - // force than walls alone. - auto force_norm = [](Obstacle* extra, int n_extra) { - Board board(10.0, 10.0, n_extra, extra); - Pedestrian p = MakePedestrian(Vector2d(5.0, 5.0), Vector2d(0.0, 0.0), - Vector2d(1.0, 0.0)); - p.f_i = Vector2d(0.0, 0.0); - add_repulsive_effects_from_borders(board, &p); - return p.f_i.norm(); - }; - - double baseline = force_norm(nullptr, 0); - Obstacle near_wall[1] = { - Obstacle(Vector2d(5.1, 4.0), Vector2d(5.1, 6.0))}; - double with_obstacle = force_norm(near_wall, 1); - - EXPECT_GT(with_obstacle, baseline + 1e-3); -} \ No newline at end of file