Inlining + Reorganization + Roofline
This commit is contained in:
[Quy Anh] «Elliot» Nguyen
committed by
Linus Baumberger
Linus Baumberger
parent
43f11a661b
commit
b86c856e08
@@ -0,0 +1,419 @@
|
||||
// =============================================================================
|
||||
// Tests for forces.hpp
|
||||
// =============================================================================
|
||||
|
||||
#include "../headers/board.hpp"
|
||||
#include "../headers/constants.hpp"
|
||||
#include "../headers/forces.hpp"
|
||||
#include "../headers/pedestrian.hpp"
|
||||
#include "../headers/vector2d.hpp"
|
||||
#include "../headers/test_helpers.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
namespace {
|
||||
|
||||
// Build a pedestrian with default testing values so each test only has to override what it actually cares about.
|
||||
Pedestrian MakePed(Vector2d pos = Vector2d(0.0, 0.0), Vector2d vel = Vector2d(1.0, 0.0), Vector2d e = Vector2d(1.0, 0.0), double v_max = 1.5, double desired_v0 = 1.15, Vector2d dest = Vector2d(100.0, 0.0)) { return Pedestrian(WHITE, vel, pos, e, v_max, desired_v0, dest); }
|
||||
|
||||
// One Euler step, mirroring social_force_baseline.cpp:
|
||||
// w_new = w + total_force_dt
|
||||
// v_new = w_new * min(1, v_max / ||w_new||)
|
||||
// pos_new = pos + v_new * dt
|
||||
struct StepResult {
|
||||
Vector2d w, v, pos;
|
||||
};
|
||||
|
||||
StepResult IntegrateOneStep(const Pedestrian &p, Vector2d total_force_dt) {
|
||||
Vector2d w_new = p.w + total_force_dt;
|
||||
double w_norm = w_new.norm();
|
||||
double cap = std::min(1.0, p.v_max / (w_norm > EPS ? w_norm : EPS));
|
||||
Vector2d v_new = w_new * cap;
|
||||
Vector2d pos_new = p.curr_pos + v_new * dt;
|
||||
return {w_new, v_new, pos_new};
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// =============================================================================
|
||||
// Tests for w(e, f)
|
||||
// =============================================================================
|
||||
|
||||
TEST(WeightFn, ForceAlignedWithDirectionIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(1.0, 0.0)), dt_neg); }
|
||||
|
||||
TEST(WeightFn, ForceOppositeToDirectionIsOutOfView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(-1.0, 0.0)), c_neg_times_dt); }
|
||||
|
||||
TEST(WeightFn, PerpendicularForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 1.0), Vector2d(-1.0, 1.0)), dt_neg); }
|
||||
|
||||
TEST(WeightFn, ZeroForceIsInView) { EXPECT_DOUBLE_EQ(w(Vector2d(1.0, 0.0), Vector2d(0.0, 0.0)), dt_neg); }
|
||||
|
||||
TEST(WeightFn, AngleSlightlyInsidePhiIsInView) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
const double angle = 99.0 / 180.0 * M_PI;
|
||||
Vector2d f(std::cos(angle), std::sin(angle));
|
||||
EXPECT_DOUBLE_EQ(w(e, f), dt_neg);
|
||||
}
|
||||
|
||||
TEST(WeightFn, AngleSlightlyOutsidePhiIsOutOfView) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
const double angle = 101.0 / 180.0 * M_PI;
|
||||
Vector2d f(std::cos(angle), std::sin(angle));
|
||||
EXPECT_DOUBLE_EQ(w(e, f), c_neg_times_dt);
|
||||
}
|
||||
|
||||
TEST(WeightFn, ScalingForceMagnitudeDoesNotChangeView) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
Vector2d f(0.7, 0.7);
|
||||
EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 1000.0));
|
||||
EXPECT_DOUBLE_EQ(w(e, f), w(e, f * 0.001));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Tests for acceleration_term_dt(alpha)
|
||||
// =============================================================================
|
||||
|
||||
TEST(AccelerationTerm, AtDesiredVelocityProducesNoForce) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
double v0 = 1.5;
|
||||
Pedestrian p = MakePed(Vector2d(), e * v0, e, 2.0, v0);
|
||||
EXPECT_VEC2_NEAR_TOL(acceleration_term_dt(p), Vector2d(0.0, 0.0), 0.0);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, BelowDesiredVelocityAccelerates) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
double v0 = 1.5;
|
||||
Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, 0.0), e, 2.0, v0);
|
||||
Vector2d expected = Vector2d(2.0, 0.0) * dt;
|
||||
EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
|
||||
EXPECT_GT(acceleration_term_dt(p).x, 0.0);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, AboveDesiredVelocityDecelerates) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
double v0 = 1.5;
|
||||
Pedestrian p = MakePed(Vector2d(), Vector2d(1.75, 0.0), e, 2.0, v0);
|
||||
Vector2d expected = Vector2d(-0.5, 0.0) * dt;
|
||||
EXPECT_VEC2_NEAR(acceleration_term_dt(p), expected);
|
||||
EXPECT_LT(acceleration_term_dt(p).x, 0.0);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, MovingBackwardProducesLargerForwardForceThanMovingForward) {
|
||||
Vector2d e(1.0, 0.0);
|
||||
Pedestrian forward = MakePed(Vector2d(3.0, 0.0), Vector2d(0.5, 0.0), e, 2.0, 1.5);
|
||||
Pedestrian backward = MakePed(Vector2d(3.0, 0.0), Vector2d(-1.0, 0.0), e, 2.0, 1.5);
|
||||
EXPECT_GT(acceleration_term_dt(backward).x, acceleration_term_dt(forward).x);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, DiagonalWithVelocity) {
|
||||
Pedestrian p = MakePed(Vector2d(), Vector2d(0.5, -0.25), Vector2d(1.0, 1.0), 5.0, 2.0);
|
||||
EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(3.0, 4.5) * dt);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, ZeroDesiredSpeed) {
|
||||
Pedestrian p = MakePed(Vector2d(), Vector2d(0.7, -0.3), Vector2d(1.0, 0.0), 2.0, 0.0);
|
||||
EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(-1.4, 0.6) * dt);
|
||||
}
|
||||
|
||||
TEST(AccelerationTerm, SidewaysVelocityProducesPerpendicularCorrection) {
|
||||
Pedestrian p = MakePed(Vector2d(), Vector2d(1.0, 0.5), Vector2d(1, 0), 1.3, 1.0);
|
||||
EXPECT_DOUBLE_EQ(acceleration_term_dt(p).x, 0.0);
|
||||
EXPECT_VEC2_NEAR(acceleration_term_dt(p), Vector2d(0.0, -1.0 * dt));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Tests for pedestrian_repulsion_dt(alpha, beta)
|
||||
// =============================================================================
|
||||
|
||||
TEST(PedRepulsion, DistantPedestriansProduceNegligibleForce) {
|
||||
Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
|
||||
Pedestrian b = MakePed(Vector2d(100.0, 0.0), Vector2d(-1.2, 0.0), Vector2d(-1.0, 0.0));
|
||||
Vector2d f = pedestrian_repulsion_dt(a, b);
|
||||
EXPECT_LT(f.norm(), 1e-30);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, CloseIsStrongerThanFar) {
|
||||
Pedestrian a = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
|
||||
Pedestrian close = MakePed(Vector2d(2.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
|
||||
Pedestrian far = MakePed(Vector2d(8.0, 0.0), Vector2d(-1.0, 0.0), Vector2d(-1.0, 0.0));
|
||||
EXPECT_GT(pedestrian_repulsion_dt(a, close).norm(), pedestrian_repulsion_dt(a, far).norm());
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, StationaryBetaInFront) {
|
||||
Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
|
||||
Vector2d expected(-0.24971795 * dt, 0.0);
|
||||
EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, StationaryBetaToTheSide) {
|
||||
Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, -100));
|
||||
Vector2d expected(0.0, -0.24971795 * dt);
|
||||
EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, StationaryBetaDiagonal) {
|
||||
Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, -100));
|
||||
Vector2d expected(-0.04439144 * dt, -0.04439144 * dt);
|
||||
EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), expected);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, HeadOnWithMovingBeta) {
|
||||
Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
|
||||
EXPECT_VEC2_NEAR(pedestrian_repulsion_dt(a, b), Vector2d((-0.02512853) * dt, 0.0));
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, OutOfViewAttenuatedByFactorC) {
|
||||
Pedestrian a_front = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
|
||||
Pedestrian a_back = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b_back = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
|
||||
|
||||
Vector2d f_in = pedestrian_repulsion_dt(a_front, b_front);
|
||||
Vector2d f_out = pedestrian_repulsion_dt(a_back, b_back);
|
||||
|
||||
EXPECT_NEAR(f_in.norm() / f_out.norm(), 1.0 / -c_neg, tolerance);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, ForceOnAlphaPointsAwayFromBeta) {
|
||||
Pedestrian a = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 1), 1.3, 1.0, Vector2d(100, 100));
|
||||
Pedestrian b = MakePed(Vector2d(1, 1), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0);
|
||||
Vector2d f = pedestrian_repulsion_dt(a, b);
|
||||
Vector2d to_b(1.0, 1.0);
|
||||
EXPECT_LT(f.dot(to_b), 0.0);
|
||||
}
|
||||
|
||||
TEST(PedRepulsion, OverlappingPedestriansProduceFiniteOutput) {
|
||||
Pedestrian a = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian b = MakePed(Vector2d(2, 2), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(0, 0));
|
||||
Vector2d f = pedestrian_repulsion_dt(a, b);
|
||||
EXPECT_TRUE(std::isfinite(f.x));
|
||||
EXPECT_TRUE(std::isfinite(f.y));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Tests for obstacle_repulsion_dt(alpha, B)
|
||||
// =============================================================================
|
||||
|
||||
TEST(ObstRepulsion, DistantObstacleProducesNegligibleForce) {
|
||||
Pedestrian p = MakePed(Vector2d(0.0, 0.0), Vector2d(1.0, 0.0), Vector2d(1.0, 0.0));
|
||||
Obstacle o(Vector2d(100.0, -50.0), Vector2d(100.0, 50.0));
|
||||
EXPECT_LT(obstacle_repulsion_dt(p, o).norm(), 1e-30);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, PerpendicularWallBelow) {
|
||||
Pedestrian p = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));
|
||||
Vector2d expected(0.0, 0.33689735 * dt);
|
||||
EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, ClampedToEndpoint) {
|
||||
Pedestrian p = MakePed(Vector2d(12, 0), Vector2d(0, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(0, 0));
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Vector2d expected(0.0022699965 * dt, 0.0);
|
||||
EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, ClampedToStart) {
|
||||
Pedestrian p = MakePed(Vector2d(-3, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Vector2d expected(-0.0000152951 * dt, 0.0);
|
||||
EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, OutOfViewAttenuatedByFactorC) {
|
||||
Pedestrian p = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(0, 1), 1.3, 1.0, Vector2d(5, 100));
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Vector2d expected(0.0, 0.33689735 * dt * -c_neg);
|
||||
EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, wall), expected);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, SinglePointObstacle) {
|
||||
Pedestrian p = MakePed(Vector2d(2, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Obstacle pt(Vector2d(5, 0), Vector2d(5, 0));
|
||||
Vector2d expected(-0.0000152951 * dt, 0.0);
|
||||
EXPECT_VEC2_NEAR(obstacle_repulsion_dt(p, pt), expected);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, ForceOnAlphaPointsAwayFromObstacle) {
|
||||
Pedestrian p = MakePed(Vector2d(5, 2), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 2));
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Vector2d f = obstacle_repulsion_dt(p, wall);
|
||||
EXPECT_GT(f.y, 0.0);
|
||||
EXPECT_NEAR(f.x, 0.0, 1e-12);
|
||||
}
|
||||
|
||||
TEST(ObstRepulsion, AlphaOnObstacleProducesFiniteOutput) {
|
||||
Pedestrian p = MakePed(Vector2d(5, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Vector2d f = obstacle_repulsion_dt(p, wall);
|
||||
EXPECT_TRUE(std::isfinite(f.x));
|
||||
EXPECT_TRUE(std::isfinite(f.y));
|
||||
EXPECT_VEC2_NEAR(f, Vector2d(0.0, 0.0));
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// Integration tests
|
||||
// =============================================================================
|
||||
|
||||
TEST(Integration, OnePedNoInteractions_AccelerationOnly) {
|
||||
Pedestrian alpha = MakePed(Vector2d(2, 3), Vector2d(0.5, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha);
|
||||
|
||||
Vector2d expected_total(dt, 0.0);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, Vector2d(0.52, 0.0));
|
||||
EXPECT_VEC2_NEAR(step.v, Vector2d(0.52, 0.0));
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(2.0 + 0.52 * dt, 3.0));
|
||||
}
|
||||
|
||||
TEST(Integration, OnePedOneStationaryBeta) {
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);
|
||||
|
||||
double Fa0_x = dt_times_tau_alpha_inv;
|
||||
double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
|
||||
Vector2d expected_total(Fa0_x + Fab_x, 0.0);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_total);
|
||||
EXPECT_LT(step.w.norm(), alpha.v_max);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, OnePedOneWall) {
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0.5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Obstacle wall(Vector2d(0, 0), Vector2d(10, 0));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, wall);
|
||||
|
||||
double Fa0_x = dt_times_tau_alpha_inv;
|
||||
double FaB_y = (U0_AB / R) * std::exp(-0.5 / R) * dt;
|
||||
Vector2d expected_total(Fa0_x, FaB_y);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_total);
|
||||
EXPECT_LT(step.w.norm(), alpha.v_max);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(0, 0.5) + expected_total * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, AllThreeForcesCombined) {
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian beta = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(-100, 0));
|
||||
Obstacle wall(Vector2d(0, -1), Vector2d(10, -1));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta) + obstacle_repulsion_dt(alpha, wall);
|
||||
|
||||
double Fa0_x = dt_times_tau_alpha_inv;
|
||||
double Fab_x = -(V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
|
||||
double FaB_y = (U0_AB / R) * std::exp(-1.0 / R) * dt;
|
||||
Vector2d expected_total(Fa0_x + Fab_x, FaB_y);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_total);
|
||||
EXPECT_LT(step.w.norm(), alpha.v_max);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.pos, expected_total * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, SymmetricStationaryBetas_VerticalForcesCancel) {
|
||||
Pedestrian alpha = MakePed(Vector2d(5, 5), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 5));
|
||||
Pedestrian b_north = MakePed(Vector2d(5, 6), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, 100));
|
||||
Pedestrian b_south = MakePed(Vector2d(5, 4), Vector2d(0, 0), Vector2d(0, 0), 1.3, 1.0, Vector2d(5, -100));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_north) + pedestrian_repulsion_dt(alpha, b_south);
|
||||
|
||||
Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 5) + expected_total * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, PedBetweenTwoParallelWalls_VerticalForcesCancel) {
|
||||
Pedestrian alpha = MakePed(Vector2d(5, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));
|
||||
Obstacle bottom(Vector2d(0, 0), Vector2d(10, 0));
|
||||
Obstacle top(Vector2d(0, 2), Vector2d(10, 2));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + obstacle_repulsion_dt(alpha, bottom) + obstacle_repulsion_dt(alpha, top);
|
||||
|
||||
Vector2d expected_total(dt_times_tau_alpha_inv, 0.0);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_total);
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(5, 1) + expected_total * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, HeadOnEncounter) {
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(1, 0), Vector2d(1, 0), 1.3, 1.0);
|
||||
Pedestrian beta = MakePed(Vector2d(3, 0), Vector2d(-1, 0), Vector2d(-1, 0), 1.3, 1.0, Vector2d(-100, 0));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, beta);
|
||||
|
||||
double sqrt3 = std::sqrt(3.0);
|
||||
double Fab_x = -(2.0 * V0_AB / SIGMA / sqrt3) * std::exp(-sqrt3 / SIGMA) * dt;
|
||||
Vector2d expected_total(Fab_x, 0.0);
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
Vector2d expected_w(1.0 + Fab_x, 0.0);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_w);
|
||||
EXPECT_LT(step.w.norm(), alpha.v_max);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_w);
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(0.0, 0.0) + expected_w * dt);
|
||||
}
|
||||
|
||||
TEST(Integration, SpeedCapAppliesWhenPreferredExceedsVMax) {
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 100.0, Vector2d(1000, 0));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha);
|
||||
EXPECT_VEC2_NEAR(total, Vector2d(4.0, 0.0));
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
EXPECT_VEC2_NEAR(step.w, Vector2d(4.0, 0.0));
|
||||
EXPECT_VEC2_NEAR(step.v, Vector2d(alpha.v_max, 0.0));
|
||||
EXPECT_VEC2_NEAR(step.pos, Vector2d(alpha.v_max * dt, 0.0));
|
||||
}
|
||||
|
||||
TEST(Integration, FourPedestriansNoWalls_AsymmetricCrowd) {
|
||||
|
||||
Pedestrian alpha = MakePed(Vector2d(0, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Pedestrian b_front = MakePed(Vector2d(1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Pedestrian b_behind = MakePed(Vector2d(-1, 0), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 0));
|
||||
Pedestrian b_north = MakePed(Vector2d(0, 1), Vector2d(0, 0), Vector2d(1, 0), 1.3, 1.0, Vector2d(100, 1));
|
||||
|
||||
Vector2d total = acceleration_term_dt(alpha) + pedestrian_repulsion_dt(alpha, b_front) + pedestrian_repulsion_dt(alpha, b_behind) + pedestrian_repulsion_dt(alpha, b_north);
|
||||
const double A = (V0_AB / SIGMA) * std::exp(-1.0 / SIGMA) * dt;
|
||||
Vector2d expected_total(dt_times_tau_alpha_inv - A + -c_neg * A, -A);
|
||||
|
||||
EXPECT_VEC2_NEAR(total, expected_total);
|
||||
|
||||
StepResult step = IntegrateOneStep(alpha, total);
|
||||
|
||||
Vector2d expected_w = expected_total;
|
||||
Vector2d expected_v = expected_w;
|
||||
Vector2d expected_pos = alpha.curr_pos + expected_v * dt;
|
||||
|
||||
EXPECT_LT(expected_w.norm(), alpha.v_max);
|
||||
EXPECT_VEC2_NEAR(step.w, expected_w);
|
||||
EXPECT_VEC2_NEAR(step.v, expected_v);
|
||||
EXPECT_VEC2_NEAR(step.pos, expected_pos);
|
||||
}
|
||||
Reference in New Issue
Block a user