[Domain] Propagate stalled state through queues

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 <algorithm>
@@ -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<engine::Entity> simulationEntities(engine::WorldQuery& query) {
     return query.view<Position, Agent, Velocity, AvoidanceState, EvacuationRoute, EvacuationStatus>();
 }
 
+void propagateStalledStateThroughQueues(SimulationFrame& frame) {
+    std::vector<std::size_t> 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<std::size_t> 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<engine::Entity>& entities,

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<engine::Entity> simulationEntities(engine::WorldQuery& query);
+void propagateStalledStateThroughQueues(SimulationFrame& frame);
 AgentSpatialIndex buildAgentSpatialIndex(engine::WorldQuery& query, const std::vector<engine::Entity>& entities, double cellSize);
 std::vector<engine::Entity> nearbyAgents(engine::WorldQuery& query, const AgentSpatialIndex& index, const Point2D& point, double radius);
 std::vector<engine::Entity> nearbyAgents(

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;

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<ScenarioResultArtifactsResource>()) {
         auto& result = resources.get<ScenarioResultArtifactsResource>();

tests/ScenarioSimulationSystemsTests.cpp

@@ -1281,6 +1281,116 @@ SC_TEST(ScenarioAgentSpawnSystem_ConfiguresClockAndSpawnsAgentSeeds) {
     SC_EXPECT_TRUE(frame.agents.front().stalled);
 }
 
+SC_TEST(ScenarioFrameSyncSystem_PropagatesStalledStateToAgentBetweenStalledNeighbors) {
+    std::vector<safecrowd::domain::ScenarioAgentSeed> 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<safecrowd::domain::ScenarioAgentSpawnSystem>(std::move(seeds), 15.0));
+    runtime.addSystem(
+        std::make_unique<safecrowd::domain::ScenarioFrameSyncSystem>(),
+        {.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<safecrowd::domain::ScenarioSimulationFrameResource>().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<safecrowd::domain::ScenarioAgentSeed> 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<safecrowd::domain::ScenarioAgentSpawnSystem>(std::move(seeds), 15.0));
+    runtime.addSystem(
+        std::make_unique<safecrowd::domain::ScenarioFrameSyncSystem>(),
+        {.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<safecrowd::domain::ScenarioSimulationFrameResource>().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<safecrowd::domain::ScenarioAgentSeed> seeds;
     seeds.push_back(localWayfindingSeed({.x = 1.0, .y = 3.0}));