diff --git a/OPTIMIZATIONS.md b/OPTIMIZATIONS.md new file mode 100644 index 0000000..2e8c66a --- /dev/null +++ b/OPTIMIZATIONS.md @@ -0,0 +1,163 @@ +# Optimizations of the Social Force Model + +## Table of Contents +1. [Performance Results](#performance-results) +2. [Baseline](#2-baseline) +3. [Optimization 1](#3-optimization-1) +4. [Optimization 2](#4-optimization-2) +5. [Optimization 3](#5-optimization-3) +6. [Optimization 4](#6-optimization-4) +7. [Optimization 5](#7-optimization-5) +8. [Optimization 6](#8-optimization-6) +9. [Optimization 7](#9-optimization-7) +10. [Optimization 8](#10-optimization-8) +11. [Optimization 9](#11-optimization-9) + +--- + +## Performance Results + +| # | Optimization | Cycle Count | Author | +|---|---|---|---| +| 0 | Baseline | - | - | +| 1 | | | | +| 2 | | | | +| 3 | | | | +| 4 | | | | +| 5 | | | | +| 6 | | | | +| 7 | | | | +| 8 | | | | +| 9 | | | | + +--- + +## 2. Baseline + +**Author**: [Name] + +**Description**: +Standard implementation without optimizations. + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 3. Optimization 1 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 4. Optimization 2 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 5. Optimization 3 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 6. Optimization 4 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 7. Optimization 5 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 8. Optimization 6 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 9. Optimization 7 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 10. Optimization 8 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- + +## 11. Optimization 9 + +**Author**: [Name] + +**Description**: +[Briefly describe the optimization technique] + +```cpp +// Paste your code here only the relevant part +``` + +--- \ No newline at end of file diff --git a/forces.cpp b/forces.cpp new file mode 100644 index 0000000..0a74fea --- /dev/null +++ b/forces.cpp @@ -0,0 +1,130 @@ +#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 new file mode 100644 index 0000000..a12f968 --- /dev/null +++ b/tests.cpp @@ -0,0 +1,442 @@ +// 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