From 81691932050d5cfda0989b96a04418db38ca4c8c Mon Sep 17 00:00:00 2001 From: Silversupplier Date: Wed, 13 May 2026 02:38:19 +0900 Subject: [PATCH] [Domain] Add door closure crowd response --- src/application/SimulationCanvasWidget.cpp | 27 +- src/domain/AgentComponents.h | 2 + src/domain/ScenarioAuthoring.cpp | 23 ++ src/domain/ScenarioAuthoring.h | 2 + src/domain/ScenarioSimulationMotionSystem.cpp | 252 ++++++++++++++---- src/domain/ScenarioSimulationSystems.cpp | 20 +- tests/ScenarioAuthoringTests.cpp | 25 ++ tests/ScenarioSimulationSystemsTests.cpp | 218 +++++++++++++++ 8 files changed, 480 insertions(+), 89 deletions(-) diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index add9679..435eee3 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -51,27 +51,6 @@ bool matchesFloor(const std::string& elementFloorId, const std::string& floorId) return floorId.empty() || elementFloorId.empty() || elementFloorId == floorId; } -bool intervalContains(const safecrowd::domain::ConnectionBlockIntervalDraft& interval, double timeSeconds) { - const auto start = std::max(0.0, interval.startSeconds); - const auto end = std::max(start, interval.endSeconds); - return timeSeconds + 1e-9 >= start && timeSeconds <= end + 1e-9; -} - -bool connectionShouldBeBlocked(const safecrowd::domain::ConnectionBlockDraft& block, double timeSeconds) { - if (block.connectionId.empty()) { - return false; - } - if (block.intervals.empty()) { - return true; - } - for (const auto& interval : block.intervals) { - if (intervalContains(interval, timeSeconds)) { - return true; - } - } - return false; -} - QString hazardKindLabel(safecrowd::domain::EnvironmentHazardKind kind) { switch (kind) { case safecrowd::domain::EnvironmentHazardKind::Smoke: @@ -217,7 +196,7 @@ std::optional hoveredBlockedConnectionIndex( for (std::size_t index = 0; index < blocks.size(); ++index) { const auto& block = blocks[index]; - if (!connectionShouldBeBlocked(block, elapsedSeconds)) { + if (!safecrowd::domain::connectionBlockActiveAt(block, elapsedSeconds)) { continue; } const auto it = std::find_if(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { @@ -363,7 +342,7 @@ std::optional routeGuidanceMarkerCenter( std::vector blockedCenters; blockedCenters.reserve(blocks.size()); for (const auto& block : blocks) { - if (!connectionShouldBeBlocked(block, elapsedSeconds)) { + if (!safecrowd::domain::connectionBlockActiveAt(block, elapsedSeconds)) { continue; } const auto connectionIt = std::find_if(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { @@ -886,7 +865,7 @@ void SimulationCanvasWidget::drawConnectionBlockOverlay(QPainter& painter, const painter.setPen(QPen(QColor("#c0392b"), 2.8, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); for (const auto& block : connectionBlocks_) { - if (!connectionShouldBeBlocked(block, elapsedSeconds)) { + if (!safecrowd::domain::connectionBlockActiveAt(block, elapsedSeconds)) { continue; } diff --git a/src/domain/AgentComponents.h b/src/domain/AgentComponents.h index 27a2acd..00ca4b9 100644 --- a/src/domain/AgentComponents.h +++ b/src/domain/AgentComponents.h @@ -51,6 +51,8 @@ struct EvacuationRoute { double nextSegmentReplanSeconds{0.0}; std::uint64_t observedLayoutRevision{0}; bool noExitAvailable{false}; + bool holdingForClosure{false}; + Point2D closureHoldTarget{}; bool followsGuidance{false}; std::string destinationZoneId{}; std::string originalDestinationZoneId{}; diff --git a/src/domain/ScenarioAuthoring.cpp b/src/domain/ScenarioAuthoring.cpp index 16479c4..7934cfe 100644 --- a/src/domain/ScenarioAuthoring.cpp +++ b/src/domain/ScenarioAuthoring.cpp @@ -379,4 +379,27 @@ std::string environmentHazardFloorId(const FacilityLayout2D& layout, const Envir return it == layout.zones.end() ? std::string{} : it->floorId; } +bool connectionBlockIntervalActiveAt(const ConnectionBlockIntervalDraft& interval, double elapsedSeconds) { + const auto start = std::max(0.0, interval.startSeconds); + if (elapsedSeconds + 1e-9 < start) { + return false; + } + if (interval.endSeconds <= interval.startSeconds) { + return true; + } + return elapsedSeconds <= std::max(start, interval.endSeconds) + 1e-9; +} + +bool connectionBlockActiveAt(const ConnectionBlockDraft& block, double elapsedSeconds) { + if (block.connectionId.empty()) { + return false; + } + if (block.intervals.empty()) { + return true; + } + return std::any_of(block.intervals.begin(), block.intervals.end(), [&](const auto& interval) { + return connectionBlockIntervalActiveAt(interval, elapsedSeconds); + }); +} + } // namespace safecrowd::domain diff --git a/src/domain/ScenarioAuthoring.h b/src/domain/ScenarioAuthoring.h index 3b5300f..e1a3837 100644 --- a/src/domain/ScenarioAuthoring.h +++ b/src/domain/ScenarioAuthoring.h @@ -143,5 +143,7 @@ EnvironmentHazardRuntimeProfile environmentHazardRuntimeProfile(const Environmen bool environmentHazardHasOpenEndedSchedule(const EnvironmentHazardDraft& hazard); bool environmentHazardActiveAt(const EnvironmentHazardDraft& hazard, double elapsedSeconds); std::string environmentHazardFloorId(const FacilityLayout2D& layout, const EnvironmentHazardDraft& hazard); +bool connectionBlockIntervalActiveAt(const ConnectionBlockIntervalDraft& interval, double elapsedSeconds); +bool connectionBlockActiveAt(const ConnectionBlockDraft& block, double elapsedSeconds); } // namespace safecrowd::domain diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index edad42f..2d58b52 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -1,5 +1,6 @@ #include "domain/ScenarioSimulationSystems.h" +#include "domain/GeometryQueries.h" #include "domain/ScenarioRiskMetrics.h" #include "domain/ScenarioSimulationInternal.h" @@ -63,17 +64,18 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto entities = simulationEntities(query); std::vector plans; plans.reserve(entities.size()); - const auto* reactions = resources.contains() - ? &resources.get() - : nullptr; + if (!resources.contains()) { + resources.set(ScenarioEnvironmentReactionResource{}); + } + auto* reactions = &resources.get(); const auto* activeHazards = resources.contains() ? &resources.get() : nullptr; applyRouteGuidance(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed); advanceRoutesForCurrentZones(query, entities, layoutCache); + replanBlockedExitRoutes(query, entities, layoutCache, clock.elapsedSeconds, layoutRevision, reactions); advanceRoutesForWaypointProgress(query, 0.0, entities, layoutCache); - replanBlockedExitRoutes(query, entities, layoutCache, clock.elapsedSeconds, layoutRevision); replanBlockedRouteSegments(query, entities, layoutCache, clock.elapsedSeconds, layoutRevision); replanHazardAwareExitRoutes(query, entities, layoutCache, clock.elapsedSeconds, reactions, activeHazards); updateAgentPhysicsFloorIds(query, layoutCache, entities); @@ -107,7 +109,8 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto& floorLayout = cachedLayoutForFloor(layoutCache, route.currentFloorId); const auto* destinationZone = findZone(floorLayout, route.destinationZoneId); - if (destinationZone != nullptr && pointInRing(destinationZone->area.outline, position.value)) { + if (destinationZone != nullptr + && simulation_internal::pointInRing(destinationZone->area.outline, position.value)) { status.evacuated = true; status.completionTimeSeconds = clock.elapsedSeconds; velocity.value = {}; @@ -222,7 +225,9 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto hazardSpeedFactor = hazardState == nullptr ? 1.0 : std::clamp(hazardState->hazardSpeedFactor, 0.0, 1.0); - const auto adjustedHazardMaxSpeed = adjustedMaxSpeed * hazardSpeedFactor; + const auto* closureState = activeClosureSlowdownState(reactions, entity.index); + const auto closureSpeedFactor = closureState == nullptr ? 1.0 : kClosureApproachSpeedFactor; + const auto adjustedHazardMaxSpeed = adjustedMaxSpeed * hazardSpeedFactor * closureSpeedFactor; const auto desiredVelocity = routeDirection * adjustedHazardMaxSpeed; double speedScale = 1.0; const auto neighborRadius = std::max( @@ -313,6 +318,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { static constexpr double kNoExitReplanCooldownSeconds = 7.0; static constexpr double kSegmentReplanCooldownSeconds = 0.25; static constexpr double kFailedSegmentReplanCooldownSeconds = 1.25; + static constexpr double kClosureApproachSpeedFactor = 0.35; struct RoutePlan { std::vector waypoints{}; @@ -345,6 +351,39 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return &it->second; } + static const ScenarioEnvironmentReactionAgentState* activeClosureSlowdownState( + const ScenarioEnvironmentReactionResource* reactions, + std::uint64_t agentId) { + if (reactions == nullptr) { + return nullptr; + } + const auto it = reactions->agentsById.find(agentId); + if (it == reactions->agentsById.end() + || !it->second.closureDetected + || it->second.closureAware + || it->second.blockedConnectionId.empty()) { + return nullptr; + } + return &it->second; + } + + static void clearClosureReaction( + ScenarioEnvironmentReactionResource* reactions, + std::uint64_t agentId) { + if (reactions == nullptr) { + return; + } + const auto it = reactions->agentsById.find(agentId); + if (it == reactions->agentsById.end()) { + return; + } + it->second.closureDetected = false; + it->second.closureAware = false; + it->second.blockedConnectionId.clear(); + it->second.closureDetectedAtSeconds = 0.0; + it->second.closureReactionReadySeconds = 0.0; + } + static bool sameFloor(const std::string& lhs, const std::string& rhs) { return lhs == rhs || lhs.empty() || rhs.empty(); } @@ -382,10 +421,12 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return it == layoutCache.layout.connections.end() ? nullptr : &(*it); } - bool nextConnectionBlocked(const ScenarioLayoutCacheResource& layoutCache, const EvacuationRoute& route) const { + const Connection2D* nextBlockedConnection( + const ScenarioLayoutCacheResource& layoutCache, + const EvacuationRoute& route) const { if (route.nextWaypointIndex >= route.waypoints.size() || route.nextWaypointIndex >= route.waypointConnectionIds.size()) { - return false; + return nullptr; } for (std::size_t index = route.nextWaypointIndex; index < route.waypointConnectionIds.size(); ++index) { const auto& connectionId = route.waypointConnectionIds[index]; @@ -393,9 +434,16 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { continue; } const auto* connection = findConnectionById(layoutCache, connectionId); - return connection != nullptr && connection->directionality == TravelDirection::Closed; + if (connection != nullptr && connection->directionality == TravelDirection::Closed) { + return connection; + } + return nullptr; } - return false; + return nullptr; + } + + bool nextConnectionBlocked(const ScenarioLayoutCacheResource& layoutCache, const EvacuationRoute& route) const { + return nextBlockedConnection(layoutCache, route) != nullptr; } std::optional verticalTransitionNormal( @@ -555,7 +603,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { if (dot(movement, *normal) < -kGeometryEpsilon) { return false; } - return pointInRing(endpointZone->area.outline, candidate) + return simulation_internal::pointInRing(endpointZone->area.outline, candidate) && !movementCrossesBarrier(layout, from, candidate); }; @@ -680,7 +728,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } auto validLanding = [&](const Point2D& candidate) { - return pointInRing(toZone->area.outline, candidate) + return simulation_internal::pointInRing(toZone->area.outline, candidate) && pointHasBarrierClearance(targetLayout, candidate, clearance); }; @@ -1266,6 +1314,8 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { : distanceToRouteWaypoint(route, start); route.stalledSeconds = 0.0; route.noExitAvailable = false; + route.holdingForClosure = false; + route.closureHoldTarget = {}; route.nextSegmentReplanSeconds = 0.0; } @@ -1631,12 +1681,124 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } } + bool closureReadyForBlockedConnection( + ScenarioEnvironmentReactionResource* reactions, + engine::Entity entity, + const Agent& agent, + const Connection2D& blockedConnection, + double elapsedSeconds) const { + if (reactions == nullptr) { + return true; + } + + auto& state = reactions->agentsById[entity.index]; + if (!state.closureDetected || state.blockedConnectionId != blockedConnection.id) { + state.closureDetected = true; + state.closureAware = false; + state.blockedConnectionId = blockedConnection.id; + state.closureDetectedAtSeconds = elapsedSeconds; + state.closureReactionReadySeconds = elapsedSeconds + std::max(0.0, agent.closurePatienceSeconds); + } + + state.closureAware = elapsedSeconds + 1e-9 >= state.closureReactionReadySeconds; + return state.closureAware; + } + + Point2D closureHoldTarget( + const ScenarioLayoutCacheResource& layoutCache, + const std::string& zoneId, + const Point2D& currentPosition, + engine::Entity entity, + double clearance) const { + const auto* zone = findCachedZone(layoutCache, zoneId); + if (zone == nullptr) { + return currentPosition; + } + + auto center = representativePointInPolygon(zone->area).value_or(polygonCenter(zone->area)); + if (!pointInPolygon(zone->area, center)) { + center = currentPosition; + } + + constexpr double kTwoPi = 6.28318530717958647692; + constexpr double kGoldenAngle = 2.39996322972865332223; + const auto baseAngle = + std::fmod(static_cast((entity.index % 997U) + 1U) * kGoldenAngle, kTwoPi); + const auto boundaryRadius = + std::max(0.0, distanceToPolygonBoundary(zone->area, center) - clearance - 0.05); + const auto radiusScale = 0.35 + (static_cast(entity.index % 5U) * 0.10); + + for (int attempt = 0; attempt < 16; ++attempt) { + const auto angle = baseAngle + (static_cast(attempt) * 0.71); + const auto candidateRadius = + boundaryRadius * std::max(0.15, radiusScale - (static_cast(attempt) * 0.04)); + const Point2D candidate{ + .x = center.x + (std::cos(angle) * candidateRadius), + .y = center.y + (std::sin(angle) * candidateRadius), + }; + if (pointInPolygon(zone->area, candidate) + && distanceToPolygonBoundary(zone->area, candidate) + 1e-9 >= clearance * 0.75) { + return candidate; + } + } + + if (pointInPolygon(zone->area, center) + && distanceToPolygonBoundary(zone->area, center) + 1e-9 >= clearance * 0.5) { + return center; + } + return currentPosition; + } + + void replaceRouteWithClosureHold( + EvacuationRoute& route, + const Point2D& position, + const Point2D& holdTarget) const { + route.destinationZoneId.clear(); + route.waypoints.clear(); + route.waypointPassages.clear(); + route.waypointFromZoneIds.clear(); + route.waypointZoneIds.clear(); + route.waypointFloorIds.clear(); + route.waypointConnectionIds.clear(); + route.waypointVerticalTransitions.clear(); + if (distanceBetween(position, holdTarget) > kArrivalEpsilon) { + route.waypoints.push_back(holdTarget); + route.waypointPassages.push_back(pointPassage(holdTarget)); + route.waypointFromZoneIds.push_back({}); + route.waypointZoneIds.push_back({}); + route.waypointFloorIds.push_back(route.currentFloorId); + route.waypointConnectionIds.push_back({}); + route.waypointVerticalTransitions.push_back(false); + } + route.nextWaypointIndex = 0; + route.currentSegmentStart = position; + route.displayFloorId = route.currentFloorId; + route.previousDistanceToWaypoint = route.waypoints.empty() + ? 0.0 + : distanceToRouteWaypoint(route, position); + route.stalledSeconds = 0.0; + route.noExitAvailable = true; + route.holdingForClosure = true; + route.closureHoldTarget = holdTarget; + route.nextSegmentReplanSeconds = 0.0; + } + + static bool routePlanUsesConnection(const RoutePlan& plan, const std::string& connectionId) { + if (connectionId.empty()) { + return false; + } + return std::any_of(plan.waypointConnectionIds.begin(), plan.waypointConnectionIds.end(), [&](const auto& id) { + return id == connectionId; + }); + } + void replanBlockedExitRoutes( engine::WorldQuery& query, const std::vector& entities, const ScenarioLayoutCacheResource& layoutCache, double elapsedSeconds, - std::uint64_t layoutRevision) const { + std::uint64_t layoutRevision, + ScenarioEnvironmentReactionResource* reactions) const { for (const auto entity : entities) { const auto& status = query.get(entity); if (status.evacuated) { @@ -1644,20 +1806,37 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } auto& route = query.get(entity); + const auto& agent = query.get(entity); if (layoutRevision != route.observedLayoutRevision) { route.observedLayoutRevision = layoutRevision; route.nextExitReplanSeconds = 0.0; route.nextSegmentReplanSeconds = 0.0; } - const bool blockedAhead = nextConnectionBlocked(layoutCache, route); - if (!blockedAhead && !route.noExitAvailable) { + const auto* blockedConnection = nextBlockedConnection(layoutCache, route); + if (blockedConnection == nullptr) { + clearClosureReaction(reactions, entity.index); + } + if (blockedConnection == nullptr && !route.noExitAvailable) { continue; } if (elapsedSeconds + 1e-9 < route.nextExitReplanSeconds) { continue; } + if (blockedConnection != nullptr + && !closureReadyForBlockedConnection(reactions, entity, agent, *blockedConnection, elapsedSeconds)) { + auto retryAt = elapsedSeconds + kExitReplanCooldownSeconds; + if (reactions != nullptr) { + const auto stateIt = reactions->agentsById.find(entity.index); + if (stateIt != reactions->agentsById.end()) { + retryAt = std::min(retryAt, stateIt->second.closureReactionReadySeconds); + } + } + route.nextExitReplanSeconds = retryAt; + continue; + } + const auto& position = query.get(entity); const auto startZoneId = zoneAt(layoutCache, position.value, route.currentFloorId); if (startZoneId.empty()) { @@ -1672,42 +1851,23 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { if (plan.destinationZoneId.empty()) { plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); } + if (blockedConnection != nullptr && routePlanUsesConnection(plan, blockedConnection->id)) { + plan = {}; + } if (plan.destinationZoneId.empty()) { - route.noExitAvailable = true; - route.destinationZoneId.clear(); - route.waypoints.clear(); - route.waypointPassages.clear(); - route.waypointFromZoneIds.clear(); - route.waypointZoneIds.clear(); - route.waypointFloorIds.clear(); - route.waypointConnectionIds.clear(); - route.waypointVerticalTransitions.clear(); - route.nextWaypointIndex = 0; - route.currentSegmentStart = position.value; - route.displayFloorId = route.currentFloorId; - route.previousDistanceToWaypoint = 0.0; - route.stalledSeconds = 0.0; + const auto holdTarget = closureHoldTarget( + layoutCache, + startZoneId, + position.value, + entity, + static_cast(agent.radius) + kPathClearance); + replaceRouteWithClosureHold(route, position.value, holdTarget); route.nextExitReplanSeconds = elapsedSeconds + kNoExitReplanCooldownSeconds; continue; } - route.destinationZoneId = plan.destinationZoneId; - route.waypoints = plan.waypoints; - route.waypointPassages = plan.waypointPassages; - route.waypointFromZoneIds = plan.waypointFromZoneIds; - route.waypointZoneIds = plan.waypointZoneIds; - route.waypointFloorIds = plan.waypointFloorIds; - route.waypointConnectionIds = plan.waypointConnectionIds; - route.waypointVerticalTransitions = plan.waypointVerticalTransitions; - route.nextWaypointIndex = 0; - route.currentSegmentStart = position.value; - route.displayFloorId = route.currentFloorId; - route.previousDistanceToWaypoint = route.waypoints.empty() - ? 0.0 - : distanceToRouteWaypoint(route, position.value); - route.stalledSeconds = 0.0; - route.noExitAvailable = false; - route.nextSegmentReplanSeconds = 0.0; + replaceRouteWithPlan(route, plan, position.value); + clearClosureReaction(reactions, entity.index); route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; } } diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 0a86ef3..ab6812c 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -240,24 +240,6 @@ bool isCriticalPressureEventWorse( return candidate.pressureScore > current.pressureScore; } -bool intervalContains(const ConnectionBlockIntervalDraft& interval, double timeSeconds) { - const auto start = std::max(0.0, interval.startSeconds); - const auto end = std::max(start, interval.endSeconds); - return timeSeconds + 1e-9 >= start && timeSeconds <= end + 1e-9; -} - -bool connectionShouldBeBlocked(const ConnectionBlockDraft& block, double timeSeconds) { - if (block.connectionId.empty()) { - return false; - } - if (block.intervals.empty()) { - return true; - } - return std::any_of(block.intervals.begin(), block.intervals.end(), [&](const auto& interval) { - return intervalContains(interval, timeSeconds); - }); -} - std::string hazardRuntimeKey(const EnvironmentHazardDraft& hazard, std::size_t index) { if (!hazard.id.empty()) { return hazard.id; @@ -310,7 +292,7 @@ std::unordered_set activeBlockedConnectionIds( std::unordered_set ids; ids.reserve(blocks.size()); for (const auto& block : blocks) { - if (!connectionShouldBeBlocked(block, elapsedSeconds)) { + if (!connectionBlockActiveAt(block, elapsedSeconds)) { continue; } if (std::any_of(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { diff --git a/tests/ScenarioAuthoringTests.cpp b/tests/ScenarioAuthoringTests.cpp index eb394aa..0e4abdf 100644 --- a/tests/ScenarioAuthoringTests.cpp +++ b/tests/ScenarioAuthoringTests.cpp @@ -136,6 +136,31 @@ SC_TEST(environmentHazardFloorId_FallsBackToAffectedZoneFloor) { SC_EXPECT_EQ(environmentHazardFloorId(layout, hazard), std::string{"Manual"}); } +SC_TEST(connectionBlockActiveAt_UsesCentralScheduleSemantics) { + ConnectionBlockDraft emptyConnection; + SC_EXPECT_TRUE(!connectionBlockActiveAt(emptyConnection, 5.0)); + + ConnectionBlockDraft alwaysClosed; + alwaysClosed.connectionId = "door-a"; + SC_EXPECT_TRUE(connectionBlockActiveAt(alwaysClosed, 0.0)); + SC_EXPECT_TRUE(connectionBlockActiveAt(alwaysClosed, 120.0)); + + ConnectionBlockDraft finite; + finite.connectionId = "door-b"; + finite.intervals.push_back({.startSeconds = 5.0, .endSeconds = 10.0}); + SC_EXPECT_TRUE(!connectionBlockActiveAt(finite, 4.9)); + SC_EXPECT_TRUE(connectionBlockActiveAt(finite, 5.0)); + SC_EXPECT_TRUE(connectionBlockActiveAt(finite, 10.0)); + SC_EXPECT_TRUE(!connectionBlockActiveAt(finite, 10.1)); + + ConnectionBlockDraft openEnded; + openEnded.connectionId = "door-c"; + openEnded.intervals.push_back({.startSeconds = 15.0, .endSeconds = 15.0}); + SC_EXPECT_TRUE(!connectionBlockActiveAt(openEnded, 14.9)); + SC_EXPECT_TRUE(connectionBlockActiveAt(openEnded, 15.0)); + SC_EXPECT_TRUE(connectionBlockActiveAt(openEnded, 90.0)); +} + SC_TEST(computeScenarioDiffKeys_returnsEmptyForFreshDuplicate) { const auto baseline = makeBaselineDraft(); const auto variant = duplicateScenarioDraft(baseline, "scenario-2", "Variant"); diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index fcd4e2b..1f5bf29 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -423,6 +423,42 @@ safecrowd::domain::ScenarioAgentSeed straightRouteSeed( }; } +safecrowd::domain::ScenarioAgentSeed doorRouteSeed( + safecrowd::domain::Point2D start, + std::string destinationZoneId, + std::string connectionId, + safecrowd::domain::LineSegment2D passage, + double maxSpeed = 1.0, + double closurePatienceSeconds = 0.0) { + const auto target = safecrowd::domain::simulation_internal::closestPointOnSegment( + start, + passage.start, + passage.end); + return { + .position = {.value = start}, + .agent = { + .radius = 0.25f, + .maxSpeed = static_cast(maxSpeed), + .closurePatienceSeconds = closurePatienceSeconds, + }, + .velocity = {.value = {}}, + .route = { + .waypoints = {target}, + .waypointPassages = {passage}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {destinationZoneId}, + .waypointFloorIds = {""}, + .waypointConnectionIds = {connectionId}, + .waypointVerticalTransitions = {false}, + .nextWaypointIndex = 0, + .currentSegmentStart = start, + .previousDistanceToWaypoint = 1.0, + .destinationZoneId = destinationZoneId, + }, + .status = {}, + }; +} + safecrowd::domain::EnvironmentHazardDraft hazardDraft( std::string id, safecrowd::domain::EnvironmentHazardKind kind, @@ -462,6 +498,29 @@ void addHazardMotionSystems( .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); } +void addClosureMotionSystems( + safecrowd::engine::EngineRuntime& runtime, + const safecrowd::domain::FacilityLayout2D& layout, + std::vector blocks) { + runtime.addSystem( + safecrowd::domain::makeScenarioControlSystem(layout, std::move(blocks)), + {.phase = safecrowd::engine::UpdatePhase::PreSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem(layout), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + runtime.addSystem( + std::make_unique(), + {.phase = safecrowd::engine::UpdatePhase::RenderSync, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); +} + +void stepScenarioRuntime(safecrowd::engine::EngineRuntime& runtime, double deltaSeconds) { + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = deltaSeconds}); + runtime.stepFrame(0.0); +} + safecrowd::domain::FacilityLayout2D overlappingFloorBottleneckLayout() { safecrowd::domain::FacilityLayout2D layout; layout.floors.push_back({.id = "L1", .label = "Floor 1"}); @@ -2131,6 +2190,165 @@ SC_TEST(ScenarioControlSystem_BlocksConnectionsUsingScenarioClock) { } } +SC_TEST(ScenarioSimulationMotionSystem_SlowsBeforeDoorClosureReroute) { + auto layout = twoExitGuidanceDetourLayout(); + safecrowd::domain::ConnectionBlockDraft block; + block.id = "block-near"; + block.connectionId = "room-near-exit"; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 91, + }); + runtime.addSystem(std::make_unique( + std::vector{ + doorRouteSeed( + {.x = 1.2, .y = 0.5}, + "near-exit", + "room-near-exit", + {{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}, + 1.0, + 0.5), + }, + 5.0)); + addClosureMotionSystems(runtime, layout, {block}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + auto& query = runtime.world().query(); + const auto entities = query.view< + safecrowd::domain::Position, + safecrowd::domain::Velocity, + safecrowd::domain::EvacuationRoute>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto entity = entities.front(); + const auto& velocityBeforeReady = query.get(entity).value; + const auto& routeBeforeReady = query.get(entity); + SC_EXPECT_EQ(routeBeforeReady.destinationZoneId, std::string{"near-exit"}); + SC_EXPECT_TRUE(safecrowd::domain::simulation_internal::lengthOf(velocityBeforeReady) > 0.0); + SC_EXPECT_TRUE(safecrowd::domain::simulation_internal::lengthOf(velocityBeforeReady) < 0.5); + + const auto& reactions = + runtime.world().resources().get(); + const auto stateIt = reactions.agentsById.find(entity.index); + SC_EXPECT_TRUE(stateIt != reactions.agentsById.end()); + SC_EXPECT_TRUE(stateIt->second.closureDetected); + SC_EXPECT_TRUE(!stateIt->second.closureAware); + SC_EXPECT_EQ(stateIt->second.blockedConnectionId, std::string{"room-near-exit"}); + + for (int i = 0; i < 6; ++i) { + stepScenarioRuntime(runtime, 0.1); + } + + const auto& routeAfterReady = query.get(entity); + SC_EXPECT_EQ(routeAfterReady.destinationZoneId, std::string{"far-exit"}); + SC_EXPECT_TRUE(!routeAfterReady.noExitAvailable); +} + +SC_TEST(ScenarioSimulationMotionSystem_HoldsInsideZoneWhenDoorClosureLeavesNoExit) { + auto layout = straightExitLayout(); + safecrowd::domain::ConnectionBlockDraft block; + block.id = "block-exit"; + block.connectionId = "room-exit"; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 92, + }); + runtime.addSystem(std::make_unique( + std::vector{ + doorRouteSeed( + {.x = 0.8, .y = 0.0}, + "exit", + "room-exit", + {{.x = 1.0, .y = -0.4}, {.x = 1.0, .y = 0.4}}), + }, + 5.0)); + addClosureMotionSystems(runtime, layout, {block}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + auto& query = runtime.world().query(); + const auto entities = query.view< + safecrowd::domain::Position, + safecrowd::domain::Velocity, + safecrowd::domain::EvacuationRoute>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto entity = entities.front(); + const auto& route = query.get(entity); + const auto& position = query.get(entity).value; + const auto& layoutCache = + runtime.world().resources().get(); + + SC_EXPECT_EQ( + layoutCache.layout.connections.front().directionality, + safecrowd::domain::TravelDirection::Closed); + SC_EXPECT_EQ(route.destinationZoneId, std::string{}); + SC_EXPECT_TRUE(route.noExitAvailable); + SC_EXPECT_TRUE(route.holdingForClosure); + SC_EXPECT_TRUE(route.destinationZoneId.empty()); + SC_EXPECT_TRUE(!route.waypoints.empty()); + SC_EXPECT_TRUE(position.x < 0.8); +} + +SC_TEST(ScenarioSimulationMotionSystem_RetriesNoExitAfterFiniteDoorClosureReopens) { + auto layout = straightExitLayout(); + safecrowd::domain::ConnectionBlockDraft block; + block.id = "block-exit"; + block.connectionId = "room-exit"; + block.intervals.push_back({.startSeconds = 0.0, .endSeconds = 0.3}); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 93, + }); + runtime.addSystem(std::make_unique( + std::vector{ + doorRouteSeed( + {.x = 0.8, .y = 0.0}, + "exit", + "room-exit", + {{.x = 1.0, .y = -0.4}, {.x = 1.0, .y = 0.4}}), + }, + 5.0)); + addClosureMotionSystems(runtime, layout, {block}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + auto& query = runtime.world().query(); + const auto entities = query.view< + safecrowd::domain::Position, + safecrowd::domain::Velocity, + safecrowd::domain::EvacuationRoute>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto entity = entities.front(); + const auto& layoutCache = + runtime.world().resources().get(); + SC_EXPECT_EQ( + layoutCache.layout.connections.front().directionality, + safecrowd::domain::TravelDirection::Closed); + SC_EXPECT_EQ(query.get(entity).destinationZoneId, std::string{}); + SC_EXPECT_TRUE(query.get(entity).noExitAvailable); + + bool routeRecovered = false; + for (int i = 0; i < 12; ++i) { + stepScenarioRuntime(runtime, 0.1); + const auto& route = query.get(entity); + if (!route.noExitAvailable && route.destinationZoneId == "exit") { + routeRecovered = true; + break; + } + } + + SC_EXPECT_TRUE(routeRecovered); +} + SC_TEST(ScenarioRiskMetricsSystem_PublishesStalledHotspotAndBottleneckMetrics) { std::vector seeds; for (int index = 0; index < 5; ++index) {