From 75d3e71ad0a6b67fa4c23e9a56afc493d24b6484 Mon Sep 17 00:00:00 2001 From: muzygosu Date: Fri, 5 Jun 2026 00:15:53 +0900 Subject: [PATCH] Propagate stalled state through queues --- src/domain/ScenarioSimulationInternal.cpp | 75 ++++++++++++ src/domain/ScenarioSimulationInternal.h | 5 + src/domain/ScenarioSimulationMotionSystem.cpp | 1 + src/domain/ScenarioSimulationSystems.cpp | 1 + tests/ScenarioSimulationSystemsTests.cpp | 110 ++++++++++++++++++ 5 files changed, 192 insertions(+) diff --git a/src/domain/ScenarioSimulationInternal.cpp b/src/domain/ScenarioSimulationInternal.cpp index c3822fa..4872680 100644 --- a/src/domain/ScenarioSimulationInternal.cpp +++ b/src/domain/ScenarioSimulationInternal.cpp @@ -1,5 +1,7 @@ #include "domain/ScenarioSimulationInternal.h" +#include "domain/ScenarioRiskMetrics.h" +#include "domain/ScenarioSimulationFrame.h" #include "domain/ScenarioSimulationSystems.h" #include @@ -66,6 +68,10 @@ double dot(const Point2D& lhs, const Point2D& rhs) { return (lhs.x * rhs.x) + (lhs.y * rhs.y); } +double crossMagnitude(const Point2D& lhs, const Point2D& rhs) { + return std::fabs((lhs.x * rhs.y) - (lhs.y * rhs.x)); +} + Point2D perpendicularLeft(const Point2D& point) { return {.x = -point.y, .y = point.x}; } @@ -1153,6 +1159,75 @@ std::vector simulationEntities(engine::WorldQuery& query) { return query.view(); } +void propagateStalledStateThroughQueues(SimulationFrame& frame) { + std::vector stalledIndexes; + stalledIndexes.reserve(frame.agents.size()); + for (std::size_t index = 0; index < frame.agents.size(); ++index) { + if (frame.agents[index].stalled) { + stalledIndexes.push_back(index); + } + } + if (stalledIndexes.size() < 2) { + return; + } + + std::vector propagatedIndexes; + for (std::size_t index = 0; index < frame.agents.size(); ++index) { + auto& candidate = frame.agents[index]; + if (candidate.stalled) { + continue; + } + + const auto speed = lengthOf(candidate.velocity); + if (speed <= kScenarioStalledSpeedThreshold) { + continue; + } + + const auto forward = candidate.velocity * (1.0 / speed); + bool hasStalledAhead = false; + bool hasStalledBehind = false; + for (const auto stalledIndex : stalledIndexes) { + const auto& stalled = frame.agents[stalledIndex]; + if (stalled.floorId != candidate.floorId) { + continue; + } + + const auto offset = stalled.position - candidate.position; + const auto distance = lengthOf(offset); + const auto reach = std::max(0.0, candidate.radius) + + std::max(0.0, stalled.radius) + + kStalledQueuePropagationExtraReach; + if (distance > reach) { + continue; + } + + const auto lateralDistance = crossMagnitude(forward, offset); + const auto lateralTolerance = std::max(0.0, candidate.radius) + + std::max(0.0, stalled.radius) + + kStalledQueuePropagationLateralBuffer; + if (lateralDistance > lateralTolerance) { + continue; + } + + const auto longitudinalDistance = dot(offset, forward); + if (longitudinalDistance >= kStalledQueuePropagationMinimumLongitudinal) { + hasStalledAhead = true; + } else if (longitudinalDistance <= -kStalledQueuePropagationMinimumLongitudinal) { + hasStalledBehind = true; + } + + if (hasStalledAhead && hasStalledBehind) { + propagatedIndexes.push_back(index); + break; + } + } + } + + for (const auto index : propagatedIndexes) { + frame.agents[index].stalled = true; + } +} + AgentSpatialIndex buildAgentSpatialIndex( engine::WorldQuery& query, const std::vector& entities, diff --git a/src/domain/ScenarioSimulationInternal.h b/src/domain/ScenarioSimulationInternal.h index f981310..6c6408e 100644 --- a/src/domain/ScenarioSimulationInternal.h +++ b/src/domain/ScenarioSimulationInternal.h @@ -16,6 +16,7 @@ namespace safecrowd::domain { struct ScenarioConnectionTraversal; struct ScenarioLayoutCacheResource; +struct SimulationFrame; } namespace safecrowd::domain::simulation_internal { @@ -42,6 +43,9 @@ inline constexpr double kWaypointProgressEpsilon = 0.02; inline constexpr double kWaypointBypassLongitudinalTolerance = 0.5; inline constexpr double kWaypointBypassLateralTolerance = 0.65; inline constexpr double kWaypointStallSeconds = 0.75; +inline constexpr double kStalledQueuePropagationExtraReach = 0.35; +inline constexpr double kStalledQueuePropagationLateralBuffer = 0.15; +inline constexpr double kStalledQueuePropagationMinimumLongitudinal = 0.05; inline constexpr double kPortalCrossingEpsilon = 0.02; inline constexpr double kRouteReplanCooldownSeconds = 0.35; @@ -161,6 +165,7 @@ bool routePassageCrossed(const FacilityLayout2D& layout, const EvacuationRoute& double speedOf(const Point2D& velocity); bool pointHasBarrierClearance(const FacilityLayout2D& layout, const Point2D& point, double clearance); std::vector simulationEntities(engine::WorldQuery& query); +void propagateStalledStateThroughQueues(SimulationFrame& frame); AgentSpatialIndex buildAgentSpatialIndex(engine::WorldQuery& query, const std::vector& entities, double cellSize); std::vector nearbyAgents(engine::WorldQuery& query, const AgentSpatialIndex& index, const Point2D& point, double radius); std::vector nearbyAgents( diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 727a8b1..82d21dd 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -203,6 +203,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { && scenarioAgentStalled(simulation_internal::lengthOf(velocity.value), route->stalledSeconds), }); } + simulation_internal::propagateStalledStateThroughQueues(keyframe); if (shouldCaptureT90) { timingKeyframes.t90Frame = keyframe; diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 84ec7f6..03585ac 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -1286,6 +1286,7 @@ void ScenarioFrameSyncSystem::update(engine::EngineWorld& world, const engine::E && scenarioAgentStalled(simulation_internal::lengthOf(velocity.value), route->stalledSeconds), }); } + simulation_internal::propagateStalledStateThroughQueues(frame); if (resources.contains()) { auto& result = resources.get(); diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index 038855a..1b407f5 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -1281,6 +1281,116 @@ SC_TEST(ScenarioAgentSpawnSystem_ConfiguresClockAndSpawnsAgentSeeds) { SC_EXPECT_TRUE(frame.agents.front().stalled); } +SC_TEST(ScenarioFrameSyncSystem_PropagatesStalledStateToAgentBetweenStalledNeighbors) { + std::vector seeds; + seeds.push_back({ + .position = {.value = {.x = 0.0, .y = 0.0}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {.x = 0.0, .y = 0.5}}, + .route = {.stalledSeconds = 0.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + seeds.push_back({ + .position = {.value = {.x = 0.0, .y = 0.55}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {}}, + .route = {.stalledSeconds = 1.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + seeds.push_back({ + .position = {.value = {.x = 0.0, .y = -0.55}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {}}, + .route = {.stalledSeconds = 1.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 2, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 15.0)); + runtime.addSystem( + std::make_unique(), + {.phase = safecrowd::engine::UpdatePhase::RenderSync, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.stepFrame(1.0 / 30.0); + + const auto& frame = runtime.world().resources().get().frame; + std::size_t stalledCount = 0; + bool middleStalled = false; + for (const auto& agent : frame.agents) { + if (agent.stalled) { + ++stalledCount; + } + if (std::fabs(agent.position.y) <= 1e-9) { + middleStalled = agent.stalled; + } + } + + SC_EXPECT_EQ(frame.agents.size(), std::size_t{3}); + SC_EXPECT_EQ(stalledCount, std::size_t{3}); + SC_EXPECT_TRUE(middleStalled); +} + +SC_TEST(ScenarioFrameSyncSystem_DoesNotPropagateStalledStateFromSideLane) { + std::vector seeds; + seeds.push_back({ + .position = {.value = {.x = 0.0, .y = 0.0}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {.x = 0.0, .y = 0.5}}, + .route = {.stalledSeconds = 0.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + seeds.push_back({ + .position = {.value = {.x = 0.0, .y = 0.55}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {}}, + .route = {.stalledSeconds = 1.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + seeds.push_back({ + .position = {.value = {.x = 1.0, .y = -0.55}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.2f}, + .velocity = {.value = {}}, + .route = {.stalledSeconds = 1.0, .currentFloorId = "L1", .displayFloorId = "L1"}, + .status = {}, + }); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 2, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 15.0)); + runtime.addSystem( + std::make_unique(), + {.phase = safecrowd::engine::UpdatePhase::RenderSync, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.stepFrame(1.0 / 30.0); + + const auto& frame = runtime.world().resources().get().frame; + std::size_t stalledCount = 0; + bool middleStalled = true; + for (const auto& agent : frame.agents) { + if (agent.stalled) { + ++stalledCount; + } + if (std::fabs(agent.position.y) <= 1e-9) { + middleStalled = agent.stalled; + } + } + + SC_EXPECT_EQ(frame.agents.size(), std::size_t{3}); + SC_EXPECT_EQ(stalledCount, std::size_t{2}); + SC_EXPECT_TRUE(!middleStalled); +} + SC_TEST(ScenarioWayfindingSystem_DirectionArrowSelectsAlignedConnection) { std::vector seeds; seeds.push_back(localWayfindingSeed({.x = 1.0, .y = 3.0}));