Merge branch 'variant_optimization_md_template' into 'main'
Variant optimization md template See merge request course-asl-fastcode26/team70!10
This commit is contained in:
salijagic
@@ -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
|
||||
```
|
||||
|
||||
---
|
||||
+130
@@ -0,0 +1,130 @@
|
||||
#include "forces.hpp"
|
||||
#include <math.h>
|
||||
|
||||
// # 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());
|
||||
}
|
||||
}
|
||||
@@ -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 <cmath>
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// 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);
|
||||
}
|
||||
Reference in New Issue
Block a user