diff --git a/src/application/ScenarioCanvasWidget.cpp b/src/application/ScenarioCanvasWidget.cpp index d37d291..a35d393 100644 --- a/src/application/ScenarioCanvasWidget.cpp +++ b/src/application/ScenarioCanvasWidget.cpp @@ -251,6 +251,9 @@ QString formatEnvironmentHazardTooltip(const safecrowd::domain::EnvironmentHazar .arg(std::max(start, hazard.endSeconds), 0, 'f', 1)); } text.append(QString("\nSeverity: %1").arg(severityLabel(hazard.severity))); + text.append(QString("\nInfluence radius: %1m") + .arg(safecrowd::domain::environmentHazardRadiusMeters(hazard.severity), 0, 'f', 1)); + text.append(QStringLiteral("\nDetection varies by agent sensitivity")); return text; } diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index f680d0b..3489dad 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -87,6 +87,9 @@ QString formatEnvironmentHazardTooltip(const safecrowd::domain::EnvironmentHazar .arg(std::max(start, hazard.endSeconds), 0, 'f', 1)); } text.append(QString("\nSeverity: %1").arg(severityLabel(hazard.severity))); + text.append(QString("\nInfluence radius: %1m") + .arg(safecrowd::domain::environmentHazardRadiusMeters(hazard.severity), 0, 'f', 1)); + text.append(QStringLiteral("\nDetection varies by agent sensitivity")); return text; } @@ -1132,8 +1135,16 @@ void SimulationCanvasWidget::drawEnvironmentHazardOverlay(QPainter& painter, con markerFill = QColor(100, 116, 139, 115); } QRadialGradient gradient(center, radius); + auto falloffColor = [&](double ratio, QColor color) { + const auto influence = safecrowd::domain::environmentHazardInfluenceAt( + hazard, + radiusMeters * std::clamp(ratio, 0.0, 1.0)); + color.setAlpha(std::clamp(static_cast(std::lround(static_cast(color.alpha()) * influence)), 0, 255)); + return color; + }; gradient.setColorAt(0.0, core); - gradient.setColorAt(0.48, mid); + gradient.setColorAt(0.35, falloffColor(0.35, core)); + gradient.setColorAt(0.65, falloffColor(0.65, mid)); gradient.setColorAt(1.0, edge); painter.setPen(Qt::NoPen); painter.setBrush(gradient); diff --git a/src/domain/ScenarioAuthoring.cpp b/src/domain/ScenarioAuthoring.cpp index 66c898e..acf3f8a 100644 --- a/src/domain/ScenarioAuthoring.cpp +++ b/src/domain/ScenarioAuthoring.cpp @@ -256,6 +256,17 @@ double environmentHazardRadiusMeters(ScenarioElementSeverity severity) { } } +double environmentHazardInfluenceAt(const EnvironmentHazardDraft& hazard, double distanceMeters) { + const auto radius = environmentHazardRadiusMeters(hazard.severity); + if (radius <= 1e-9) { + return 0.0; + } + + const auto t = std::clamp(std::max(0.0, distanceMeters) / radius, 0.0, 1.0); + const auto smooth = t * t * (3.0 - (2.0 * t)); + return 1.0 - smooth; +} + double environmentHazardRoutePenaltyMeters(ScenarioElementSeverity severity) { switch (severity) { case ScenarioElementSeverity::Low: @@ -323,13 +334,12 @@ double environmentHazardSmokeVisibilityMetersAt(const EnvironmentHazardDraft& ha break; } - const auto distance = std::max(0.0, distanceMeters); - if (distance >= radius) { + const auto influence = environmentHazardInfluenceAt(hazard, distanceMeters); + if (influence <= 1e-9) { return 3.0; } - const auto t = std::clamp(distance / radius, 0.0, 1.0); - return sourceVisibility + ((3.0 - sourceVisibility) * t); + return 3.0 - ((3.0 - sourceVisibility) * influence); } double environmentHazardSmokeSpeedMetersPerSecond(double smokeFreeSpeedMetersPerSecond, double visibilityMeters) { @@ -367,8 +377,8 @@ double environmentHazardSpeedFactorAt( } const auto centerFactor = environmentHazardSpeedFactor(hazard.kind, hazard.severity); - const auto proximity = 1.0 - std::clamp(std::max(0.0, distanceMeters) / radius, 0.0, 1.0); - return 1.0 - ((1.0 - centerFactor) * proximity); + const auto influence = environmentHazardInfluenceAt(hazard, distanceMeters); + return 1.0 - ((1.0 - centerFactor) * influence); } EnvironmentHazardRuntimeProfile environmentHazardRuntimeProfile(const EnvironmentHazardDraft& hazard) { diff --git a/src/domain/ScenarioAuthoring.h b/src/domain/ScenarioAuthoring.h index 65c8eb7..7744028 100644 --- a/src/domain/ScenarioAuthoring.h +++ b/src/domain/ScenarioAuthoring.h @@ -133,6 +133,7 @@ std::vector computeScenarioDiffKeys(const ScenarioDraft& baseline, const ScenarioDraft& variant); double environmentHazardRadiusMeters(ScenarioElementSeverity severity); +double environmentHazardInfluenceAt(const EnvironmentHazardDraft& hazard, double distanceMeters); double environmentHazardRoutePenaltyMeters(ScenarioElementSeverity severity); double environmentHazardSeverityWeight(ScenarioElementSeverity severity); double environmentHazardSpeedFactor(EnvironmentHazardKind kind, ScenarioElementSeverity severity); diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 4dd9f8c..e756dfe 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -78,7 +78,15 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { ? &resources.get() : nullptr; - routeGuidance_.apply(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed, sharedSpatialIndex); + routeGuidance_.apply( + query, + entities, + layoutCache, + clock.elapsedSeconds, + step.derivedSeed, + reactions, + activeHazards, + sharedSpatialIndex); advanceRoutesForCurrentZones(query, activeEntities_, layoutCache); replanBlockedExitRoutes(query, activeEntities_, layoutCache, clock.elapsedSeconds, layoutRevision, reactions); advanceRoutesForWaypointProgress(query, 0.0, activeEntities_, layoutCache); @@ -1208,7 +1216,19 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return; } - for (const auto entity : entities) { + if (entities.empty()) { + hazardReplanCursor_ = 0; + return; + } + + constexpr std::size_t kHazardReplanEntityBudgetPerFrame = 50; + if (hazardReplanCursor_ >= entities.size()) { + hazardReplanCursor_ = 0; + } + const auto startCursor = hazardReplanCursor_; + const auto visitCount = std::min(entities.size(), kHazardReplanEntityBudgetPerFrame); + for (std::size_t offset = 0; offset < visitCount; ++offset) { + const auto entity = entities[(startCursor + offset) % entities.size()]; const auto* hazardState = activeHazardState(reactions, entity.index); if (hazardState == nullptr) { continue; @@ -1246,6 +1266,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { routeGuidance_.replaceRouteWithPlan(route, plan, position.value); route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; } + hazardReplanCursor_ = (startCursor + visitCount) % entities.size(); } bool closureReadyForBlockedConnection( @@ -1708,6 +1729,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { std::optional layoutCache_{}; ScenarioRouteGuidanceController routeGuidance_{}; std::vector activeEntities_{}; + mutable std::size_t hazardReplanCursor_{0}; }; diff --git a/src/domain/ScenarioSimulationRouteGuidance.cpp b/src/domain/ScenarioSimulationRouteGuidance.cpp index b83460e..5a8ef3b 100644 --- a/src/domain/ScenarioSimulationRouteGuidance.cpp +++ b/src/domain/ScenarioSimulationRouteGuidance.cpp @@ -64,6 +64,8 @@ class ScenarioRouteGuidanceController::Impl final { nearestExitRouteCache_.clear(); exitRouteCache_.clear(); pathCache_.clear(); + hazardAwareExitPlanCache_.clear(); + routeHazardPenaltyCache_.clear(); } std::optional cachedZoneRouteToNearestExit( @@ -420,6 +422,210 @@ class ScenarioRouteGuidanceController::Impl final { return signature; } + static std::string activeHazardSignature(const ScenarioActiveEnvironmentHazardsResource* activeHazards) { + if (activeHazards == nullptr || activeHazards->hazards.empty()) { + return {}; + } + if (!activeHazards->signature.empty()) { + return activeHazards->signature; + } + + std::string signature; + for (const auto& hazard : activeHazards->hazards) { + if (!signature.empty()) { + signature.push_back('\x1f'); + } + signature.append(hazard.key); + signature.push_back('\x1e'); + signature.append(hazard.floorId); + signature.push_back('\x1e'); + signature.append(pointCachePart(hazard.draft.position)); + signature.push_back('\x1e'); + signature.append(std::to_string(quantizedPlanningCoordinate(hazard.radiusMeters))); + signature.push_back('\x1e'); + signature.append(std::to_string(quantizedPlanningCoordinate(hazard.routePenaltyMeters))); + } + return signature; + } + + static constexpr double kHazardSafeRoutePenaltyMeters = 1.0; + + void refreshRouteSafetyCache(const std::string& activeGuidanceSignature, + const std::string& activeHazardSignature) const { + if (routeSafetyGuidanceSignature_ == activeGuidanceSignature + && routeSafetyHazardSignature_ == activeHazardSignature) { + return; + } + routeSafetyGuidanceSignature_ = activeGuidanceSignature; + routeSafetyHazardSignature_ = activeHazardSignature; + hazardAwareExitPlanCache_.clear(); + routeHazardPenaltyCache_.clear(); + } + + static bool hasActiveHazards(const ScenarioActiveEnvironmentHazardsResource* activeHazards) { + return activeHazards != nullptr && !activeHazards->hazards.empty(); + } + + static bool agentHazardAware( + const ScenarioEnvironmentReactionResource* reactions, + engine::Entity entity) { + if (reactions == nullptr) { + return false; + } + const auto it = reactions->agentsById.find(entity.index); + return it != reactions->agentsById.end() && it->second.hazardAware; + } + + static ScenarioRoutePlan remainingRoutePlan(const EvacuationRoute& route) { + ScenarioRoutePlan plan; + plan.destinationZoneId = route.destinationZoneId; + if (route.nextWaypointIndex >= route.waypoints.size()) { + return plan; + } + + for (std::size_t index = route.nextWaypointIndex; index < route.waypoints.size(); ++index) { + plan.waypoints.push_back(route.waypoints[index]); + plan.waypointPassages.push_back(index < route.waypointPassages.size() + ? route.waypointPassages[index] + : pointPassage(route.waypoints[index])); + plan.waypointFromZoneIds.push_back(index < route.waypointFromZoneIds.size() + ? route.waypointFromZoneIds[index] + : std::string{}); + plan.waypointZoneIds.push_back(index < route.waypointZoneIds.size() + ? route.waypointZoneIds[index] + : std::string{}); + plan.waypointFloorIds.push_back(index < route.waypointFloorIds.size() + ? route.waypointFloorIds[index] + : std::string{}); + plan.waypointConnectionIds.push_back(index < route.waypointConnectionIds.size() + ? route.waypointConnectionIds[index] + : std::string{}); + plan.waypointVerticalTransitions.push_back(index < route.waypointVerticalTransitions.size() + ? route.waypointVerticalTransitions[index] + : false); + } + return plan; + } + + static std::string routePlanSafetyKey( + const std::string& activeHazardSignature, + const Point2D& start, + const std::string& startFloorId, + const ScenarioRoutePlan& plan) { + std::string key = activeHazardSignature; + key.push_back('\x1d'); + key.append(startFloorId); + key.push_back('\x1d'); + key.append(pointCachePart(start)); + key.push_back('\x1d'); + key.append(plan.destinationZoneId); + for (std::size_t index = 0; index < plan.waypoints.size(); ++index) { + key.push_back('\x1d'); + key.append(pointCachePart(plan.waypoints[index])); + if (index < plan.waypointFloorIds.size()) { + key.push_back('@'); + key.append(plan.waypointFloorIds[index]); + } + if (index < plan.waypointConnectionIds.size()) { + key.push_back('#'); + key.append(plan.waypointConnectionIds[index]); + } + } + return key; + } + + std::string hazardAwareExitPlanCacheKey( + const Point2D& start, + const std::string& startZoneId, + const std::string& agentFloorId, + const std::string& activeHazardSignature) const { + return std::to_string(planningCacheRevision_) + + '\x1f' + + activeHazardSignature + + '\x1f' + + startZoneId + + '\x1f' + + agentFloorId + + '\x1f' + + pointCachePart(start); + } + + double cachedHazardRoutePenalty( + const ScenarioLayoutCacheResource& layoutCache, + const ScenarioRoutePlan& plan, + const Point2D& start, + const std::string& startFloorId, + const ScenarioActiveEnvironmentHazardsResource& activeHazards, + const std::string& activeHazardSignature) const { + if (activeHazardSignature.empty() || plan.destinationZoneId.empty()) { + return 0.0; + } + + const auto key = routePlanSafetyKey(activeHazardSignature, start, startFloorId, plan); + const auto it = routeHazardPenaltyCache_.find(key); + if (it != routeHazardPenaltyCache_.end()) { + return it->second; + } + + const auto penalty = hazardRoutePenalty(layoutCache, plan, start, startFloorId, activeHazards); + routeHazardPenaltyCache_.emplace(key, penalty); + return penalty; + } + + bool remainingRouteHazardSafe( + const ScenarioLayoutCacheResource& layoutCache, + const EvacuationRoute& route, + const Point2D& position, + const std::string& startFloorId, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, + const std::string& activeHazardSignature) const { + if (!hasActiveHazards(activeHazards) || activeHazardSignature.empty()) { + return true; + } + const auto plan = remainingRoutePlan(route); + return cachedHazardRoutePenalty( + layoutCache, + plan, + position, + startFloorId, + *activeHazards, + activeHazardSignature) <= kHazardSafeRoutePenaltyMeters; + } + + ScenarioRoutePlan safetyConstrainedGuidancePlan( + const ScenarioLayoutCacheResource& layoutCache, + const ScenarioRoutePlan& preferredPlan, + const Point2D& start, + const std::string& startZoneId, + const std::string& agentFloorId, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, + const std::string& activeHazardSignature) const { + if (!hasActiveHazards(activeHazards) || activeHazardSignature.empty()) { + return preferredPlan; + } + if (!preferredPlan.destinationZoneId.empty() + && cachedHazardRoutePenalty( + layoutCache, + preferredPlan, + start, + agentFloorId, + *activeHazards, + activeHazardSignature) <= kHazardSafeRoutePenaltyMeters) { + return preferredPlan; + } + + auto fallback = routePlanToHazardAwareNearestExit( + layoutCache, + start, + startZoneId, + agentFloorId, + *activeHazards); + if (!fallback.destinationZoneId.empty()) { + return fallback; + } + return preferredPlan; + } + const ActiveRouteGuidance* matchingActiveGuidance( const std::vector& activeGuidances, const std::string& guidanceEventId) const { @@ -604,7 +810,10 @@ class ScenarioRouteGuidanceController::Impl final { const ScenarioLayoutCacheResource& layoutCache, const std::vector& activeGuidances, double elapsedSeconds, - std::uint64_t stableSeed) { + std::uint64_t stableSeed, + const ScenarioEnvironmentReactionResource* reactions, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, + const std::string& activeHazardSignature) { const auto& status = query.get(entity); if (status.evacuated) { return; @@ -621,6 +830,10 @@ class ScenarioRouteGuidanceController::Impl final { if (startZoneId.empty()) { return; } + const auto agentFloorId = !route.displayFloorId.empty() ? route.displayFloorId : route.currentFloorId; + const auto useHazardAwareSafety = agentHazardAware(reactions, entity) + && hasActiveHazards(activeHazards) + && !activeHazardSignature.empty(); if (activeGuidances.empty()) { route.guidanceEventId.clear(); @@ -631,7 +844,15 @@ class ScenarioRouteGuidanceController::Impl final { return; } - if (route.destinationZoneId == desiredExit && !route.waypoints.empty()) { + if (route.destinationZoneId == desiredExit && !route.waypoints.empty() + && (!useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature))) { return; } @@ -639,9 +860,30 @@ class ScenarioRouteGuidanceController::Impl final { if (plan.destinationZoneId.empty()) { plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); } + if (useHazardAwareSafety) { + plan = safetyConstrainedGuidancePlan( + layoutCache, + plan, + position.value, + startZoneId, + agentFloorId, + activeHazards, + activeHazardSignature); + } if (plan.destinationZoneId.empty()) { return; } + if (route.destinationZoneId == plan.destinationZoneId && !route.waypoints.empty() + && (!useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature))) { + return; + } replaceRouteWithPlan(route, plan, position.value); route.nextExitReplanSeconds = elapsedSeconds + 0.25; return; @@ -655,7 +897,15 @@ class ScenarioRouteGuidanceController::Impl final { const bool routeNeedsConnectionCorrection = guidanceTargetsExitConnection(layoutCache, *visibleGuidance->guidance) && !routeUsesConnection(route, visibleGuidance->guidance->installConnectionId); - if (!routeNeedsConnectionCorrection) { + if (!routeNeedsConnectionCorrection + && (!useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature))) { return; } } @@ -670,7 +920,15 @@ class ScenarioRouteGuidanceController::Impl final { const bool routeNeedsConnectionCorrection = guidanceTargetsExitConnection(layoutCache, *retained->guidance) && !routeUsesConnection(route, retained->guidance->installConnectionId); - if (!routeNeedsConnectionCorrection) { + if (!routeNeedsConnectionCorrection + && (!useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature))) { return; } activeGuidance = retained; @@ -682,12 +940,39 @@ class ScenarioRouteGuidanceController::Impl final { route.guidanceEventId.clear(); route.followsGuidance = false; const auto& desiredExit = route.originalDestinationZoneId; - if (!desiredExit.empty() && (route.destinationZoneId != desiredExit || route.waypoints.empty())) { + if (!desiredExit.empty()) { + const bool currentRouteSafe = !useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature); + const bool currentNeedsPlan = + route.destinationZoneId != desiredExit || route.waypoints.empty() || !currentRouteSafe; + if (!currentNeedsPlan) { + return; + } + auto plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); if (plan.destinationZoneId.empty()) { plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); } - if (!plan.destinationZoneId.empty()) { + if (useHazardAwareSafety) { + plan = safetyConstrainedGuidancePlan( + layoutCache, + plan, + position.value, + startZoneId, + agentFloorId, + activeHazards, + activeHazardSignature); + } + if (!plan.destinationZoneId.empty() + && (plan.destinationZoneId != route.destinationZoneId + || route.waypoints.empty() + || !currentRouteSafe)) { replaceRouteWithPlan(route, plan, position.value); route.nextExitReplanSeconds = elapsedSeconds + 0.25; } @@ -743,7 +1028,16 @@ class ScenarioRouteGuidanceController::Impl final { && guidanceTargetsExitConnection(layoutCache, selectedGuidance); if (!desiredExit.empty() && route.destinationZoneId == desiredExit && !route.waypoints.empty() && !shouldUseGuidanceConnection) { - return; + if (!useHazardAwareSafety + || remainingRouteHazardSafe( + layoutCache, + route, + position.value, + agentFloorId, + activeHazards, + activeHazardSignature)) { + return; + } } ScenarioRoutePlan plan; @@ -754,8 +1048,28 @@ class ScenarioRouteGuidanceController::Impl final { plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); } } + if (useHazardAwareSafety) { + plan = safetyConstrainedGuidancePlan( + layoutCache, + plan, + position.value, + startZoneId, + agentFloorId, + activeHazards, + activeHazardSignature); + } if (plan.destinationZoneId.empty()) { - plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); + if (useHazardAwareSafety && activeHazards != nullptr) { + plan = routePlanToHazardAwareNearestExit( + layoutCache, + position.value, + startZoneId, + agentFloorId, + *activeHazards); + } + if (plan.destinationZoneId.empty()) { + plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); + } } if (plan.destinationZoneId.empty()) { return; @@ -770,6 +1084,8 @@ class ScenarioRouteGuidanceController::Impl final { const ScenarioLayoutCacheResource& layoutCache, double elapsedSeconds, std::uint64_t derivedSeed, + const ScenarioEnvironmentReactionResource* reactions, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, const ScenarioAgentSpatialIndexResource* sharedSpatialIndex) { // Keep this small to avoid frame spikes when guidance toggles. // Higher values converge faster but may cause noticeable hitching with many agents. @@ -777,17 +1093,24 @@ class ScenarioRouteGuidanceController::Impl final { const auto activeGuidances = activeRouteGuidances(elapsedSeconds); const auto activeSignature = activeRouteGuidanceSignature(activeGuidances); + const auto hazardSignature = activeHazardSignature(activeHazards); + refreshRouteSafetyCache(activeSignature, hazardSignature); const auto hasVisibilityAnchoredGuidance = std::any_of(activeGuidances.begin(), activeGuidances.end(), [&](const auto& active) { return active.guidance != nullptr && (!active.guidance->installConnectionId.empty() || guidanceHasInstallPosition(*active.guidance)); }); + const auto hasGlobalGuidance = std::any_of(activeGuidances.begin(), activeGuidances.end(), [&](const auto& active) { + return active.guidance != nullptr + && active.guidance->installConnectionId.empty() + && !guidanceHasInstallPosition(*active.guidance); + }); if (activeSignature != activeRouteGuidanceId_) { activeRouteGuidanceId_ = activeSignature; guidanceReplanCursor_ = 0; guidanceReplanSeed_ = derivedSeed; guidanceReplanPending_ = true; - } else if (hasVisibilityAnchoredGuidance && !guidanceReplanPending_) { + } else if (hasGlobalGuidance && !guidanceReplanPending_) { guidanceReplanCursor_ = 0; guidanceReplanSeed_ = derivedSeed; guidanceReplanPending_ = true; @@ -812,7 +1135,10 @@ class ScenarioRouteGuidanceController::Impl final { layoutCache, activeGuidances, elapsedSeconds, - guidanceReplanSeed_); + guidanceReplanSeed_, + reactions, + activeHazards, + hazardSignature); } } @@ -824,7 +1150,16 @@ class ScenarioRouteGuidanceController::Impl final { const auto stableSeed = guidanceReplanSeed_; for (std::size_t i = guidanceReplanCursor_; i < endIndex; ++i) { - applyRouteGuidanceToEntity(query, entities[i], layoutCache, activeGuidances, elapsedSeconds, stableSeed); + applyRouteGuidanceToEntity( + query, + entities[i], + layoutCache, + activeGuidances, + elapsedSeconds, + stableSeed, + reactions, + activeHazards, + hazardSignature); } guidanceReplanCursor_ = endIndex; @@ -1118,63 +1453,139 @@ class ScenarioRouteGuidanceController::Impl final { return plan; } + static Point2D interpolateSegmentPoint(const Point2D& start, const Point2D& end, double t) { + return { + .x = start.x + ((end.x - start.x) * t), + .y = start.y + ((end.y - start.y) * t), + }; + } + + static double dotDelta(double ax, double ay, double bx, double by) { + return (ax * bx) + (ay * by); + } + + static double hazardSegmentExposureMeters( + const ScenarioActiveEnvironmentHazard& hazard, + const Point2D& segmentStart, + const Point2D& segmentEnd) { + const auto radius = std::max(0.0, hazard.radiusMeters); + if (radius <= 1e-9) { + return 0.0; + } + + const auto dx = segmentEnd.x - segmentStart.x; + const auto dy = segmentEnd.y - segmentStart.y; + const auto lengthSquared = (dx * dx) + (dy * dy); + if (lengthSquared <= 1e-12) { + return 0.0; + } + + const auto fx = segmentStart.x - hazard.draft.position.x; + const auto fy = segmentStart.y - hazard.draft.position.y; + const auto a = lengthSquared; + const auto b = 2.0 * dotDelta(fx, fy, dx, dy); + const auto c = dotDelta(fx, fy, fx, fy) - (radius * radius); + const auto discriminant = (b * b) - (4.0 * a * c); + if (discriminant < -1e-12) { + return 0.0; + } + + double enter = 0.0; + double exit = 1.0; + if (discriminant >= 0.0) { + const auto root = std::sqrt(std::max(0.0, discriminant)); + enter = (-b - root) / (2.0 * a); + exit = (-b + root) / (2.0 * a); + } + + enter = std::clamp(enter, 0.0, 1.0); + exit = std::clamp(exit, 0.0, 1.0); + if (exit <= enter + 1e-12) { + const auto startInside = distanceBetween(segmentStart, hazard.draft.position) <= radius + 1e-9; + const auto endInside = distanceBetween(segmentEnd, hazard.draft.position) <= radius + 1e-9; + if (!startInside && !endInside) { + return 0.0; + } + enter = 0.0; + exit = 1.0; + } + + const auto overlapLength = std::sqrt(lengthSquared) * (exit - enter); + if (overlapLength <= 1e-9) { + return 0.0; + } + + const auto midpoint = interpolateSegmentPoint(segmentStart, segmentEnd, (enter + exit) * 0.5); + const auto influence = environmentHazardInfluenceAt( + hazard.draft, + distanceBetween(midpoint, hazard.draft.position)); + return overlapLength * influence; + } + double hazardRoutePenalty( const ScenarioLayoutCacheResource& layoutCache, const ScenarioRoutePlan& plan, const Point2D& start, const std::string& startFloorId, const ScenarioActiveEnvironmentHazardsResource& activeHazards) const { - double penalty = 0.0; - for (const auto& hazard : activeHazards.hazards) { - bool routeTouchesHazard = false; - - Point2D segmentStart = start; - std::string segmentFloorId = startFloorId; - for (std::size_t waypointIndex = 0; waypointIndex < plan.waypoints.size(); ++waypointIndex) { - const auto& segmentEnd = plan.waypoints[waypointIndex]; - if (matchesFloor(segmentFloorId, hazard.floorId) - && distanceBetween( - hazard.draft.position, - closestPointOnSegment(hazard.draft.position, segmentStart, segmentEnd)) <= hazard.radiusMeters + 1e-9) { - routeTouchesHazard = true; - break; + if (activeHazards.hazards.empty()) { + return 0.0; + } + + std::vector exposureMeters(activeHazards.hazards.size(), 0.0); + + auto addSegmentExposure = [&](const Point2D& segmentStart, + const Point2D& segmentEnd, + const std::string& segmentFloorId) { + const auto candidateHazards = scenarioHazardIndicesNearSegment( + activeHazards, + segmentStart, + segmentEnd, + segmentFloorId, + activeHazards.maxRadiusMeters); + for (const auto hazardIndex : candidateHazards) { + if (hazardIndex >= activeHazards.hazards.size()) { + continue; + } + const auto& hazard = activeHazards.hazards[hazardIndex]; + if (!matchesFloor(segmentFloorId, hazard.floorId)) { + continue; } + exposureMeters[hazardIndex] += hazardSegmentExposureMeters(hazard, segmentStart, segmentEnd); + } + }; - if (waypointIndex < plan.waypointConnectionIds.size()) { - const auto* connection = findConnectionById(layoutCache, plan.waypointConnectionIds[waypointIndex]); - if (connection == nullptr) { - segmentStart = segmentEnd; - if (waypointIndex < plan.waypointFloorIds.size() - && !plan.waypointFloorIds[waypointIndex].empty()) { - segmentFloorId = plan.waypointFloorIds[waypointIndex]; - } - continue; - } + Point2D segmentStart = start; + std::string segmentFloorId = startFloorId; + for (std::size_t waypointIndex = 0; waypointIndex < plan.waypoints.size(); ++waypointIndex) { + const auto& segmentEnd = plan.waypoints[waypointIndex]; + addSegmentExposure(segmentStart, segmentEnd, segmentFloorId); + + if (waypointIndex < plan.waypointConnectionIds.size()) { + const auto* connection = findConnectionById(layoutCache, plan.waypointConnectionIds[waypointIndex]); + if (connection != nullptr) { const auto connectionFloorId = connection->floorId.empty() ? cachedFloorIdForZone(layoutCache, connection->fromZoneId) : connection->floorId; - if (matchesFloor(connectionFloorId, hazard.floorId) - && distanceBetween( - hazard.draft.position, - closestPointOnSegment( - hazard.draft.position, - connection->centerSpan.start, - connection->centerSpan.end)) <= hazard.radiusMeters + 1e-9) { - routeTouchesHazard = true; - break; - } + addSegmentExposure(connection->centerSpan.start, connection->centerSpan.end, connectionFloorId); } + } - segmentStart = segmentEnd; - if (waypointIndex < plan.waypointFloorIds.size() - && !plan.waypointFloorIds[waypointIndex].empty()) { - segmentFloorId = plan.waypointFloorIds[waypointIndex]; - } + segmentStart = segmentEnd; + if (waypointIndex < plan.waypointFloorIds.size() + && !plan.waypointFloorIds[waypointIndex].empty()) { + segmentFloorId = plan.waypointFloorIds[waypointIndex]; } + } - if (routeTouchesHazard) { - penalty += hazard.routePenaltyMeters; + double penalty = 0.0; + for (std::size_t hazardIndex = 0; hazardIndex < activeHazards.hazards.size(); ++hazardIndex) { + const auto& hazard = activeHazards.hazards[hazardIndex]; + const auto radius = std::max(0.0, hazard.radiusMeters); + if (radius <= 1e-9 || exposureMeters[hazardIndex] <= 1e-9) { + continue; } + penalty += hazard.routePenaltyMeters * std::clamp(exposureMeters[hazardIndex] / radius, 0.0, 1.0); } return penalty; } @@ -1185,8 +1596,15 @@ class ScenarioRouteGuidanceController::Impl final { const std::string& startZoneId, const std::string& agentFloorId, const ScenarioActiveEnvironmentHazardsResource& activeHazards) const { - std::string bestExitZoneId; - double bestScore = std::numeric_limits::max(); + const auto hazardSignature = activeHazardSignature(&activeHazards); + const auto cacheKey = hazardAwareExitPlanCacheKey(start, startZoneId, agentFloorId, hazardSignature); + const auto cachedIt = hazardAwareExitPlanCache_.find(cacheKey); + if (cachedIt != hazardAwareExitPlanCache_.end()) { + return cachedIt->second; + } + + ScenarioRoutePlan bestPlan; + double bestPenalty = std::numeric_limits::max(); double bestDistance = std::numeric_limits::max(); for (const auto& zone : layoutCache.layout.zones) { @@ -1199,29 +1617,28 @@ class ScenarioRouteGuidanceController::Impl final { continue; } - if (result->distance >= bestScore) { - continue; - } - const auto plan = routePlanToExit(layoutCache, start, startZoneId, zone.id); if (plan.destinationZoneId.empty()) { continue; } - const auto penalty = hazardRoutePenalty(layoutCache, plan, start, agentFloorId, activeHazards); - const auto score = result->distance + penalty; - if (score + 1e-9 < bestScore - || (std::fabs(score - bestScore) <= 1e-9 && result->distance < bestDistance)) { - bestScore = score; + const auto penalty = cachedHazardRoutePenalty( + layoutCache, + plan, + start, + agentFloorId, + activeHazards, + hazardSignature); + if (penalty + 1e-9 < bestPenalty + || (std::fabs(penalty - bestPenalty) <= 1e-9 && result->distance < bestDistance)) { + bestPenalty = penalty; bestDistance = result->distance; - bestExitZoneId = zone.id; + bestPlan = plan; } } - if (bestExitZoneId.empty()) { - return {}; - } - return routePlanToExit(layoutCache, start, startZoneId, bestExitZoneId); + hazardAwareExitPlanCache_.emplace(cacheKey, bestPlan); + return bestPlan; } void replaceRouteWithPlan(EvacuationRoute& route, const ScenarioRoutePlan& plan, const Point2D& start) const { @@ -1257,6 +1674,10 @@ class ScenarioRouteGuidanceController::Impl final { mutable std::unordered_map> nearestExitRouteCache_{}; mutable std::unordered_map> exitRouteCache_{}; mutable std::unordered_map> pathCache_{}; + mutable std::string routeSafetyGuidanceSignature_{}; + mutable std::string routeSafetyHazardSignature_{}; + mutable std::unordered_map hazardAwareExitPlanCache_{}; + mutable std::unordered_map routeHazardPenaltyCache_{}; }; ScenarioRouteGuidanceController::ScenarioRouteGuidanceController() @@ -1323,8 +1744,18 @@ void ScenarioRouteGuidanceController::apply( const ScenarioLayoutCacheResource& layoutCache, double elapsedSeconds, std::uint64_t derivedSeed, + const ScenarioEnvironmentReactionResource* reactions, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, const ScenarioAgentSpatialIndexResource* sharedSpatialIndex) { - impl_->applyRouteGuidance(query, entities, layoutCache, elapsedSeconds, derivedSeed, sharedSpatialIndex); + impl_->applyRouteGuidance( + query, + entities, + layoutCache, + elapsedSeconds, + derivedSeed, + reactions, + activeHazards, + sharedSpatialIndex); } bool routePlanUsesConnection(const ScenarioRoutePlan& plan, const std::string& connectionId) { diff --git a/src/domain/ScenarioSimulationRouteGuidance.h b/src/domain/ScenarioSimulationRouteGuidance.h index a292ebe..4033182 100644 --- a/src/domain/ScenarioSimulationRouteGuidance.h +++ b/src/domain/ScenarioSimulationRouteGuidance.h @@ -68,6 +68,8 @@ class ScenarioRouteGuidanceController { const ScenarioLayoutCacheResource& layoutCache, double elapsedSeconds, std::uint64_t derivedSeed, + const ScenarioEnvironmentReactionResource* reactions, + const ScenarioActiveEnvironmentHazardsResource* activeHazards, const ScenarioAgentSpatialIndexResource* sharedSpatialIndex); private: diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 9a6c605..1783404 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -339,6 +339,77 @@ std::string hazardRuntimeKey(const EnvironmentHazardDraft& hazard, std::size_t i return "hazard-" + std::to_string(index + 1); } +long long quantizedHazardSignatureValue(double value) { + return static_cast(std::llround(value * 1000.0)); +} + +std::string hazardSignaturePoint(const Point2D& point) { + return std::to_string(quantizedHazardSignatureValue(point.x)) + + "," + + std::to_string(quantizedHazardSignatureValue(point.y)); +} + +std::string activeHazardsSignature(const std::vector& hazards) { + std::string signature; + for (const auto& hazard : hazards) { + if (!signature.empty()) { + signature.push_back('\x1f'); + } + signature.append(hazard.key); + signature.push_back('\x1e'); + signature.append(hazard.floorId); + signature.push_back('\x1e'); + signature.append(hazardSignaturePoint(hazard.draft.position)); + signature.push_back('\x1e'); + signature.append(std::to_string(static_cast(hazard.draft.kind))); + signature.push_back('\x1e'); + signature.append(std::to_string(static_cast(hazard.draft.severity))); + signature.push_back('\x1e'); + signature.append(std::to_string(quantizedHazardSignatureValue(hazard.radiusMeters))); + signature.push_back('\x1e'); + signature.append(std::to_string(quantizedHazardSignatureValue(hazard.routePenaltyMeters))); + } + return signature; +} + +void insertHazardCoverageCells( + std::unordered_map>>& cellsByFloor, + const ScenarioActiveEnvironmentHazard& hazard, + double cellSize, + std::size_t hazardIndex) { + const auto radius = std::max(0.0, hazard.radiusMeters); + if (cellSize <= 1e-9 || radius <= 1e-9) { + return; + } + + auto& floorCells = cellsByFloor[hazard.floorId]; + const Point2D minPoint{ + .x = hazard.draft.position.x - radius, + .y = hazard.draft.position.y - radius, + }; + const Point2D maxPoint{ + .x = hazard.draft.position.x + radius, + .y = hazard.draft.position.y + radius, + }; + for (const auto& cell : spatialCellsForBounds(minPoint, maxPoint, cellSize)) { + floorCells[spatialKey(cell)].push_back(hazardIndex); + } +} + +void refreshActiveHazardSpatialCache(ScenarioActiveEnvironmentHazardsResource& resource) { + constexpr double kHazardCellSizeMeters = 1.0; + resource.signature = activeHazardsSignature(resource.hazards); + resource.cellSizeMeters = kHazardCellSizeMeters; + resource.maxRadiusMeters = 0.0; + resource.hazardIndicesByFloor.clear(); + + for (std::size_t index = 0; index < resource.hazards.size(); ++index) { + const auto& hazard = resource.hazards[index]; + resource.maxRadiusMeters = std::max(resource.maxRadiusMeters, std::max(0.0, hazard.radiusMeters)); + insertHazardCoverageCells(resource.hazardIndicesByFloor, hazard, resource.cellSizeMeters, index); + } +} + HazardExposureMetric hazardExposureMetric( const EnvironmentHazardDraft& hazard, const std::string& key, @@ -504,9 +575,10 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { ? std::max(0.0, resources.get().deltaSeconds) : 0.0; - auto activeHazards = buildActiveHazards(elapsedSeconds); auto& activeResource = resources.get(); - activeResource.hazards = activeHazards; + activeResource.hazards = buildActiveHazards(elapsedSeconds); + refreshActiveHazardSpatialCache(activeResource); + const auto& activeHazards = activeResource.hazards; auto& reactions = resources.get(); auto& exposure = resources.get(); @@ -531,7 +603,18 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { double detectedRadius = 0.0; double bestProximity = std::numeric_limits::max(); - for (const auto& hazard : activeHazards) { + const auto detectionRadius = activeResource.maxRadiusMeters * std::max({ + 1.0, + std::max(0.0, agent.hazardSensitivity), + std::max(0.0, agent.smokeSensitivity), + }); + const auto candidateHazards = + scenarioNearbyHazardIndices(activeResource, position.value, agentFloorId, detectionRadius); + for (const auto hazardIndex : candidateHazards) { + if (hazardIndex >= activeHazards.size()) { + continue; + } + const auto& hazard = activeHazards[hazardIndex]; if (!sameFloor(agentFloorId, hazard.floorId)) { continue; } @@ -815,6 +898,147 @@ std::vector scenarioNearbyBarriers( return candidates; } +namespace { + +bool hazardFloorMatchesQuery(const std::string& hazardFloorId, const std::string& floorId) { + return floorId.empty() || hazardFloorId.empty() || hazardFloorId == floorId; +} + +void appendUniqueHazardIndex(std::vector& indices, std::size_t index) { + if (std::find(indices.begin(), indices.end(), index) == indices.end()) { + indices.push_back(index); + } +} + +std::vector allHazardIndicesForFloor( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const std::string& floorId) { + std::vector indices; + indices.reserve(hazards.hazards.size()); + for (std::size_t index = 0; index < hazards.hazards.size(); ++index) { + if (hazardFloorMatchesQuery(hazards.hazards[index].floorId, floorId)) { + indices.push_back(index); + } + } + return indices; +} + +void appendHazardCellCandidates( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const std::string& candidateFloorId, + const std::vector& cellKeys, + std::vector& indices) { + const auto floorIt = hazards.hazardIndicesByFloor.find(candidateFloorId); + if (floorIt == hazards.hazardIndicesByFloor.end()) { + return; + } + for (const auto cellKey : cellKeys) { + const auto cellIt = floorIt->second.find(cellKey); + if (cellIt == floorIt->second.end()) { + continue; + } + for (const auto index : cellIt->second) { + appendUniqueHazardIndex(indices, index); + } + } +} + +void appendHazardFloorCandidates( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const std::string& floorId, + const std::vector& cellKeys, + std::vector& indices) { + if (floorId.empty()) { + for (const auto& [candidateFloorId, _] : hazards.hazardIndicesByFloor) { + appendHazardCellCandidates(hazards, candidateFloorId, cellKeys, indices); + } + return; + } + appendHazardCellCandidates(hazards, floorId, cellKeys, indices); + appendHazardCellCandidates(hazards, std::string{}, cellKeys, indices); +} + +std::vector spatialKeysAroundPoint(const Point2D& point, double radius, double cellSize) { + std::vector keys; + if (cellSize <= 1e-9) { + return keys; + } + const auto center = spatialCellFor(point, cellSize); + const auto range = std::max(1, static_cast(std::ceil(std::max(0.0, radius) / cellSize))); + keys.reserve(static_cast((range * 2 + 1) * (range * 2 + 1))); + for (int dy = -range; dy <= range; ++dy) { + for (int dx = -range; dx <= range; ++dx) { + keys.push_back(spatialKey({.x = center.x + dx, .y = center.y + dy})); + } + } + return keys; +} + +std::vector spatialKeysForSegmentBounds( + const Point2D& start, + const Point2D& end, + double radius, + double cellSize) { + std::vector keys; + if (cellSize <= 1e-9) { + return keys; + } + const auto expansion = std::max(0.0, radius); + const Point2D minPoint{ + .x = std::min(start.x, end.x) - expansion, + .y = std::min(start.y, end.y) - expansion, + }; + const Point2D maxPoint{ + .x = std::max(start.x, end.x) + expansion, + .y = std::max(start.y, end.y) + expansion, + }; + const auto cells = spatialCellsForBounds(minPoint, maxPoint, cellSize); + keys.reserve(cells.size()); + for (const auto& cell : cells) { + keys.push_back(spatialKey(cell)); + } + return keys; +} + +} // namespace + +std::vector scenarioNearbyHazardIndices( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const Point2D& point, + const std::string& floorId, + double radius) { + if (hazards.hazards.empty()) { + return {}; + } + if (hazards.hazardIndicesByFloor.empty()) { + return allHazardIndicesForFloor(hazards, floorId); + } + + std::vector indices; + const auto cellKeys = spatialKeysAroundPoint(point, radius, hazards.cellSizeMeters); + appendHazardFloorCandidates(hazards, floorId, cellKeys, indices); + return indices; +} + +std::vector scenarioHazardIndicesNearSegment( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const Point2D& start, + const Point2D& end, + const std::string& floorId, + double radius) { + if (hazards.hazards.empty()) { + return {}; + } + if (hazards.hazardIndicesByFloor.empty()) { + return allHazardIndicesForFloor(hazards, floorId); + } + + std::vector indices; + const auto cellKeys = spatialKeysForSegmentBounds(start, end, radius, hazards.cellSizeMeters); + appendHazardFloorCandidates(hazards, floorId, cellKeys, indices); + return indices; +} + ScenarioSpatialIndexSystem::ScenarioSpatialIndexSystem(double cellSize) : cellSize_(std::max(0.1, cellSize)) { } diff --git a/src/domain/ScenarioSimulationSystems.h b/src/domain/ScenarioSimulationSystems.h index f524825..aa722d1 100644 --- a/src/domain/ScenarioSimulationSystems.h +++ b/src/domain/ScenarioSimulationSystems.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -126,6 +127,10 @@ struct ScenarioActiveEnvironmentHazard { struct ScenarioActiveEnvironmentHazardsResource { std::vector hazards{}; + std::string signature{}; + double cellSizeMeters{1.0}; + double maxRadiusMeters{0.0}; + std::unordered_map>> hazardIndicesByFloor{}; }; struct ScenarioHazardExposureResource { @@ -186,6 +191,17 @@ std::vector scenarioNearbyBarriers( const Point2D& point, const std::string& floorId, double radius); +std::vector scenarioNearbyHazardIndices( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const Point2D& point, + const std::string& floorId, + double radius); +std::vector scenarioHazardIndicesNearSegment( + const ScenarioActiveEnvironmentHazardsResource& hazards, + const Point2D& start, + const Point2D& end, + const std::string& floorId, + double radius); std::unique_ptr makeScenarioSimulationMotionSystem(); std::unique_ptr makeScenarioControlSystem( diff --git a/tests/ScenarioAuthoringTests.cpp b/tests/ScenarioAuthoringTests.cpp index a0e13c8..52c792d 100644 --- a/tests/ScenarioAuthoringTests.cpp +++ b/tests/ScenarioAuthoringTests.cpp @@ -121,15 +121,31 @@ SC_TEST(environmentHazardRuntimeProfile_UsesSharedSeverityAndScheduleRules) { SC_EXPECT_TRUE(!environmentHazardHasOpenEndedSchedule(hazard)); } +SC_TEST(environmentHazardInfluenceAt_UsesSmoothSeverityFalloff) { + auto hazard = makeSmokeHazard(); + hazard.severity = ScenarioElementSeverity::High; + + SC_EXPECT_NEAR(environmentHazardInfluenceAt(hazard, 0.0), 1.0, 1e-9); + SC_EXPECT_NEAR(environmentHazardInfluenceAt(hazard, 2.5), 0.5, 1e-9); + SC_EXPECT_NEAR(environmentHazardInfluenceAt(hazard, 5.0), 0.0, 1e-9); + SC_EXPECT_NEAR(environmentHazardInfluenceAt(hazard, 7.5), 0.0, 1e-9); +} + SC_TEST(environmentHazardSmokeSpeed_UsesVisibilityBasedPathfinderRule) { auto hazard = makeSmokeHazard(); hazard.severity = ScenarioElementSeverity::High; SC_EXPECT_NEAR(environmentHazardSmokeVisibilityMetersAt(hazard, 0.0), 0.5, 1e-9); + SC_EXPECT_NEAR(environmentHazardSmokeVisibilityMetersAt(hazard, 2.5), 1.75, 1e-9); SC_EXPECT_NEAR(environmentHazardSmokeVisibilityMetersAt(hazard, 5.0), 3.0, 1e-9); SC_EXPECT_NEAR(environmentHazardSmokeSpeedMetersPerSecond(1.5, 0.5), 0.65, 1e-9); SC_EXPECT_NEAR(environmentHazardSpeedFactorAt(hazard, 0.0, 1.5), 0.65 / 1.5, 1e-9); SC_EXPECT_NEAR(environmentHazardSpeedFactorAt(hazard, 5.0, 1.5), 1.0, 1e-9); + + hazard.kind = EnvironmentHazardKind::Fire; + SC_EXPECT_NEAR(environmentHazardSpeedFactorAt(hazard, 0.0, 1.5), 0.60, 1e-9); + SC_EXPECT_NEAR(environmentHazardSpeedFactorAt(hazard, 2.5, 1.5), 0.80, 1e-9); + SC_EXPECT_NEAR(environmentHazardSpeedFactorAt(hazard, 5.0, 1.5), 1.0, 1e-9); } SC_TEST(environmentHazardFloorId_FallsBackToAffectedZoneFloor) { diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index 681d00d..b073f70 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -598,6 +598,45 @@ void addClosureMotionSystems( .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); } +void addClosureGuidanceMotionSystems( + safecrowd::engine::EngineRuntime& runtime, + const safecrowd::domain::FacilityLayout2D& layout, + std::vector blocks, + std::vector guidances) { + 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, std::move(guidances)), + {.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 addHazardGuidanceMotionSystems( + safecrowd::engine::EngineRuntime& runtime, + const safecrowd::domain::FacilityLayout2D& layout, + std::vector hazards, + std::vector guidances) { + runtime.addSystem( + safecrowd::domain::makeScenarioEnvironmentHazardSystem(layout, std::move(hazards)), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .order = -20, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem(layout, std::move(guidances)), + {.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 addHazardClosureMotionSystems( safecrowd::engine::EngineRuntime& runtime, const safecrowd::domain::FacilityLayout2D& layout, @@ -1349,6 +1388,12 @@ SC_TEST(ScenarioEnvironmentHazardSystem_ReroutesOnlyAfterHazardAwareness) { runtime.stepFrame(0.0); SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); + + for (int i = 0; i < 3; ++i) { + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); + } } SC_TEST(ScenarioEnvironmentHazardSystem_RoutePenaltyUsesPathGeometryNotOnlyAffectedZone) { @@ -1488,6 +1533,14 @@ SC_TEST(ScenarioEnvironmentHazardSystem_IgnoresInactiveAndDifferentFloorHazards) const auto reactionIt = reactions.agentsById.find(0); SC_EXPECT_TRUE(reactionIt == reactions.agentsById.end() || !reactionIt->second.hazardInRange); + const auto& activeHazards = + runtime.world().resources().get(); + SC_EXPECT_EQ(activeHazards.hazards.size(), std::size_t{1}); + SC_EXPECT_TRUE(!activeHazards.signature.empty()); + SC_EXPECT_TRUE(activeHazards.maxRadiusMeters > 0.0); + SC_EXPECT_TRUE(safecrowd::domain::scenarioNearbyHazardIndices(activeHazards, {.x = 0.25, .y = 0.0}, "L1", 5.0).empty()); + SC_EXPECT_TRUE(!safecrowd::domain::scenarioNearbyHazardIndices(activeHazards, {.x = 0.25, .y = 0.0}, "L2", 5.0).empty()); + const auto& exposure = runtime.world().resources().get(); for (const auto& [_, metric] : exposure.hazardsById) { @@ -2293,6 +2346,287 @@ SC_TEST(ScenarioSimulationMotionSystem_PrioritizesAgentsNearInstalledGuidanceBef SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); } +SC_TEST(ScenarioSimulationMotionSystem_GuidanceFallsBackWhenTargetConnectionBlocked) { + auto layout = twoExitGuidanceDetourLayout(); + safecrowd::domain::ConnectionBlockDraft block; + block.id = "block-near-exit"; + block.connectionId = "room-near-exit"; + + auto seed = doorRouteSeed( + {.x = 1.6, .y = 0.5}, + "far-exit", + "room-far-exit", + {{.x = 2.0, .y = 3.3}, {.x = 2.0, .y = 3.7}}); + seed.agent.guidancePropensity = 1.0; + seed.route.originalDestinationZoneId = "far-exit"; + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "blocked-near-guidance"; + guidance.guidedExitZoneId = "near-exit"; + guidance.installConnectionId = "room-near-exit"; + guidance.baseComplianceRate = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 121, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addClosureGuidanceMotionSystems(runtime, layout, {block}, {guidance}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.guidanceEventId, std::string{"blocked-near-guidance"}); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); + SC_EXPECT_TRUE(std::none_of( + route.waypointConnectionIds.begin(), + route.waypointConnectionIds.end(), + [](const auto& connectionId) { + return connectionId == "room-near-exit"; + })); +} + +SC_TEST(ScenarioSimulationMotionSystem_HazardAwareGuidanceAvoidsUnsafeGuidedRoute) { + auto seed = doorRouteSeed( + {.x = 5.0, .y = 5.0}, + "far-exit", + "room-far-exit", + {{.x = 10.0, .y = 8.7}, {.x = 10.0, .y = 9.3}}); + seed.agent.guidancePropensity = 1.0; + seed.agent.hazardSensitivity = 4.0; + seed.agent.reactionDelaySeconds = 0.0; + seed.route.originalDestinationZoneId = "far-exit"; + + auto fire = hazardDraft( + "near-route-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::Low, + {.x = 9.5, .y = 1.0}, + "room"); + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "global-near-guidance"; + guidance.guidedExitZoneId = "near-exit"; + guidance.baseComplianceRate = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 122, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addHazardGuidanceMotionSystems(runtime, wideTwoExitHazardRouteLayout(), {fire}, {guidance}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& reaction = + runtime.world().resources().get().agentsById.at(entities.front().index); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_TRUE(reaction.hazardAware); + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.guidanceEventId, std::string{"global-near-guidance"}); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); +} + +SC_TEST(ScenarioSimulationMotionSystem_DelaysGuidanceHazardAvoidanceUntilAwareness) { + auto seed = doorRouteSeed( + {.x = 5.0, .y = 5.0}, + "far-exit", + "room-far-exit", + {{.x = 10.0, .y = 8.7}, {.x = 10.0, .y = 9.3}}); + seed.agent.guidancePropensity = 1.0; + seed.agent.hazardSensitivity = 4.0; + seed.agent.reactionDelaySeconds = 10.0; + seed.route.originalDestinationZoneId = "far-exit"; + + auto fire = hazardDraft( + "delayed-near-route-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::Low, + {.x = 9.5, .y = 1.0}, + "room"); + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "delayed-global-near-guidance"; + guidance.guidedExitZoneId = "near-exit"; + guidance.baseComplianceRate = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 123, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addHazardGuidanceMotionSystems(runtime, wideTwoExitHazardRouteLayout(), {fire}, {guidance}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& reaction = + runtime.world().resources().get().agentsById.at(entities.front().index); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_TRUE(reaction.hazardDetected); + SC_EXPECT_TRUE(!reaction.hazardAware); + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"near-exit"}); +} + +SC_TEST(ScenarioSimulationMotionSystem_BudgetsHazardAwareExitReplans) { + std::vector seeds; + for (int index = 0; index < 60; ++index) { + auto seed = doorRouteSeed( + {.x = 5.0, .y = 5.0}, + "near-exit", + "room-near-exit", + {{.x = 10.0, .y = 0.7}, {.x = 10.0, .y = 1.3}}); + seed.agent.hazardSensitivity = 2.0; + seed.agent.reactionDelaySeconds = 0.0; + seeds.push_back(seed); + } + + auto fire = hazardDraft( + "budget-near-route-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::High, + {.x = 9.5, .y = 1.0}, + "room"); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 125, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + addHazardMotionSystems(runtime, wideTwoExitHazardRouteLayout(), {fire}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + int farExitCount = 0; + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + for (const auto entity : entities) { + const auto& route = runtime.world().query().get(entity); + if (route.destinationZoneId == "far-exit") { + ++farExitCount; + } + } + + SC_EXPECT_EQ(farExitCount, 50); +} + +SC_TEST(ScenarioSimulationMotionSystem_GlobalGuidanceKeepsBudgetedDeterministicSweep) { + std::vector seeds; + for (int index = 0; index < 60; ++index) { + const auto y = 0.2 + (0.05 * static_cast(index)); + seeds.push_back({ + .position = {.value = {.x = 0.5, .y = y}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f, .guidancePropensity = 1.0}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 0.5, .y = y}, + .previousDistanceToWaypoint = 1.5, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + } + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "global-guidance"; + guidance.guidedExitZoneId = "far-exit"; + guidance.baseComplianceRate = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 0.1, + .maxCatchUpSteps = 1, + .baseSeed = 124, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem( + twoExitGuidanceDetourLayout(), + std::vector{guidance}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + stepScenarioRuntime(runtime, 0.1); + + auto guidedCount = [&]() { + int count = 0; + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + for (const auto entity : entities) { + const auto& route = runtime.world().query().get(entity); + if (route.guidanceEventId == "global-guidance") { + ++count; + } + } + return count; + }; + + SC_EXPECT_EQ(guidedCount(), 50); + + stepScenarioRuntime(runtime, 0.1); + SC_EXPECT_EQ(guidedCount(), 60); +} + SC_TEST(ScenarioSimulationMotionSystem_SkipsIntermediateWaypointWhenCrowdPushesAgentPastApproachArea) { std::vector seeds; seeds.push_back({