diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 4dd9f8c..dad41f7 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -535,6 +535,24 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const EvacuationRoute& route, const Point2D& position, double agentRadius) const { + return verticalPassageReached(layoutCache, route, position, agentRadius, kGeometryEpsilon); + } + + bool verticalPassageNearlyReachedAfterStall( + const ScenarioLayoutCacheResource& layoutCache, + const EvacuationRoute& route, + const Point2D& position, + double agentRadius) const { + const auto tolerance = std::max(kPortalCrossingEpsilon * 0.25, static_cast(agentRadius) * 0.02); + return verticalPassageReached(layoutCache, route, position, agentRadius, tolerance); + } + + bool verticalPassageReached( + const ScenarioLayoutCacheResource& layoutCache, + const EvacuationRoute& route, + const Point2D& position, + double agentRadius, + double planeTolerance) const { if (route.nextWaypointIndex >= route.waypointPassages.size() || route.nextWaypointIndex >= route.waypointFromZoneIds.size() || route.nextWaypointIndex >= route.waypointZoneIds.size()) { @@ -566,7 +584,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return false; } - return dot(position - passageMidpoint, *normal) >= -kGeometryEpsilon; + return dot(position - passageMidpoint, *normal) >= -std::max(0.0, planeTolerance); } std::optional passageNormalTowardCurrentWaypoint( @@ -1114,6 +1132,16 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { break; } } + if (verticalTransition + && route.stalledSeconds >= kWaypointStallSeconds + && verticalPassageNearlyReachedAfterStall(layoutCache, route, position.value, agent.radius)) { + const auto advance = advanceRouteWaypoint(layoutCache, route, agent, position.value); + position.value = advance.position; + if (advance.advanced) { + continue; + } + break; + } route.previousDistanceToWaypoint = std::min(route.previousDistanceToWaypoint, distance); break; diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index 681d00d..57f7857 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -2457,6 +2457,61 @@ SC_TEST(ScenarioSimulationMotionSystem_DoesNotAdvanceVerticalTransitionBeforePor SC_EXPECT_EQ(route.nextWaypointIndex, std::size_t{0}); } +SC_TEST(ScenarioSimulationMotionSystem_AdvancesStalledVerticalTransitionNearPortalPlane) { + std::vector seeds; + seeds.push_back({ + .position = {.value = {.x = 1.0, .y = 1.004}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 1.0, .y = 1.0}, {.x = 1.8, .y = 0.5}}, + .waypointPassages = { + {{.x = 0.6, .y = 1.0}, {.x = 1.4, .y = 1.0}}, + {{.x = 1.8, .y = 0.5}, {.x = 1.8, .y = 0.5}}, + }, + .waypointFromZoneIds = {"upper-stair", ""}, + .waypointZoneIds = {"lower-stair", "missing-exit"}, + .waypointFloorIds = {"L1", "L1"}, + .waypointConnectionIds = {"vertical-stair", ""}, + .waypointVerticalTransitions = {true, false}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 1.0, .y = 1.8}, + .previousDistanceToWaypoint = 0.004, + .stalledSeconds = 1.0, + .destinationZoneId = "missing-exit", + .currentFloorId = "L2", + .displayFloorId = "L2", + }, + .status = {}, + }); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 19, + }); + const auto layout = verticalPortalTransitionLayout(); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem(layout), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.001}); + runtime.stepFrame(0.0); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_EQ(route.currentFloorId, std::string{"L1"}); + SC_EXPECT_EQ(route.nextWaypointIndex, std::size_t{1}); +} + SC_TEST(ScenarioSimulationMotionSystem_KeepsVerticalLandingOnPortalLine) { std::vector seeds; seeds.push_back({