From 0530f3b5b51f88f16d7d77e106e8baa561914b67 Mon Sep 17 00:00:00 2001 From: Silversupplier Date: Mon, 11 May 2026 19:45:48 +0900 Subject: [PATCH 1/2] [Domain] Add fire smoke runtime reaction --- src/application/ScenarioBatchResultWidget.cpp | 1 + src/application/ScenarioResultWidget.cpp | 1 + src/application/ScenarioRunWidget.cpp | 2 + src/application/SimulationCanvasWidget.cpp | 260 +++++++++++++- src/application/SimulationCanvasWidget.h | 4 + src/domain/ScenarioResultArtifacts.h | 22 ++ src/domain/ScenarioSimulationMotionSystem.cpp | 202 ++++++++++- src/domain/ScenarioSimulationRunner.cpp | 5 + src/domain/ScenarioSimulationSystems.cpp | 321 ++++++++++++++++++ src/domain/ScenarioSimulationSystems.h | 31 ++ tests/ScenarioSimulationRunnerTests.cpp | 51 +++ tests/ScenarioSimulationSystemsTests.cpp | 276 +++++++++++++++ 12 files changed, 1154 insertions(+), 22 deletions(-) diff --git a/src/application/ScenarioBatchResultWidget.cpp b/src/application/ScenarioBatchResultWidget.cpp index df46669..21e90a6 100644 --- a/src/application/ScenarioBatchResultWidget.cpp +++ b/src/application/ScenarioBatchResultWidget.cpp @@ -997,6 +997,7 @@ void ScenarioBatchResultWidget::applyReplayFrameData(const safecrowd::domain::Si if (canvas_ != nullptr) { canvas_->setConnectionBlocks(result.scenario.control.connectionBlocks); + canvas_->setEnvironmentHazards(result.scenario.environment.hazards); canvas_->setFrame(frame); canvas_->setHotspotOverlay(result.risk.hotspots); canvas_->setBottleneckOverlay(result.risk.bottlenecks); diff --git a/src/application/ScenarioResultWidget.cpp b/src/application/ScenarioResultWidget.cpp index 606fe08..24dccd9 100644 --- a/src/application/ScenarioResultWidget.cpp +++ b/src/application/ScenarioResultWidget.cpp @@ -1284,6 +1284,7 @@ ScenarioResultWidget::ScenarioResultWidget( auto* canvas = new SimulationCanvasWidget(layout_, shell_); canvas->setFrame(frame_); + canvas->setEnvironmentHazards(scenario_.environment.hazards); canvas->setHotspotOverlay(risk_.hotspots); canvas->setBottleneckOverlay(risk_.bottlenecks); ResultReplayControls* replayControls = nullptr; diff --git a/src/application/ScenarioRunWidget.cpp b/src/application/ScenarioRunWidget.cpp index abf61c3..a2e9deb 100644 --- a/src/application/ScenarioRunWidget.cpp +++ b/src/application/ScenarioRunWidget.cpp @@ -377,6 +377,7 @@ QWidget* ScenarioRunWidget::createRunCanvas() { if (!batchRunner_.empty()) { const auto& run = batchRunner_.run(0); canvas_->setConnectionBlocks(run.scenario.control.connectionBlocks); + canvas_->setEnvironmentHazards(run.scenario.environment.hazards); canvas_->setRouteGuidances(run.scenario.control.routeGuidances); canvas_->setFrame(run.frame); } @@ -568,6 +569,7 @@ void ScenarioRunWidget::refreshStatus() { const auto& frame = selectedRun.frame; if (canvas_ != nullptr) { canvas_->setConnectionBlocks(selectedRun.scenario.control.connectionBlocks); + canvas_->setEnvironmentHazards(selectedRun.scenario.environment.hazards); canvas_->setRouteGuidances(selectedRun.scenario.control.routeGuidances); canvas_->setFrame(selectedRun.frame); } diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index aee2ecf..94d2fbd 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -71,6 +72,84 @@ bool connectionShouldBeBlocked(const safecrowd::domain::ConnectionBlockDraft& bl return false; } +bool hazardIsActive(const safecrowd::domain::EnvironmentHazardDraft& hazard, double timeSeconds) { + const auto start = std::max(0.0, hazard.startSeconds); + if (timeSeconds + 1e-9 < start) { + return false; + } + if (hazard.endSeconds <= hazard.startSeconds) { + return true; + } + const auto end = std::max(start, hazard.endSeconds); + return timeSeconds <= end + 1e-9; +} + +double hazardRadiusMeters(safecrowd::domain::ScenarioElementSeverity severity) { + switch (severity) { + case safecrowd::domain::ScenarioElementSeverity::Low: + return 2.0; + case safecrowd::domain::ScenarioElementSeverity::High: + return 5.0; + case safecrowd::domain::ScenarioElementSeverity::Medium: + default: + return 3.5; + } +} + +std::string hazardFloorId( + const safecrowd::domain::FacilityLayout2D& layout, + const safecrowd::domain::EnvironmentHazardDraft& hazard) { + if (!hazard.floorId.empty()) { + return hazard.floorId; + } + if (hazard.affectedZoneId.empty()) { + return {}; + } + const auto zoneIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { + return zone.id == hazard.affectedZoneId; + }); + return zoneIt == layout.zones.end() ? std::string{} : zoneIt->floorId; +} + +QString hazardKindLabel(safecrowd::domain::EnvironmentHazardKind kind) { + switch (kind) { + case safecrowd::domain::EnvironmentHazardKind::Smoke: + return "Smoke"; + case safecrowd::domain::EnvironmentHazardKind::Fire: + default: + return "Fire"; + } +} + +QString severityLabel(safecrowd::domain::ScenarioElementSeverity severity) { + switch (severity) { + case safecrowd::domain::ScenarioElementSeverity::Low: + return "Low"; + case safecrowd::domain::ScenarioElementSeverity::High: + return "High"; + case safecrowd::domain::ScenarioElementSeverity::Medium: + default: + return "Medium"; + } +} + +QString formatEnvironmentHazardTooltip(const safecrowd::domain::EnvironmentHazardDraft& hazard) { + QString text = QString("%1 hazard").arg(hazardKindLabel(hazard.kind)); + if (!hazard.name.empty()) { + text.append(QString("\n%1").arg(QString::fromStdString(hazard.name))); + } + const auto start = std::max(0.0, hazard.startSeconds); + if (hazard.endSeconds <= hazard.startSeconds) { + text.append(QString("\nActive: %1s ~ open").arg(start, 0, 'f', 1)); + } else { + text.append(QString("\nActive: %1s ~ %2s") + .arg(start, 0, 'f', 1) + .arg(std::max(start, hazard.endSeconds), 0, 'f', 1)); + } + text.append(QString("\nSeverity: %1").arg(severityLabel(hazard.severity))); + return text; +} + safecrowd::domain::Point2D connectionCenter(const safecrowd::domain::Connection2D& connection) { return { .x = (connection.centerSpan.start.x + connection.centerSpan.end.x) * 0.5, @@ -203,6 +282,38 @@ std::optional hoveredBlockedConnectionIndex( return closestIndex; } +std::optional hoveredActiveEnvironmentHazardIndex( + const safecrowd::domain::FacilityLayout2D& layout, + const std::vector& hazards, + const LayoutCanvasTransform& transform, + const std::string& currentFloorId, + double elapsedSeconds, + const QPointF& screenPosition) { + constexpr double kHoverRadiusPixels = 15.0; + + std::optional closestIndex; + double closestDistanceSq = kHoverRadiusPixels * kHoverRadiusPixels; + for (std::size_t index = 0; index < hazards.size(); ++index) { + const auto& hazard = hazards[index]; + if (!hazardIsActive(hazard, elapsedSeconds)) { + continue; + } + if (!matchesFloor(hazardFloorId(layout, hazard), currentFloorId)) { + continue; + } + + const auto center = transform.map(hazard.position); + const auto dx = center.x() - screenPosition.x(); + const auto dy = center.y() - screenPosition.y(); + const auto distanceSq = (dx * dx) + (dy * dy); + if (distanceSq <= closestDistanceSq) { + closestDistanceSq = distanceSq; + closestIndex = index; + } + } + return closestIndex; +} + struct ActiveRouteGuidanceSelection { std::size_t guidanceIndex{0}; std::size_t periodIndex{0}; @@ -407,6 +518,11 @@ void SimulationCanvasWidget::setConnectionBlocks(std::vector hazards) { + environmentHazards_ = std::move(hazards); + update(); +} + void SimulationCanvasWidget::setRouteGuidances(std::vector guidances) { routeGuidances_ = std::move(guidances); update(); @@ -506,6 +622,7 @@ void SimulationCanvasWidget::keyReleaseEvent(QKeyEvent* event) { void SimulationCanvasWidget::leaveEvent(QEvent* event) { hoveredConnectionBlockId_.clear(); + hoveredEnvironmentHazardId_.clear(); hoveredRouteGuidanceId_.clear(); QToolTip::hideText(); QWidget::leaveEvent(event); @@ -524,8 +641,11 @@ void SimulationCanvasWidget::mouseDoubleClickEvent(QMouseEvent* event) { void SimulationCanvasWidget::mouseMoveEvent(QMouseEvent* event) { if (camera_.updatePan(event)) { - if (!hoveredConnectionBlockId_.empty() || !hoveredRouteGuidanceId_.empty()) { + if (!hoveredConnectionBlockId_.empty() + || !hoveredEnvironmentHazardId_.empty() + || !hoveredRouteGuidanceId_.empty()) { hoveredConnectionBlockId_.clear(); + hoveredEnvironmentHazardId_.clear(); hoveredRouteGuidanceId_.clear(); QToolTip::hideText(); } @@ -536,8 +656,12 @@ void SimulationCanvasWidget::mouseMoveEvent(QMouseEvent* event) { const auto bounds = collectBounds(); if (!bounds.has_value()) { - if (!hoveredConnectionBlockId_.empty()) { + if (!hoveredConnectionBlockId_.empty() + || !hoveredEnvironmentHazardId_.empty() + || !hoveredRouteGuidanceId_.empty()) { hoveredConnectionBlockId_.clear(); + hoveredEnvironmentHazardId_.clear(); + hoveredRouteGuidanceId_.clear(); QToolTip::hideText(); } QWidget::mouseMoveEvent(event); @@ -561,6 +685,13 @@ void SimulationCanvasWidget::mouseMoveEvent(QMouseEvent* event) { currentFloorId_, elapsedSeconds, event->position()); + const auto hoveredHazard = hoveredActiveEnvironmentHazardIndex( + layout_, + environmentHazards_, + transform, + currentFloorId_, + elapsedSeconds, + event->position()); if (hoveredGuidance.has_value()) { const auto& guidance = routeGuidances_[*hoveredGuidance]; @@ -571,34 +702,59 @@ void SimulationCanvasWidget::mouseMoveEvent(QMouseEvent* event) { if (hoveredId != hoveredRouteGuidanceId_) { hoveredRouteGuidanceId_ = hoveredId; hoveredConnectionBlockId_.clear(); + hoveredEnvironmentHazardId_.clear(); QToolTip::showText(event->globalPosition().toPoint(), tooltip, this); } QWidget::mouseMoveEvent(event); return; } - if (!hoveredIndex.has_value()) { - if (!hoveredConnectionBlockId_.empty() || !hoveredRouteGuidanceId_.empty()) { + if (hoveredIndex.has_value()) { + const auto& block = connectionBlocks_[*hoveredIndex]; + const auto tooltip = formatScheduleTooltip(block); + if (tooltip.isEmpty()) { + QWidget::mouseMoveEvent(event); + return; + } + + const auto hoveredId = block.id.empty() ? block.connectionId : block.id; + if (hoveredId != hoveredConnectionBlockId_) { + hoveredConnectionBlockId_ = hoveredId; + hoveredEnvironmentHazardId_.clear(); hoveredRouteGuidanceId_.clear(); - hoveredConnectionBlockId_.clear(); - QToolTip::hideText(); + QToolTip::showText(event->globalPosition().toPoint(), tooltip, this); } QWidget::mouseMoveEvent(event); return; } - const auto& block = connectionBlocks_[*hoveredIndex]; - const auto tooltip = formatScheduleTooltip(block); - if (tooltip.isEmpty()) { + if (hoveredHazard.has_value()) { + const auto& hazard = environmentHazards_[*hoveredHazard]; + const auto tooltip = formatEnvironmentHazardTooltip(hazard); + const auto hoveredId = hazard.id.empty() + ? QString("%1:%2:%3") + .arg(hazardKindLabel(hazard.kind)) + .arg(hazard.position.x, 0, 'f', 3) + .arg(hazard.position.y, 0, 'f', 3) + .toStdString() + : hazard.id; + if (hoveredId != hoveredEnvironmentHazardId_) { + hoveredEnvironmentHazardId_ = hoveredId; + hoveredConnectionBlockId_.clear(); + hoveredRouteGuidanceId_.clear(); + QToolTip::showText(event->globalPosition().toPoint(), tooltip, this); + } QWidget::mouseMoveEvent(event); return; } - const auto hoveredId = block.id.empty() ? block.connectionId : block.id; - if (hoveredId != hoveredConnectionBlockId_) { - hoveredConnectionBlockId_ = hoveredId; + if (!hoveredConnectionBlockId_.empty() + || !hoveredEnvironmentHazardId_.empty() + || !hoveredRouteGuidanceId_.empty()) { + hoveredConnectionBlockId_.clear(); + hoveredEnvironmentHazardId_.clear(); hoveredRouteGuidanceId_.clear(); - QToolTip::showText(event->globalPosition().toPoint(), tooltip, this); + QToolTip::hideText(); } QWidget::mouseMoveEvent(event); } @@ -639,8 +795,6 @@ void SimulationCanvasWidget::paintEvent(QPaintEvent* event) { painter.drawPixmap(0, 0, layoutCache_); const auto transform = currentTransform(*bounds); - drawConnectionBlockOverlay(painter, transform); - drawRouteGuidanceOverlay(painter, transform); if (overlayMode_ == ResultOverlayMode::Density) { drawDensityOverlay(painter, transform); } else if (overlayMode_ == ResultOverlayMode::Pressure) { @@ -650,6 +804,9 @@ void SimulationCanvasWidget::paintEvent(QPaintEvent* event) { } else if (overlayMode_ == ResultOverlayMode::Bottlenecks) { drawBottleneckOverlay(painter, transform); } + drawEnvironmentHazardOverlay(painter, transform); + drawConnectionBlockOverlay(painter, transform); + drawRouteGuidanceOverlay(painter, transform); for (const auto& agent : frame_.agents) { if (!matchesFloor(agent.floorId, currentFloorId_)) { continue; @@ -791,6 +948,79 @@ void SimulationCanvasWidget::drawConnectionBlockOverlay(QPainter& painter, const painter.restore(); } +void SimulationCanvasWidget::drawEnvironmentHazardOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const { + if (environmentHazards_.empty()) { + return; + } + + const auto elapsedSeconds = std::max(0.0, frame_.elapsedSeconds); + + painter.save(); + painter.setRenderHint(QPainter::Antialiasing, true); + painter.setCompositionMode(QPainter::CompositionMode_SourceOver); + + for (const auto& hazard : environmentHazards_) { + if (!hazardIsActive(hazard, elapsedSeconds)) { + continue; + } + if (!matchesFloor(hazardFloorId(layout_, hazard), currentFloorId_)) { + continue; + } + + const auto center = transform.map(hazard.position); + const auto radiusMeters = hazardRadiusMeters(hazard.severity); + const auto radiusAnchor = transform.map({ + .x = hazard.position.x + radiusMeters, + .y = hazard.position.y, + }); + const auto radius = std::max( + 18.0, + std::hypot(radiusAnchor.x() - center.x(), radiusAnchor.y() - center.y())); + + const auto isFire = hazard.kind == safecrowd::domain::EnvironmentHazardKind::Fire; + const QColor core = isFire ? QColor(220, 38, 38, 110) : QColor(71, 85, 105, 92); + const QColor mid = isFire ? QColor(249, 115, 22, 46) : QColor(148, 163, 184, 40); + const QColor edge = isFire ? QColor(249, 115, 22, 0) : QColor(148, 163, 184, 0); + QRadialGradient gradient(center, radius); + gradient.setColorAt(0.0, core); + gradient.setColorAt(0.48, mid); + gradient.setColorAt(1.0, edge); + painter.setPen(Qt::NoPen); + painter.setBrush(gradient); + painter.drawEllipse(center, radius, radius); + + painter.setBrush(Qt::NoBrush); + painter.setPen(QPen( + isFire ? QColor(185, 28, 28, 180) : QColor(71, 85, 105, 165), + 1.8, + Qt::DashLine, + Qt::RoundCap, + Qt::RoundJoin)); + painter.drawEllipse(center, radius, radius); + + const QColor fill = isFire ? QColor("#c2410c") : QColor("#64748b"); + painter.setPen(Qt::NoPen); + painter.setBrush(fill); + painter.drawEllipse(center, 11.0, 11.0); + + painter.setPen(QPen(Qt::white, 2.0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(Qt::NoBrush); + if (isFire) { + QPainterPath flame; + flame.moveTo(center + QPointF(0.0, 6.0)); + flame.cubicTo(center + QPointF(-5.0, 2.0), center + QPointF(-3.5, -4.0), center + QPointF(-0.5, -7.0)); + flame.cubicTo(center + QPointF(4.0, -3.0), center + QPointF(4.0, 3.0), center + QPointF(0.0, 6.0)); + painter.drawPath(flame); + } else { + painter.drawArc(QRectF(center.x() - 7.0, center.y() - 1.0, 9.0, 7.0), 20 * 16, 220 * 16); + painter.drawArc(QRectF(center.x() - 1.0, center.y() - 3.0, 10.0, 7.0), 20 * 16, 220 * 16); + painter.drawArc(QRectF(center.x() - 5.0, center.y() - 8.0, 8.0, 6.0), 20 * 16, 220 * 16); + } + } + + painter.restore(); +} + void SimulationCanvasWidget::drawRouteGuidanceOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const { const auto elapsedSeconds = std::max(0.0, frame_.elapsedSeconds); const auto active = activeRouteGuidanceSelection(routeGuidances_, elapsedSeconds); diff --git a/src/application/SimulationCanvasWidget.h b/src/application/SimulationCanvasWidget.h index 8c0a2e2..4a9c7a9 100644 --- a/src/application/SimulationCanvasWidget.h +++ b/src/application/SimulationCanvasWidget.h @@ -42,6 +42,7 @@ class SimulationCanvasWidget : public QWidget { void setFrame(safecrowd::domain::SimulationFrame frame); void setConnectionBlocks(std::vector blocks); + void setEnvironmentHazards(std::vector hazards); void setRouteGuidances(std::vector guidances); void setDensityOverlay( std::vector densityCells, @@ -75,6 +76,7 @@ class SimulationCanvasWidget : public QWidget { QRectF previewViewport() const; void focusWorldPoint(const safecrowd::domain::Point2D& point, double zoom); void drawConnectionBlockOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; + void drawEnvironmentHazardOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawRouteGuidanceOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawDensityOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawPressureOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; @@ -88,6 +90,7 @@ class SimulationCanvasWidget : public QWidget { safecrowd::domain::FacilityLayout2D layout_{}; safecrowd::domain::SimulationFrame frame_{}; std::vector connectionBlocks_{}; + std::vector environmentHazards_{}; std::vector routeGuidances_{}; std::vector densityOverlay_{}; double densityScaleMaxPeoplePerSquareMeter_{4.0}; @@ -112,6 +115,7 @@ class SimulationCanvasWidget : public QWidget { bool layoutCacheValid_{false}; std::string hoveredConnectionBlockId_{}; + std::string hoveredEnvironmentHazardId_{}; std::string hoveredRouteGuidanceId_{}; }; diff --git a/src/domain/ScenarioResultArtifacts.h b/src/domain/ScenarioResultArtifacts.h index 0e9aa32..bb181c0 100644 --- a/src/domain/ScenarioResultArtifacts.h +++ b/src/domain/ScenarioResultArtifacts.h @@ -6,6 +6,7 @@ #include #include "domain/Geometry2D.h" +#include "domain/ScenarioAuthoring.h" #include "domain/ScenarioRiskMetrics.h" #include "domain/ScenarioSimulationFrame.h" @@ -92,6 +93,26 @@ struct PressureSummary { std::vector criticalEvents{}; }; +struct HazardExposureMetric { + std::string hazardId{}; + std::string hazardName{}; + EnvironmentHazardKind kind{EnvironmentHazardKind::Fire}; + ScenarioElementSeverity severity{ScenarioElementSeverity::Medium}; + std::string affectedZoneId{}; + std::string floorId{}; + Point2D position{}; + double exposedAgentSeconds{0.0}; + std::size_t peakExposedAgentCount{0}; + std::optional firstExposureSeconds{}; + std::optional peakAtSeconds{}; + double exposureScore{0.0}; +}; + +struct HazardExposureSummary { + double totalExposureScore{0.0}; + std::vector hazards{}; +}; + struct ExitUsageMetric { std::string exitZoneId{}; std::string exitLabel{}; @@ -125,6 +146,7 @@ struct ScenarioResultArtifacts { EvacuationTimingSummary timingSummary{}; DensitySummary densitySummary{}; PressureSummary pressureSummary{}; + HazardExposureSummary hazardExposureSummary{}; std::vector exitUsage{}; std::vector zoneCompletion{}; std::vector placementCompletion{}; diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 419a90a..8358942 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -63,12 +63,19 @@ 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; + const auto* activeHazards = resources.contains() + ? &resources.get() + : nullptr; applyRouteGuidance(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed); advanceRoutesForCurrentZones(query, entities, layoutCache); 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); const auto localNeighborIndex = buildAgentSpatialIndex(query, entities, 1.0); const auto* pressureFeedback = resources.contains() @@ -211,7 +218,12 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } } const auto adjustedMaxSpeed = maxSpeed * pressureSpeedFactor; - const auto desiredVelocity = routeDirection * adjustedMaxSpeed; + const auto* hazardState = activeHazardState(reactions, entity.index); + const auto hazardSpeedFactor = hazardState == nullptr + ? 1.0 + : std::clamp(hazardState->hazardSpeedFactor, 0.35, 1.0); + const auto adjustedHazardMaxSpeed = adjustedMaxSpeed * hazardSpeedFactor; + const auto desiredVelocity = routeDirection * adjustedHazardMaxSpeed; double speedScale = 1.0; const auto neighborRadius = std::max( static_cast(agent.radius) + kDefaultAgentRadius + kPersonalSpaceBuffer, @@ -229,28 +241,32 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { speedScale) * pressureAvoidanceScale; const auto barrierReferenceSpeed = std::max( - adjustedMaxSpeed, + adjustedHazardMaxSpeed, (static_cast(agent.maxSpeed) * 0.75) * pressureSpeedFactor); const auto barrierVelocity = barrierSeparationVelocity(floorLayout, position, agent, barrierReferenceSpeed) * pressureBarrierScale; - auto finalVelocity = (desiredVelocity * speedScale) + avoidanceVelocity + barrierVelocity; + const auto hazardAvoidanceVelocity = + hazardState == nullptr + ? Point2D{} + : hazardAvoidanceVelocityFor(*hazardState, position.value, routeDirection, adjustedHazardMaxSpeed); + auto finalVelocity = (desiredVelocity * speedScale) + avoidanceVelocity + barrierVelocity + hazardAvoidanceVelocity; const auto lateral = perpendicularLeft(routeDirection); if (dot(finalVelocity, routeDirection) < 0.0) { - finalVelocity = (routeDirection * (adjustedMaxSpeed * 0.15)) + finalVelocity = (routeDirection * (adjustedHazardMaxSpeed * 0.15)) + (lateral * dot(finalVelocity, lateral)); } const auto forwardComponent = dot(finalVelocity, routeDirection); const auto lateralComponent = dot(finalVelocity, lateral); - const auto maxLateralComponent = adjustedMaxSpeed * 0.55; + const auto maxLateralComponent = adjustedHazardMaxSpeed * 0.75; if (std::fabs(lateralComponent) > maxLateralComponent) { finalVelocity = (routeDirection * std::max(0.0, forwardComponent)) + (lateral * std::clamp(lateralComponent, -maxLateralComponent, maxLateralComponent)); } - finalVelocity = velocityWithBarrierEscape(finalVelocity, barrierVelocity, adjustedMaxSpeed); + finalVelocity = velocityWithBarrierEscape(finalVelocity, barrierVelocity, adjustedHazardMaxSpeed); plans.push_back({ .entity = entity, - .velocity = clampedToLength(finalVelocity, adjustedMaxSpeed), + .velocity = clampedToLength(finalVelocity, adjustedHazardMaxSpeed), }); } @@ -314,6 +330,43 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { bool advanced{false}; }; + static const ScenarioEnvironmentReactionAgentState* activeHazardState( + 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.hazardAware + || !it->second.hazardInRange) { + return nullptr; + } + return &it->second; + } + + static bool sameFloor(const std::string& lhs, const std::string& rhs) { + return lhs == rhs || lhs.empty() || rhs.empty(); + } + + static Point2D hazardAvoidanceVelocityFor( + const ScenarioEnvironmentReactionAgentState& state, + const Point2D& position, + const Point2D& routeDirection, + double maxSpeed) { + if (state.hazardRadiusMeters <= 1e-9 || maxSpeed <= 1e-9) { + return {}; + } + + const auto away = normalizedOr(position - state.hazardPosition, perpendicularLeft(routeDirection)); + const auto proximity = std::clamp( + 1.0 - (std::max(0.0, state.hazardDistanceMeters) / state.hazardRadiusMeters), + 0.0, + 1.0); + const auto kindScale = state.hazardKind == EnvironmentHazardKind::Fire ? 0.85 : 0.65; + return away * (maxSpeed * kindScale * (0.35 + (0.65 * proximity))); + } + const Connection2D* findConnectionById( const ScenarioLayoutCacheResource& layoutCache, const std::string& connectionId) const { @@ -1094,6 +1147,90 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return plan; } + double hazardRoutePenalty( + const ScenarioLayoutCacheResource& layoutCache, + const ZoneRouteToExit& route, + const std::string& agentFloorId, + const ScenarioActiveEnvironmentHazardsResource& activeHazards) const { + double penalty = 0.0; + for (const auto& hazard : activeHazards.hazards) { + if (!sameFloor(agentFloorId, hazard.floorId)) { + continue; + } + + bool routeTouchesHazard = false; + for (const auto& zoneId : route.zoneIds) { + const auto* zone = findCachedZone(layoutCache, zoneId); + if (zone == nullptr || !sameFloor(zone->floorId, hazard.floorId)) { + continue; + } + if (zoneId == hazard.draft.affectedZoneId + || distanceBetween(polygonCenter(zone->area), hazard.draft.position) <= hazard.radiusMeters + 1e-9) { + routeTouchesHazard = true; + break; + } + } + + if (!routeTouchesHazard) { + for (const auto connectionIndex : route.connectionIndices) { + if (connectionIndex >= layoutCache.layout.connections.size()) { + continue; + } + const auto& connection = layoutCache.layout.connections[connectionIndex]; + const auto connectionFloorId = connection.floorId.empty() + ? cachedFloorIdForZone(layoutCache, connection.fromZoneId) + : connection.floorId; + if (sameFloor(connectionFloorId, hazard.floorId) + && distanceBetween(midpoint(connection.centerSpan), hazard.draft.position) <= hazard.radiusMeters + 1e-9) { + routeTouchesHazard = true; + break; + } + } + } + + if (routeTouchesHazard) { + penalty += hazard.routePenaltyMeters; + } + } + return penalty; + } + + RoutePlan routePlanToHazardAwareNearestExit( + const ScenarioLayoutCacheResource& layoutCache, + const Point2D& start, + const std::string& startZoneId, + const std::string& agentFloorId, + const ScenarioActiveEnvironmentHazardsResource& activeHazards) const { + std::string bestExitZoneId; + double bestScore = std::numeric_limits::max(); + double bestDistance = std::numeric_limits::max(); + + for (const auto& zone : layoutCache.layout.zones) { + if (zone.kind != ZoneKind::Exit) { + continue; + } + + const auto result = zoneRouteToExit(layoutCache, start, startZoneId, zone.id); + if (!result.has_value() || result->route.empty()) { + continue; + } + + const auto penalty = hazardRoutePenalty(layoutCache, result->route, agentFloorId, activeHazards); + const auto score = result->distance + penalty; + if (score + 1e-9 < bestScore + || (std::fabs(score - bestScore) <= 1e-9 && result->distance < bestDistance)) { + bestScore = score; + bestDistance = result->distance; + bestExitZoneId = zone.id; + } + } + + if (bestExitZoneId.empty()) { + return {}; + } + return routePlanToExit(layoutCache, start, startZoneId, bestExitZoneId); + } + void replaceRouteWithPlan(EvacuationRoute& route, const RoutePlan& plan, const Point2D& start) const { route.destinationZoneId = plan.destinationZoneId; route.waypoints = plan.waypoints; @@ -1425,6 +1562,57 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } } + void replanHazardAwareExitRoutes( + engine::WorldQuery& query, + const std::vector& entities, + const ScenarioLayoutCacheResource& layoutCache, + double elapsedSeconds, + const ScenarioEnvironmentReactionResource* reactions, + const ScenarioActiveEnvironmentHazardsResource* activeHazards) const { + if (reactions == nullptr || activeHazards == nullptr || activeHazards->hazards.empty()) { + return; + } + + for (const auto entity : entities) { + const auto* hazardState = activeHazardState(reactions, entity.index); + if (hazardState == nullptr) { + continue; + } + + const auto& status = query.get(entity); + if (status.evacuated) { + continue; + } + + const auto& position = query.get(entity); + auto& route = query.get(entity); + if (elapsedSeconds + 1e-9 < route.nextExitReplanSeconds) { + continue; + } + + const auto startZoneId = zoneAt(layoutCache, position.value, route.currentFloorId); + if (startZoneId.empty()) { + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + continue; + } + + const auto agentFloorId = !route.displayFloorId.empty() ? route.displayFloorId : route.currentFloorId; + const auto plan = + routePlanToHazardAwareNearestExit(layoutCache, position.value, startZoneId, agentFloorId, *activeHazards); + if (plan.destinationZoneId.empty()) { + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + continue; + } + if (plan.destinationZoneId == route.destinationZoneId && !route.waypoints.empty() && !route.noExitAvailable) { + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + continue; + } + + replaceRouteWithPlan(route, plan, position.value); + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + } + } + void replanBlockedExitRoutes( engine::WorldQuery& query, const std::vector& entities, diff --git a/src/domain/ScenarioSimulationRunner.cpp b/src/domain/ScenarioSimulationRunner.cpp index 46ef861..615b454 100644 --- a/src/domain/ScenarioSimulationRunner.cpp +++ b/src/domain/ScenarioSimulationRunner.cpp @@ -200,6 +200,11 @@ void ScenarioSimulationRunner::initializeRuntime() { std::make_unique(1.0), {.phase = engine::UpdatePhase::PreSimulation, .triggerPolicy = engine::TriggerPolicy::EveryFrame}); + runtime_->addSystem( + makeScenarioEnvironmentHazardSystem(layout_, scenario_.environment.hazards), + {.phase = engine::UpdatePhase::PostSimulation, + .order = -20, + .triggerPolicy = engine::TriggerPolicy::EveryFrame}); runtime_->addSystem( makeScenarioPressureFeedbackSystem(layout_), {.phase = engine::UpdatePhase::PostSimulation, diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 2024ffd..9959f7d 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -258,6 +258,124 @@ bool connectionShouldBeBlocked(const ConnectionBlockDraft& block, double timeSec }); } +std::string hazardRuntimeKey(const EnvironmentHazardDraft& hazard, std::size_t index) { + if (!hazard.id.empty()) { + return hazard.id; + } + return "hazard-" + std::to_string(index + 1); +} + +double severityRadiusMeters(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 2.0; + case ScenarioElementSeverity::High: + return 5.0; + case ScenarioElementSeverity::Medium: + default: + return 3.5; + } +} + +double severityRoutePenaltyMeters(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 25.0; + case ScenarioElementSeverity::High: + return 150.0; + case ScenarioElementSeverity::Medium: + default: + return 75.0; + } +} + +double severityWeight(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 1.0; + case ScenarioElementSeverity::High: + return 3.0; + case ScenarioElementSeverity::Medium: + default: + return 2.0; + } +} + +double hazardSpeedFactor(EnvironmentHazardKind kind, ScenarioElementSeverity severity) { + if (kind == EnvironmentHazardKind::Smoke) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 0.85; + case ScenarioElementSeverity::High: + return 0.55; + case ScenarioElementSeverity::Medium: + default: + return 0.70; + } + } + + switch (severity) { + case ScenarioElementSeverity::Low: + return 0.90; + case ScenarioElementSeverity::High: + return 0.60; + case ScenarioElementSeverity::Medium: + default: + return 0.75; + } +} + +bool hazardActiveAt(const EnvironmentHazardDraft& hazard, double elapsedSeconds) { + const auto start = std::max(0.0, hazard.startSeconds); + if (elapsedSeconds + 1e-9 < start) { + return false; + } + if (hazard.endSeconds <= hazard.startSeconds) { + return true; + } + return elapsedSeconds <= std::max(start, hazard.endSeconds) + 1e-9; +} + +std::string hazardFloorId( + const FacilityLayout2D& layout, + const EnvironmentHazardDraft& hazard) { + if (!hazard.floorId.empty()) { + return hazard.floorId; + } + return zoneFloorId(layout, hazard.affectedZoneId); +} + +HazardExposureMetric hazardExposureMetric( + const EnvironmentHazardDraft& hazard, + const std::string& key, + const std::string& floorId) { + return { + .hazardId = key, + .hazardName = hazard.name, + .kind = hazard.kind, + .severity = hazard.severity, + .affectedZoneId = hazard.affectedZoneId, + .floorId = floorId, + .position = hazard.position, + }; +} + +ScenarioActiveEnvironmentHazard activeHazardFromDraft( + const EnvironmentHazardDraft& hazard, + const std::string& key, + const std::string& floorId) { + const auto factor = std::max(0.35, hazardSpeedFactor(hazard.kind, hazard.severity)); + return { + .key = key, + .draft = hazard, + .floorId = floorId, + .radiusMeters = severityRadiusMeters(hazard.severity), + .speedFactor = factor, + .routePenaltyMeters = severityRoutePenaltyMeters(hazard.severity), + .severityWeight = severityWeight(hazard.severity), + }; +} + Connection2D* findConnectionById(FacilityLayout2D& layout, const std::string& connectionId) { const auto it = std::find_if(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { return connection.id == connectionId; @@ -352,6 +470,181 @@ class ScenarioControlSystem final : public engine::EngineSystem { std::uint64_t revision_{0}; }; +class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { +public: + ScenarioEnvironmentHazardSystem(FacilityLayout2D layout, std::vector hazards) + : layout_(std::move(layout)), + hazards_(std::move(hazards)) { + } + + void configure(engine::EngineWorld& world) override { + world.resources().set(ScenarioEnvironmentReactionResource{}); + world.resources().set(ScenarioActiveEnvironmentHazardsResource{}); + + ScenarioHazardExposureResource exposure; + exposure.hazardsById.reserve(hazards_.size()); + for (std::size_t index = 0; index < hazards_.size(); ++index) { + const auto key = hazardRuntimeKey(hazards_[index], index); + exposure.hazardsById.emplace(key, hazardExposureMetric(hazards_[index], key, hazardFloorId(layout_, hazards_[index]))); + } + world.resources().set(std::move(exposure)); + } + + void update(engine::EngineWorld& world, const engine::EngineStepContext& step) override { + (void)step; + + auto& resources = world.resources(); + if (!resources.contains() + || !resources.contains() + || !resources.contains()) { + configure(world); + } + + double elapsedSeconds = 0.0; + if (resources.contains()) { + elapsedSeconds = resources.get().elapsedSeconds; + } + const auto deltaSeconds = resources.contains() + ? std::max(0.0, resources.get().deltaSeconds) + : 0.0; + + auto activeHazards = buildActiveHazards(elapsedSeconds); + auto& activeResource = resources.get(); + activeResource.hazards = activeHazards; + + auto& reactions = resources.get(); + auto& exposure = resources.get(); + std::unordered_map currentExposureCounts; + currentExposureCounts.reserve(activeHazards.size()); + + auto& query = world.query(); + const auto entities = query.view(); + for (const auto entity : entities) { + const auto& status = query.get(entity); + if (status.evacuated) { + continue; + } + + const auto& position = query.get(entity); + const auto& agent = query.get(entity); + const auto& route = query.get(entity); + const auto agentFloorId = !route.displayFloorId.empty() ? route.displayFloorId : route.currentFloorId; + + const ScenarioActiveEnvironmentHazard* detectedHazard = nullptr; + double detectedDistance = 0.0; + double detectedRadius = 0.0; + double bestProximity = std::numeric_limits::max(); + + for (const auto& hazard : activeHazards) { + if (!sameFloor(agentFloorId, hazard.floorId)) { + continue; + } + + const auto sensitivity = hazard.draft.kind == EnvironmentHazardKind::Smoke + ? agent.smokeSensitivity + : agent.hazardSensitivity; + const auto radius = std::max(0.0, hazard.radiusMeters * std::max(0.0, sensitivity)); + if (radius <= 1e-9) { + continue; + } + + const auto distance = distanceBetween(position.value, hazard.draft.position); + if (distance > radius + 1e-9) { + continue; + } + + ++currentExposureCounts[hazard.key]; + const auto proximity = distance / radius; + if (proximity < bestProximity) { + bestProximity = proximity; + detectedHazard = &hazard; + detectedDistance = distance; + detectedRadius = radius; + } + } + + auto& state = reactions.agentsById[entity.index]; + if (detectedHazard == nullptr) { + state.hazardInRange = false; + continue; + } + + if (!state.hazardDetected || state.hazardKey != detectedHazard->key) { + state.hazardDetected = true; + state.hazardAware = false; + state.hazardKey = detectedHazard->key; + state.hazardDetectedAtSeconds = elapsedSeconds; + state.hazardReactionReadySeconds = + elapsedSeconds + std::max(0.0, agent.reactionDelaySeconds); + } + + state.hazardInRange = true; + state.hazardAware = elapsedSeconds + 1e-9 >= state.hazardReactionReadySeconds; + state.hazardKind = detectedHazard->draft.kind; + state.hazardSeverity = detectedHazard->draft.severity; + state.hazardPosition = detectedHazard->draft.position; + state.hazardFloorId = detectedHazard->floorId; + state.hazardAffectedZoneId = detectedHazard->draft.affectedZoneId; + state.hazardDistanceMeters = detectedDistance; + state.hazardRadiusMeters = detectedRadius; + state.hazardSpeedFactor = detectedHazard->speedFactor; + state.hazardRoutePenaltyMeters = detectedHazard->routePenaltyMeters; + } + + if (deltaSeconds <= 0.0) { + return; + } + + for (const auto& hazard : activeHazards) { + const auto currentCountIt = currentExposureCounts.find(hazard.key); + const auto currentCount = currentCountIt == currentExposureCounts.end() + ? std::size_t{0} + : currentCountIt->second; + if (currentCount == 0) { + continue; + } + + auto& metric = exposure.hazardsById[hazard.key]; + if (metric.hazardId.empty()) { + metric = hazardExposureMetric(hazard.draft, hazard.key, hazard.floorId); + } + metric.exposedAgentSeconds += static_cast(currentCount) * deltaSeconds; + metric.exposureScore += static_cast(currentCount) * deltaSeconds * hazard.severityWeight; + if (!metric.firstExposureSeconds.has_value()) { + metric.firstExposureSeconds = elapsedSeconds; + } + if (currentCount > metric.peakExposedAgentCount) { + metric.peakExposedAgentCount = currentCount; + metric.peakAtSeconds = elapsedSeconds; + } + } + } + +private: + std::vector buildActiveHazards(double elapsedSeconds) const { + std::vector active; + active.reserve(hazards_.size()); + for (std::size_t index = 0; index < hazards_.size(); ++index) { + const auto& hazard = hazards_[index]; + if (!hazardActiveAt(hazard, elapsedSeconds)) { + continue; + } + active.push_back(activeHazardFromDraft( + hazard, + hazardRuntimeKey(hazard, index), + hazardFloorId(layout_, hazard))); + } + return active; + } + + static bool sameFloor(const std::string& lhs, const std::string& rhs) { + return lhs == rhs || lhs.empty() || rhs.empty(); + } + + FacilityLayout2D layout_{}; + std::vector hazards_{}; +}; + } // namespace ScenarioAgentSpawnSystem::ScenarioAgentSpawnSystem(std::vector seeds, double timeLimitSeconds) @@ -956,6 +1249,28 @@ void ScenarioResultArtifactsSystem::update(engine::EngineWorld& world, const eng pressureSummary.criticalEvents.resize(kMaxResultCriticalPressureEvents); } + result.artifacts.hazardExposureSummary = {}; + if (resources.contains()) { + const auto& hazardExposure = resources.get(); + result.artifacts.hazardExposureSummary.hazards.reserve(hazardExposure.hazardsById.size()); + for (const auto& [_, metric] : hazardExposure.hazardsById) { + result.artifacts.hazardExposureSummary.totalExposureScore += metric.exposureScore; + result.artifacts.hazardExposureSummary.hazards.push_back(metric); + } + std::sort( + result.artifacts.hazardExposureSummary.hazards.begin(), + result.artifacts.hazardExposureSummary.hazards.end(), + [](const auto& lhs, const auto& rhs) { + if (std::fabs(lhs.exposureScore - rhs.exposureScore) > 1e-9) { + return lhs.exposureScore > rhs.exposureScore; + } + if (lhs.peakExposedAgentCount != rhs.peakExposedAgentCount) { + return lhs.peakExposedAgentCount > rhs.peakExposedAgentCount; + } + return lhs.hazardId < rhs.hazardId; + }); + } + const auto shouldRecordSample = result.artifacts.evacuationProgress.empty() || evacuatedCount != result.lastRecordedEvacuatedCount @@ -985,4 +1300,10 @@ std::unique_ptr makeScenarioControlSystem( return std::make_unique(std::move(baseLayout), std::move(blocks)); } +std::unique_ptr makeScenarioEnvironmentHazardSystem( + FacilityLayout2D layout, + std::vector hazards) { + return std::make_unique(std::move(layout), std::move(hazards)); +} + } // namespace safecrowd::domain diff --git a/src/domain/ScenarioSimulationSystems.h b/src/domain/ScenarioSimulationSystems.h index 9d17847..9a5c43c 100644 --- a/src/domain/ScenarioSimulationSystems.h +++ b/src/domain/ScenarioSimulationSystems.h @@ -82,7 +82,17 @@ struct ScenarioPressureFeedbackResource { struct ScenarioEnvironmentReactionAgentState { bool hazardDetected{false}; bool hazardAware{false}; + bool hazardInRange{false}; std::string hazardKey{}; + EnvironmentHazardKind hazardKind{EnvironmentHazardKind::Fire}; + ScenarioElementSeverity hazardSeverity{ScenarioElementSeverity::Medium}; + Point2D hazardPosition{}; + std::string hazardFloorId{}; + std::string hazardAffectedZoneId{}; + double hazardDistanceMeters{0.0}; + double hazardRadiusMeters{0.0}; + double hazardSpeedFactor{1.0}; + double hazardRoutePenaltyMeters{0.0}; double hazardDetectedAtSeconds{0.0}; double hazardReactionReadySeconds{0.0}; bool closureDetected{false}; @@ -96,6 +106,24 @@ struct ScenarioEnvironmentReactionResource { std::unordered_map agentsById{}; }; +struct ScenarioActiveEnvironmentHazard { + std::string key{}; + EnvironmentHazardDraft draft{}; + std::string floorId{}; + double radiusMeters{0.0}; + double speedFactor{1.0}; + double routePenaltyMeters{0.0}; + double severityWeight{1.0}; +}; + +struct ScenarioActiveEnvironmentHazardsResource { + std::vector hazards{}; +}; + +struct ScenarioHazardExposureResource { + std::unordered_map hazardsById{}; +}; + struct ScenarioResultArtifactsResource { ScenarioResultArtifacts artifacts{}; std::size_t lastRecordedEvacuatedCount{static_cast(-1)}; @@ -138,6 +166,9 @@ std::unique_ptr makeScenarioSimulationMotionSystem(); std::unique_ptr makeScenarioControlSystem( FacilityLayout2D baseLayout, std::vector blocks); +std::unique_ptr makeScenarioEnvironmentHazardSystem( + FacilityLayout2D layout, + std::vector hazards); std::unique_ptr makeScenarioSimulationMotionSystem(FacilityLayout2D layout); std::unique_ptr makeScenarioSimulationMotionSystem( FacilityLayout2D layout, diff --git a/tests/ScenarioSimulationRunnerTests.cpp b/tests/ScenarioSimulationRunnerTests.cpp index 778e3d9..8b2af3b 100644 --- a/tests/ScenarioSimulationRunnerTests.cpp +++ b/tests/ScenarioSimulationRunnerTests.cpp @@ -1815,3 +1815,54 @@ SC_TEST(ScenarioSimulationRunnerNoHazardNoBlockBaselineStillEvacuates) { SC_EXPECT_EQ(runner.frame().totalAgentCount, static_cast(4)); SC_EXPECT_EQ(runner.frame().evacuatedAgentCount, static_cast(4)); } + +SC_TEST(ScenarioSimulationRunnerPublishesFireSmokeExposureSummary) { + safecrowd::domain::InitialPlacement2D placement; + placement.id = "hazard-exposed-agent"; + placement.zoneId = "room"; + placement.targetAgentCount = 1; + placement.initialVelocity = {.x = 1.0, .y = 0.0}; + placement.area.outline = {{.x = 1.0, .y = 2.0}}; + + safecrowd::domain::EnvironmentHazardDraft fire; + fire.id = "fire-a"; + fire.kind = safecrowd::domain::EnvironmentHazardKind::Fire; + fire.name = "Fire A"; + fire.affectedZoneId = "room"; + fire.position = {.x = 1.0, .y = 2.25}; + fire.severity = safecrowd::domain::ScenarioElementSeverity::Medium; + + safecrowd::domain::EnvironmentHazardDraft smoke; + smoke.id = "smoke-a"; + smoke.kind = safecrowd::domain::EnvironmentHazardKind::Smoke; + smoke.name = "Smoke A"; + smoke.affectedZoneId = "room"; + smoke.position = {.x = 1.0, .y = 1.75}; + smoke.severity = safecrowd::domain::ScenarioElementSeverity::High; + + safecrowd::domain::ScenarioDraft scenario; + scenario.execution.timeLimitSeconds = 5.0; + scenario.population.initialPlacements.push_back(placement); + scenario.environment.hazards.push_back(fire); + scenario.environment.hazards.push_back(smoke); + + safecrowd::domain::ScenarioSimulationRunner runner(wideDoorLayout(), scenario); + runner.step(0.5); + + const auto& summary = runner.resultArtifacts().hazardExposureSummary; + SC_EXPECT_EQ(summary.hazards.size(), std::size_t{2}); + SC_EXPECT_TRUE(summary.totalExposureScore > 0.0); + + const auto fireIt = std::find_if(summary.hazards.begin(), summary.hazards.end(), [](const auto& metric) { + return metric.hazardId == "fire-a"; + }); + const auto smokeIt = std::find_if(summary.hazards.begin(), summary.hazards.end(), [](const auto& metric) { + return metric.hazardId == "smoke-a"; + }); + SC_EXPECT_TRUE(fireIt != summary.hazards.end()); + SC_EXPECT_TRUE(smokeIt != summary.hazards.end()); + SC_EXPECT_TRUE(fireIt->exposedAgentSeconds > 0.0); + SC_EXPECT_TRUE(smokeIt->exposedAgentSeconds > 0.0); + SC_EXPECT_EQ(fireIt->peakExposedAgentCount, std::size_t{1}); + SC_EXPECT_EQ(smokeIt->peakExposedAgentCount, std::size_t{1}); +} diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index f3ba86a..9612dab 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -357,6 +357,72 @@ safecrowd::domain::FacilityLayout2D twoExitGuidanceDetourLayout() { return layout; } +safecrowd::domain::ScenarioAgentSeed straightRouteSeed( + safecrowd::domain::Point2D start, + double maxSpeed = 1.0, + double reactionDelaySeconds = 0.0, + std::string destinationZoneId = "exit") { + return { + .position = {.value = start}, + .agent = { + .radius = 0.25f, + .maxSpeed = static_cast(maxSpeed), + .reactionDelaySeconds = reactionDelaySeconds, + }, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 1.0, .y = 0.0}}, + .waypointPassages = {{{.x = 1.0, .y = -0.4}, {.x = 1.0, .y = 0.4}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {destinationZoneId}, + .nextWaypointIndex = 0, + .currentSegmentStart = start, + .previousDistanceToWaypoint = 1.0, + .destinationZoneId = destinationZoneId, + }, + .status = {}, + }; +} + +safecrowd::domain::EnvironmentHazardDraft hazardDraft( + std::string id, + safecrowd::domain::EnvironmentHazardKind kind, + safecrowd::domain::ScenarioElementSeverity severity, + safecrowd::domain::Point2D position, + std::string affectedZoneId = "room", + std::string floorId = {}) { + return { + .id = std::move(id), + .kind = kind, + .name = "Test hazard", + .affectedZoneId = std::move(affectedZoneId), + .floorId = std::move(floorId), + .position = position, + .startSeconds = 0.0, + .endSeconds = 0.0, + .severity = severity, + }; +} + +void addHazardMotionSystems( + safecrowd::engine::EngineRuntime& runtime, + const safecrowd::domain::FacilityLayout2D& layout, + std::vector hazards) { + 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), + {.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}); +} + safecrowd::domain::FacilityLayout2D overlappingFloorBottleneckLayout() { safecrowd::domain::FacilityLayout2D layout; layout.floors.push_back({.id = "L1", .label = "Floor 1"}); @@ -827,6 +893,216 @@ SC_TEST(ScenarioSimulationMotionSystem_AdvancesAgentsFromStepResource) { SC_EXPECT_TRUE(!frame.agents.front().stalled); } +SC_TEST(ScenarioEnvironmentHazardSystem_DelaysFireAvoidanceUntilReactionReady) { + std::vector seeds; + seeds.push_back(straightRouteSeed({.x = 0.0, .y = 0.0}, 1.0, 0.5)); + + auto fire = hazardDraft( + "fire-a", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::High, + {.x = 0.0, .y = 0.4}); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 43, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + addHazardMotionSystems(runtime, straightExitLayout(), {fire}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.25}); + runtime.stepFrame(0.0); + + const auto& reactions = + runtime.world().resources().get(); + const auto& firstState = reactions.agentsById.at(0); + SC_EXPECT_TRUE(firstState.hazardDetected); + SC_EXPECT_TRUE(!firstState.hazardAware); + + const auto firstFrame = + runtime.world().resources().get().frame; + SC_EXPECT_NEAR(firstFrame.agents.front().position.y, 0.0, 1e-6); + + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.25}); + runtime.stepFrame(0.0); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.25}); + runtime.stepFrame(0.0); + + const auto& awareState = + runtime.world().resources().get().agentsById.at(0); + const auto& awareFrame = + runtime.world().resources().get().frame; + SC_EXPECT_TRUE(awareState.hazardAware); + SC_EXPECT_TRUE(awareFrame.agents.front().position.y < -0.01); +} + +SC_TEST(ScenarioEnvironmentHazardSystem_SmokeSlowsButDoesNotStopAgent) { + std::vector baselineSeeds; + baselineSeeds.push_back(straightRouteSeed({.x = 0.0, .y = 0.0}, 1.0)); + + safecrowd::engine::EngineRuntime baselineRuntime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 44, + }); + baselineRuntime.addSystem( + std::make_unique(std::move(baselineSeeds), 10.0)); + baselineRuntime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem(straightExitLayout()), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + baselineRuntime.addSystem( + std::make_unique(), + {.phase = safecrowd::engine::UpdatePhase::RenderSync, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + baselineRuntime.play(); + baselineRuntime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.5}); + baselineRuntime.stepFrame(0.0); + const auto baselineVelocity = + baselineRuntime.world().resources().get().frame.agents.front().velocity; + const auto baselineSpeed = std::hypot(baselineVelocity.x, baselineVelocity.y); + + std::vector smokeSeeds; + smokeSeeds.push_back(straightRouteSeed({.x = 0.0, .y = 0.0}, 1.0)); + auto smoke = hazardDraft( + "smoke-a", + safecrowd::domain::EnvironmentHazardKind::Smoke, + safecrowd::domain::ScenarioElementSeverity::High, + {.x = 0.0, .y = 0.0}); + + safecrowd::engine::EngineRuntime smokeRuntime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 45, + }); + smokeRuntime.addSystem(std::make_unique(std::move(smokeSeeds), 10.0)); + addHazardMotionSystems(smokeRuntime, straightExitLayout(), {smoke}); + + smokeRuntime.play(); + smokeRuntime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.5}); + smokeRuntime.stepFrame(0.0); + + const auto smokeVelocity = + smokeRuntime.world().resources().get().frame.agents.front().velocity; + const auto smokeSpeed = std::hypot(smokeVelocity.x, smokeVelocity.y); + SC_EXPECT_TRUE(smokeSpeed < baselineSpeed); + SC_EXPECT_TRUE(smokeSpeed > 0.1); +} + +SC_TEST(ScenarioEnvironmentHazardSystem_ReroutesOnlyAfterHazardAwareness) { + safecrowd::domain::ScenarioAgentSeed seed; + seed.position = {.value = {.x = 1.2, .y = 0.5}}; + seed.agent = {.radius = 0.25f, .maxSpeed = 1.0f, .reactionDelaySeconds = 0.2}; + seed.velocity = {}; + seed.route = { + .waypoints = {{.x = 2.0, .y = 0.5}, {.x = 3.0, .y = 0.5}}, + .waypointPassages = { + {{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}, + {{.x = 3.0, .y = 0.5}, {.x = 3.0, .y = 0.5}}, + }, + .waypointFromZoneIds = {"room", ""}, + .waypointZoneIds = {"near-exit", "near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 1.2, .y = 0.5}, + .previousDistanceToWaypoint = 0.8, + .destinationZoneId = "near-exit", + }; + + auto fire = hazardDraft( + "near-exit-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::Low, + {.x = 3.0, .y = 0.5}, + "near-exit"); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 46, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addHazardMotionSystems(runtime, twoExitGuidanceDetourLayout(), {fire}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + + auto entities = runtime.world().query().view(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"near-exit"}); + + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + 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_IgnoresInactiveAndDifferentFloorHazards) { + auto layout = overlappingFloorBottleneckLayout(); + auto seed = straightRouteSeed({.x = 0.25, .y = 0.0}, 1.0, 0.0, "exit-l1"); + seed.route.waypoints = {{.x = 1.0, .y = 0.0}}; + seed.route.waypointZoneIds = {"exit-l1"}; + seed.route.destinationZoneId = "exit-l1"; + seed.route.currentFloorId = "L1"; + seed.route.displayFloorId = "L1"; + + auto inactive = hazardDraft( + "inactive-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::High, + {.x = 0.25, .y = 0.0}, + "room-l1", + "L1"); + inactive.startSeconds = 5.0; + inactive.endSeconds = 10.0; + auto otherFloor = hazardDraft( + "other-floor-smoke", + safecrowd::domain::EnvironmentHazardKind::Smoke, + safecrowd::domain::ScenarioElementSeverity::High, + {.x = 0.25, .y = 0.0}, + "room-l2", + "L2"); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 47, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addHazardMotionSystems(runtime, layout, {inactive, otherFloor}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.5}); + runtime.stepFrame(0.0); + + const auto& frame = + runtime.world().resources().get().frame; + SC_EXPECT_TRUE(frame.agents.front().velocity.x > 0.9); + SC_EXPECT_NEAR(frame.agents.front().velocity.y, 0.0, 1e-6); + + const auto& reactions = + runtime.world().resources().get(); + const auto reactionIt = reactions.agentsById.find(0); + SC_EXPECT_TRUE(reactionIt == reactions.agentsById.end() || !reactionIt->second.hazardInRange); + + const auto& exposure = + runtime.world().resources().get(); + for (const auto& [_, metric] : exposure.hazardsById) { + SC_EXPECT_NEAR(metric.exposedAgentSeconds, 0.0, 1e-9); + SC_EXPECT_NEAR(metric.exposureScore, 0.0, 1e-9); + } +} + SC_TEST(ScenarioPressureFeedbackSystem_PublishesSoftFeedbackForDenseCluster) { safecrowd::engine::EngineRuntime runtime({ .fixedDeltaTime = 1.0 / 30.0, From aeec4ee00fc0925f6d79273d442bc1817a019fb0 Mon Sep 17 00:00:00 2001 From: Silversupplier Date: Wed, 13 May 2026 01:42:43 +0900 Subject: [PATCH 2/2] [Domain] Refine fire smoke reaction behavior --- src/application/ScenarioAuthoringWidget.cpp | 45 ++++- src/application/ScenarioCanvasWidget.cpp | 31 ++-- src/application/SimulationCanvasWidget.cpp | 51 +----- src/domain/ScenarioAuthoring.cpp | 166 ++++++++++++++++++ src/domain/ScenarioAuthoring.h | 22 +++ src/domain/ScenarioSimulationMotionSystem.cpp | 66 ++++--- src/domain/ScenarioSimulationSystems.cpp | 112 +++--------- tests/ScenarioAuthoringTests.cpp | 46 +++++ tests/ScenarioSimulationSystemsTests.cpp | 133 +++++++++++++- 9 files changed, 484 insertions(+), 188 deletions(-) diff --git a/src/application/ScenarioAuthoringWidget.cpp b/src/application/ScenarioAuthoringWidget.cpp index 4b56798..268e71f 100644 --- a/src/application/ScenarioAuthoringWidget.cpp +++ b/src/application/ScenarioAuthoringWidget.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -172,7 +173,11 @@ QString severityLabel(safecrowd::domain::ScenarioElementSeverity severity) { } QString hazardScheduleSummary(const safecrowd::domain::EnvironmentHazardDraft& hazard) { - return QString("%1s - %2s").arg(hazard.startSeconds, 0, 'f', 1).arg(hazard.endSeconds, 0, 'f', 1); + const auto start = std::max(0.0, hazard.startSeconds); + if (safecrowd::domain::environmentHazardHasOpenEndedSchedule(hazard)) { + return QString("%1s - open").arg(start, 0, 'f', 1); + } + return QString("%1s - %2s").arg(start, 0, 'f', 1).arg(std::max(start, hazard.endSeconds), 0, 'f', 1); } QString hazardZoneSummary( @@ -245,13 +250,35 @@ bool editEnvironmentHazard( startSpin->setRange(0.0, 86400.0); startSpin->setDecimals(1); startSpin->setSuffix(" s"); - startSpin->setValue(std::max(0.0, hazard->startSeconds)); + const auto initialStartSeconds = std::max(0.0, hazard->startSeconds); + const auto initialOpenEnded = safecrowd::domain::environmentHazardHasOpenEndedSchedule(*hazard); + startSpin->setValue(initialStartSeconds); auto* endSpin = new QDoubleSpinBox(&dialog); endSpin->setRange(0.0, 86400.0); endSpin->setDecimals(1); endSpin->setSuffix(" s"); - endSpin->setValue(std::max(0.0, hazard->endSeconds)); + endSpin->setValue(initialOpenEnded + ? initialStartSeconds + : std::max(initialStartSeconds, hazard->endSeconds)); + + auto* openEndedCheck = new QCheckBox("Open ended", &dialog); + openEndedCheck->setChecked(initialOpenEnded); + endSpin->setEnabled(!initialOpenEnded); + + QObject::connect(openEndedCheck, &QCheckBox::toggled, &dialog, [=](bool checked) { + endSpin->setEnabled(!checked); + if (checked) { + endSpin->setValue(startSpin->value()); + } else if (endSpin->value() <= startSpin->value()) { + endSpin->setValue(std::min(86400.0, startSpin->value() + 60.0)); + } + }); + QObject::connect(startSpin, qOverload(&QDoubleSpinBox::valueChanged), &dialog, [=](double value) { + if (openEndedCheck->isChecked()) { + endSpin->setValue(value); + } + }); auto* severityCombo = new QComboBox(&dialog); severityCombo->addItem("Low", static_cast(safecrowd::domain::ScenarioElementSeverity::Low)); @@ -270,6 +297,7 @@ bool editEnvironmentHazard( form->addRow("Y", ySpin); form->addRow("Start", startSpin); form->addRow("End", endSpin); + form->addRow("", openEndedCheck); form->addRow("Severity", severityCombo); form->addRow("Note", noteEdit); root->addLayout(form); @@ -301,6 +329,11 @@ bool editEnvironmentHazard( xSpin->setFocus(); return; } + if (!openEndedCheck->isChecked() && endSpin->value() <= startSpin->value()) { + QMessageBox::warning(&dialog, "Edit hazard", "Set the end time after the start time."); + endSpin->setFocus(); + return; + } dialog.accept(); }); @@ -332,6 +365,10 @@ bool editEnvironmentHazard( QMessageBox::warning(parent, "Edit hazard", "The hazard location must stay inside the affected zone."); return false; } + if (!openEndedCheck->isChecked() && endSpin->value() <= startSpin->value()) { + QMessageBox::warning(parent, "Edit hazard", "Set the end time after the start time."); + return false; + } hazard->kind = static_cast(kindCombo->currentData().toInt()); hazard->name = name.toStdString(); @@ -339,7 +376,7 @@ bool editEnvironmentHazard( hazard->floorId = selectedZone->floorId; hazard->position = selectedPosition; hazard->startSeconds = startSpin->value(); - hazard->endSeconds = std::max(hazard->startSeconds, endSpin->value()); + hazard->endSeconds = openEndedCheck->isChecked() ? hazard->startSeconds : endSpin->value(); hazard->severity = static_cast(severityCombo->currentData().toInt()); hazard->note = noteEdit->toPlainText().trimmed().toStdString(); return true; diff --git a/src/application/ScenarioCanvasWidget.cpp b/src/application/ScenarioCanvasWidget.cpp index 870affa..63f3021 100644 --- a/src/application/ScenarioCanvasWidget.cpp +++ b/src/application/ScenarioCanvasWidget.cpp @@ -215,7 +215,14 @@ QString formatEnvironmentHazardTooltip(const safecrowd::domain::EnvironmentHazar if (!hazard.name.empty()) { text.append(QString("\n%1").arg(QString::fromStdString(hazard.name))); } - text.append(QString("\nActive: %1s ~ %2s").arg(hazard.startSeconds, 0, 'f', 1).arg(hazard.endSeconds, 0, 'f', 1)); + const auto start = std::max(0.0, hazard.startSeconds); + if (safecrowd::domain::environmentHazardHasOpenEndedSchedule(hazard)) { + text.append(QString("\nActive: %1s ~ open").arg(start, 0, 'f', 1)); + } else { + text.append(QString("\nActive: %1s ~ %2s") + .arg(start, 0, 'f', 1) + .arg(std::max(start, hazard.endSeconds), 0, 'f', 1)); + } text.append(QString("\nSeverity: %1").arg(severityLabel(hazard.severity))); return text; } @@ -232,16 +239,7 @@ std::optional hoveredEnvironmentHazardIndex( double closestDistanceSq = kHoverRadiusPixels * kHoverRadiusPixels; for (std::size_t index = 0; index < hazards.size(); ++index) { const auto& hazard = hazards[index]; - auto floorId = hazard.floorId; - if (floorId.empty() && !hazard.affectedZoneId.empty()) { - const auto zoneIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { - return zone.id == hazard.affectedZoneId; - }); - if (zoneIt != layout.zones.end()) { - floorId = zoneIt->floorId; - } - } - if (!matchesFloor(floorId, currentFloorId)) { + if (!matchesFloor(safecrowd::domain::environmentHazardFloorId(layout, hazard), currentFloorId)) { continue; } @@ -2045,16 +2043,7 @@ void ScenarioCanvasWidget::drawConnectionBlocks(QPainter& painter, const LayoutC void ScenarioCanvasWidget::drawEnvironmentHazards(QPainter& painter, const LayoutCanvasTransform& transform) const { for (const auto& hazard : environmentHazards_) { - auto floorId = hazard.floorId; - if (floorId.empty() && !hazard.affectedZoneId.empty()) { - const auto zoneIt = std::find_if(layout_.zones.begin(), layout_.zones.end(), [&](const auto& zone) { - return zone.id == hazard.affectedZoneId; - }); - if (zoneIt != layout_.zones.end()) { - floorId = zoneIt->floorId; - } - } - if (!matchesFloor(floorId, currentFloorId_)) { + if (!matchesFloor(safecrowd::domain::environmentHazardFloorId(layout_, hazard), currentFloorId_)) { continue; } diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index 94d2fbd..add9679 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -72,45 +72,6 @@ bool connectionShouldBeBlocked(const safecrowd::domain::ConnectionBlockDraft& bl return false; } -bool hazardIsActive(const safecrowd::domain::EnvironmentHazardDraft& hazard, double timeSeconds) { - const auto start = std::max(0.0, hazard.startSeconds); - if (timeSeconds + 1e-9 < start) { - return false; - } - if (hazard.endSeconds <= hazard.startSeconds) { - return true; - } - const auto end = std::max(start, hazard.endSeconds); - return timeSeconds <= end + 1e-9; -} - -double hazardRadiusMeters(safecrowd::domain::ScenarioElementSeverity severity) { - switch (severity) { - case safecrowd::domain::ScenarioElementSeverity::Low: - return 2.0; - case safecrowd::domain::ScenarioElementSeverity::High: - return 5.0; - case safecrowd::domain::ScenarioElementSeverity::Medium: - default: - return 3.5; - } -} - -std::string hazardFloorId( - const safecrowd::domain::FacilityLayout2D& layout, - const safecrowd::domain::EnvironmentHazardDraft& hazard) { - if (!hazard.floorId.empty()) { - return hazard.floorId; - } - if (hazard.affectedZoneId.empty()) { - return {}; - } - const auto zoneIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { - return zone.id == hazard.affectedZoneId; - }); - return zoneIt == layout.zones.end() ? std::string{} : zoneIt->floorId; -} - QString hazardKindLabel(safecrowd::domain::EnvironmentHazardKind kind) { switch (kind) { case safecrowd::domain::EnvironmentHazardKind::Smoke: @@ -139,7 +100,7 @@ QString formatEnvironmentHazardTooltip(const safecrowd::domain::EnvironmentHazar text.append(QString("\n%1").arg(QString::fromStdString(hazard.name))); } const auto start = std::max(0.0, hazard.startSeconds); - if (hazard.endSeconds <= hazard.startSeconds) { + if (safecrowd::domain::environmentHazardHasOpenEndedSchedule(hazard)) { text.append(QString("\nActive: %1s ~ open").arg(start, 0, 'f', 1)); } else { text.append(QString("\nActive: %1s ~ %2s") @@ -295,10 +256,10 @@ std::optional hoveredActiveEnvironmentHazardIndex( double closestDistanceSq = kHoverRadiusPixels * kHoverRadiusPixels; for (std::size_t index = 0; index < hazards.size(); ++index) { const auto& hazard = hazards[index]; - if (!hazardIsActive(hazard, elapsedSeconds)) { + if (!safecrowd::domain::environmentHazardActiveAt(hazard, elapsedSeconds)) { continue; } - if (!matchesFloor(hazardFloorId(layout, hazard), currentFloorId)) { + if (!matchesFloor(safecrowd::domain::environmentHazardFloorId(layout, hazard), currentFloorId)) { continue; } @@ -960,15 +921,15 @@ void SimulationCanvasWidget::drawEnvironmentHazardOverlay(QPainter& painter, con painter.setCompositionMode(QPainter::CompositionMode_SourceOver); for (const auto& hazard : environmentHazards_) { - if (!hazardIsActive(hazard, elapsedSeconds)) { + if (!safecrowd::domain::environmentHazardActiveAt(hazard, elapsedSeconds)) { continue; } - if (!matchesFloor(hazardFloorId(layout_, hazard), currentFloorId_)) { + if (!matchesFloor(safecrowd::domain::environmentHazardFloorId(layout_, hazard), currentFloorId_)) { continue; } const auto center = transform.map(hazard.position); - const auto radiusMeters = hazardRadiusMeters(hazard.severity); + const auto radiusMeters = safecrowd::domain::environmentHazardRuntimeProfile(hazard).radiusMeters; const auto radiusAnchor = transform.map({ .x = hazard.position.x + radiusMeters, .y = hazard.position.y, diff --git a/src/domain/ScenarioAuthoring.cpp b/src/domain/ScenarioAuthoring.cpp index 35b3b20..16479c4 100644 --- a/src/domain/ScenarioAuthoring.cpp +++ b/src/domain/ScenarioAuthoring.cpp @@ -1,5 +1,7 @@ #include "domain/ScenarioAuthoring.h" +#include +#include #include namespace safecrowd::domain { @@ -213,4 +215,168 @@ std::vector computeScenarioDiffKeys(const ScenarioDraft& baseline, return keys; } +double environmentHazardRadiusMeters(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 2.0; + case ScenarioElementSeverity::High: + return 5.0; + case ScenarioElementSeverity::Medium: + default: + return 3.5; + } +} + +double environmentHazardRoutePenaltyMeters(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 25.0; + case ScenarioElementSeverity::High: + return 150.0; + case ScenarioElementSeverity::Medium: + default: + return 75.0; + } +} + +double environmentHazardSeverityWeight(ScenarioElementSeverity severity) { + switch (severity) { + case ScenarioElementSeverity::Low: + return 1.0; + case ScenarioElementSeverity::High: + return 3.0; + case ScenarioElementSeverity::Medium: + default: + return 2.0; + } +} + +double environmentHazardSpeedFactor(EnvironmentHazardKind kind, ScenarioElementSeverity severity) { + if (kind == EnvironmentHazardKind::Smoke) { + EnvironmentHazardDraft hazard; + hazard.kind = EnvironmentHazardKind::Smoke; + hazard.severity = severity; + return environmentHazardSpeedFactorAt(hazard, 0.0, 1.5); + } + + switch (severity) { + case ScenarioElementSeverity::Low: + return 0.90; + case ScenarioElementSeverity::High: + return 0.60; + case ScenarioElementSeverity::Medium: + default: + return 0.75; + } +} + +double environmentHazardSmokeVisibilityMetersAt(const EnvironmentHazardDraft& hazard, double distanceMeters) { + if (hazard.kind != EnvironmentHazardKind::Smoke) { + return std::numeric_limits::infinity(); + } + + const auto radius = environmentHazardRadiusMeters(hazard.severity); + if (radius <= 1e-9) { + return std::numeric_limits::infinity(); + } + + double sourceVisibility = 1.5; + switch (hazard.severity) { + case ScenarioElementSeverity::Low: + sourceVisibility = 2.5; + break; + case ScenarioElementSeverity::High: + sourceVisibility = 0.5; + break; + case ScenarioElementSeverity::Medium: + default: + sourceVisibility = 1.5; + break; + } + + const auto distance = std::max(0.0, distanceMeters); + if (distance >= radius) { + return 3.0; + } + + const auto t = std::clamp(distance / radius, 0.0, 1.0); + return sourceVisibility + ((3.0 - sourceVisibility) * t); +} + +double environmentHazardSmokeSpeedMetersPerSecond(double smokeFreeSpeedMetersPerSecond, double visibilityMeters) { + const auto smokeFreeSpeed = std::max(0.0, smokeFreeSpeedMetersPerSecond); + if (smokeFreeSpeed <= 1e-9) { + return 0.0; + } + + if (visibilityMeters >= 3.0) { + return smokeFreeSpeed; + } + + // Pathfinder applies the Fridolf et al. visibility relation and floors smoke walking speed at 0.2 m/s. + const auto smokeSpeed = std::max(0.2, smokeFreeSpeed - (0.34 * (3.0 - std::max(0.0, visibilityMeters)))); + return std::min(smokeFreeSpeed, smokeSpeed); +} + +double environmentHazardSpeedFactorAt( + const EnvironmentHazardDraft& hazard, + double distanceMeters, + double smokeFreeSpeedMetersPerSecond) { + const auto smokeFreeSpeed = std::max(0.0, smokeFreeSpeedMetersPerSecond); + if (smokeFreeSpeed <= 1e-9) { + return 1.0; + } + + const auto radius = environmentHazardRadiusMeters(hazard.severity); + if (radius <= 1e-9 || distanceMeters >= radius) { + return 1.0; + } + + if (hazard.kind == EnvironmentHazardKind::Smoke) { + const auto visibility = environmentHazardSmokeVisibilityMetersAt(hazard, distanceMeters); + return environmentHazardSmokeSpeedMetersPerSecond(smokeFreeSpeed, visibility) / smokeFreeSpeed; + } + + 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); +} + +EnvironmentHazardRuntimeProfile environmentHazardRuntimeProfile(const EnvironmentHazardDraft& hazard) { + return { + .radiusMeters = environmentHazardRadiusMeters(hazard.severity), + .speedFactor = std::max(0.35, environmentHazardSpeedFactorAt(hazard, 0.0, 1.5)), + .routePenaltyMeters = environmentHazardRoutePenaltyMeters(hazard.severity), + .severityWeight = environmentHazardSeverityWeight(hazard.severity), + }; +} + +bool environmentHazardHasOpenEndedSchedule(const EnvironmentHazardDraft& hazard) { + return hazard.endSeconds <= hazard.startSeconds; +} + +bool environmentHazardActiveAt(const EnvironmentHazardDraft& hazard, double elapsedSeconds) { + const auto start = std::max(0.0, hazard.startSeconds); + if (elapsedSeconds + 1e-9 < start) { + return false; + } + if (environmentHazardHasOpenEndedSchedule(hazard)) { + return true; + } + return elapsedSeconds <= std::max(start, hazard.endSeconds) + 1e-9; +} + +std::string environmentHazardFloorId(const FacilityLayout2D& layout, const EnvironmentHazardDraft& hazard) { + if (!hazard.floorId.empty()) { + return hazard.floorId; + } + if (hazard.affectedZoneId.empty()) { + return {}; + } + const auto it = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { + return zone.id == hazard.affectedZoneId; + }); + return it == layout.zones.end() ? std::string{} : it->floorId; +} + } // namespace safecrowd::domain diff --git a/src/domain/ScenarioAuthoring.h b/src/domain/ScenarioAuthoring.h index ec83716..3b5300f 100644 --- a/src/domain/ScenarioAuthoring.h +++ b/src/domain/ScenarioAuthoring.h @@ -39,6 +39,13 @@ struct EnvironmentHazardDraft { std::string note{}; }; +struct EnvironmentHazardRuntimeProfile { + double radiusMeters{0.0}; + double speedFactor{1.0}; + double routePenaltyMeters{0.0}; + double severityWeight{1.0}; +}; + struct EnvironmentState { bool reducedVisibility{false}; std::string familiarityProfile{}; @@ -122,4 +129,19 @@ ScenarioDraft duplicateScenarioDraft(const ScenarioDraft& source, std::vector computeScenarioDiffKeys(const ScenarioDraft& baseline, const ScenarioDraft& variant); +double environmentHazardRadiusMeters(ScenarioElementSeverity severity); +double environmentHazardRoutePenaltyMeters(ScenarioElementSeverity severity); +double environmentHazardSeverityWeight(ScenarioElementSeverity severity); +double environmentHazardSpeedFactor(EnvironmentHazardKind kind, ScenarioElementSeverity severity); +double environmentHazardSmokeVisibilityMetersAt(const EnvironmentHazardDraft& hazard, double distanceMeters); +double environmentHazardSmokeSpeedMetersPerSecond(double smokeFreeSpeedMetersPerSecond, double visibilityMeters); +double environmentHazardSpeedFactorAt( + const EnvironmentHazardDraft& hazard, + double distanceMeters, + double smokeFreeSpeedMetersPerSecond); +EnvironmentHazardRuntimeProfile environmentHazardRuntimeProfile(const EnvironmentHazardDraft& hazard); +bool environmentHazardHasOpenEndedSchedule(const EnvironmentHazardDraft& hazard); +bool environmentHazardActiveAt(const EnvironmentHazardDraft& hazard, double elapsedSeconds); +std::string environmentHazardFloorId(const FacilityLayout2D& layout, const EnvironmentHazardDraft& hazard); + } // namespace safecrowd::domain diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 8358942..edad42f 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -221,7 +221,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto* hazardState = activeHazardState(reactions, entity.index); const auto hazardSpeedFactor = hazardState == nullptr ? 1.0 - : std::clamp(hazardState->hazardSpeedFactor, 0.35, 1.0); + : std::clamp(hazardState->hazardSpeedFactor, 0.0, 1.0); const auto adjustedHazardMaxSpeed = adjustedMaxSpeed * hazardSpeedFactor; const auto desiredVelocity = routeDirection * adjustedHazardMaxSpeed; double speedScale = 1.0; @@ -1149,43 +1149,56 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { double hazardRoutePenalty( const ScenarioLayoutCacheResource& layoutCache, - const ZoneRouteToExit& route, - const std::string& agentFloorId, + const RoutePlan& plan, + const Point2D& start, + const std::string& startFloorId, const ScenarioActiveEnvironmentHazardsResource& activeHazards) const { double penalty = 0.0; for (const auto& hazard : activeHazards.hazards) { - if (!sameFloor(agentFloorId, hazard.floorId)) { - continue; - } - bool routeTouchesHazard = false; - for (const auto& zoneId : route.zoneIds) { - const auto* zone = findCachedZone(layoutCache, zoneId); - if (zone == nullptr || !sameFloor(zone->floorId, hazard.floorId)) { - continue; - } - if (zoneId == hazard.draft.affectedZoneId - || distanceBetween(polygonCenter(zone->area), hazard.draft.position) <= hazard.radiusMeters + 1e-9) { + + 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 (sameFloor(segmentFloorId, hazard.floorId) + && distanceBetween( + hazard.draft.position, + closestPointOnSegment(hazard.draft.position, segmentStart, segmentEnd)) <= hazard.radiusMeters + 1e-9) { routeTouchesHazard = true; break; } - } - if (!routeTouchesHazard) { - for (const auto connectionIndex : route.connectionIndices) { - if (connectionIndex >= layoutCache.layout.connections.size()) { + 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; } - const auto& connection = layoutCache.layout.connections[connectionIndex]; - const auto connectionFloorId = connection.floorId.empty() - ? cachedFloorIdForZone(layoutCache, connection.fromZoneId) - : connection.floorId; + const auto connectionFloorId = connection->floorId.empty() + ? cachedFloorIdForZone(layoutCache, connection->fromZoneId) + : connection->floorId; if (sameFloor(connectionFloorId, hazard.floorId) - && distanceBetween(midpoint(connection.centerSpan), hazard.draft.position) <= hazard.radiusMeters + 1e-9) { + && distanceBetween( + hazard.draft.position, + closestPointOnSegment( + hazard.draft.position, + connection->centerSpan.start, + connection->centerSpan.end)) <= hazard.radiusMeters + 1e-9) { routeTouchesHazard = true; break; } } + + segmentStart = segmentEnd; + if (waypointIndex < plan.waypointFloorIds.size() + && !plan.waypointFloorIds[waypointIndex].empty()) { + segmentFloorId = plan.waypointFloorIds[waypointIndex]; + } } if (routeTouchesHazard) { @@ -1215,7 +1228,12 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { continue; } - const auto penalty = hazardRoutePenalty(layoutCache, result->route, agentFloorId, activeHazards); + 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)) { diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 9959f7d..0a86ef3 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -265,86 +265,6 @@ std::string hazardRuntimeKey(const EnvironmentHazardDraft& hazard, std::size_t i return "hazard-" + std::to_string(index + 1); } -double severityRadiusMeters(ScenarioElementSeverity severity) { - switch (severity) { - case ScenarioElementSeverity::Low: - return 2.0; - case ScenarioElementSeverity::High: - return 5.0; - case ScenarioElementSeverity::Medium: - default: - return 3.5; - } -} - -double severityRoutePenaltyMeters(ScenarioElementSeverity severity) { - switch (severity) { - case ScenarioElementSeverity::Low: - return 25.0; - case ScenarioElementSeverity::High: - return 150.0; - case ScenarioElementSeverity::Medium: - default: - return 75.0; - } -} - -double severityWeight(ScenarioElementSeverity severity) { - switch (severity) { - case ScenarioElementSeverity::Low: - return 1.0; - case ScenarioElementSeverity::High: - return 3.0; - case ScenarioElementSeverity::Medium: - default: - return 2.0; - } -} - -double hazardSpeedFactor(EnvironmentHazardKind kind, ScenarioElementSeverity severity) { - if (kind == EnvironmentHazardKind::Smoke) { - switch (severity) { - case ScenarioElementSeverity::Low: - return 0.85; - case ScenarioElementSeverity::High: - return 0.55; - case ScenarioElementSeverity::Medium: - default: - return 0.70; - } - } - - switch (severity) { - case ScenarioElementSeverity::Low: - return 0.90; - case ScenarioElementSeverity::High: - return 0.60; - case ScenarioElementSeverity::Medium: - default: - return 0.75; - } -} - -bool hazardActiveAt(const EnvironmentHazardDraft& hazard, double elapsedSeconds) { - const auto start = std::max(0.0, hazard.startSeconds); - if (elapsedSeconds + 1e-9 < start) { - return false; - } - if (hazard.endSeconds <= hazard.startSeconds) { - return true; - } - return elapsedSeconds <= std::max(start, hazard.endSeconds) + 1e-9; -} - -std::string hazardFloorId( - const FacilityLayout2D& layout, - const EnvironmentHazardDraft& hazard) { - if (!hazard.floorId.empty()) { - return hazard.floorId; - } - return zoneFloorId(layout, hazard.affectedZoneId); -} - HazardExposureMetric hazardExposureMetric( const EnvironmentHazardDraft& hazard, const std::string& key, @@ -364,15 +284,15 @@ ScenarioActiveEnvironmentHazard activeHazardFromDraft( const EnvironmentHazardDraft& hazard, const std::string& key, const std::string& floorId) { - const auto factor = std::max(0.35, hazardSpeedFactor(hazard.kind, hazard.severity)); + const auto profile = environmentHazardRuntimeProfile(hazard); return { .key = key, .draft = hazard, .floorId = floorId, - .radiusMeters = severityRadiusMeters(hazard.severity), - .speedFactor = factor, - .routePenaltyMeters = severityRoutePenaltyMeters(hazard.severity), - .severityWeight = severityWeight(hazard.severity), + .radiusMeters = profile.radiusMeters, + .speedFactor = profile.speedFactor, + .routePenaltyMeters = profile.routePenaltyMeters, + .severityWeight = profile.severityWeight, }; } @@ -485,7 +405,9 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { exposure.hazardsById.reserve(hazards_.size()); for (std::size_t index = 0; index < hazards_.size(); ++index) { const auto key = hazardRuntimeKey(hazards_[index], index); - exposure.hazardsById.emplace(key, hazardExposureMetric(hazards_[index], key, hazardFloorId(layout_, hazards_[index]))); + exposure.hazardsById.emplace( + key, + hazardExposureMetric(hazards_[index], key, environmentHazardFloorId(layout_, hazards_[index]))); } world.resources().set(std::move(exposure)); } @@ -540,6 +462,11 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { continue; } + const auto distance = distanceBetween(position.value, hazard.draft.position); + if (distance <= hazard.radiusMeters + 1e-9) { + ++currentExposureCounts[hazard.key]; + } + const auto sensitivity = hazard.draft.kind == EnvironmentHazardKind::Smoke ? agent.smokeSensitivity : agent.hazardSensitivity; @@ -548,12 +475,10 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { continue; } - const auto distance = distanceBetween(position.value, hazard.draft.position); if (distance > radius + 1e-9) { continue; } - ++currentExposureCounts[hazard.key]; const auto proximity = distance / radius; if (proximity < bestProximity) { bestProximity = proximity; @@ -586,8 +511,11 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { state.hazardFloorId = detectedHazard->floorId; state.hazardAffectedZoneId = detectedHazard->draft.affectedZoneId; state.hazardDistanceMeters = detectedDistance; - state.hazardRadiusMeters = detectedRadius; - state.hazardSpeedFactor = detectedHazard->speedFactor; + state.hazardRadiusMeters = detectedHazard->radiusMeters; + state.hazardSpeedFactor = environmentHazardSpeedFactorAt( + detectedHazard->draft, + detectedDistance, + static_cast(agent.maxSpeed)); state.hazardRoutePenaltyMeters = detectedHazard->routePenaltyMeters; } @@ -626,13 +554,13 @@ class ScenarioEnvironmentHazardSystem final : public engine::EngineSystem { active.reserve(hazards_.size()); for (std::size_t index = 0; index < hazards_.size(); ++index) { const auto& hazard = hazards_[index]; - if (!hazardActiveAt(hazard, elapsedSeconds)) { + if (!environmentHazardActiveAt(hazard, elapsedSeconds)) { continue; } active.push_back(activeHazardFromDraft( hazard, hazardRuntimeKey(hazard, index), - hazardFloorId(layout_, hazard))); + environmentHazardFloorId(layout_, hazard))); } return active; } diff --git a/tests/ScenarioAuthoringTests.cpp b/tests/ScenarioAuthoringTests.cpp index 04039db..eb394aa 100644 --- a/tests/ScenarioAuthoringTests.cpp +++ b/tests/ScenarioAuthoringTests.cpp @@ -90,6 +90,52 @@ SC_TEST(duplicateScenarioDraft_doesNotMutateSource) { SC_EXPECT_NEAR(baseline.execution.timeLimitSeconds, 600.0, 1e-9); } +SC_TEST(environmentHazardRuntimeProfile_UsesSharedSeverityAndScheduleRules) { + auto hazard = makeSmokeHazard(); + hazard.startSeconds = 10.0; + hazard.endSeconds = 10.0; + + const auto profile = environmentHazardRuntimeProfile(hazard); + + SC_EXPECT_NEAR(profile.radiusMeters, 5.0, 1e-9); + SC_EXPECT_NEAR(profile.speedFactor, 0.65 / 1.5, 1e-9); + SC_EXPECT_NEAR(profile.routePenaltyMeters, 150.0, 1e-9); + SC_EXPECT_NEAR(profile.severityWeight, 3.0, 1e-9); + SC_EXPECT_TRUE(environmentHazardHasOpenEndedSchedule(hazard)); + SC_EXPECT_TRUE(!environmentHazardActiveAt(hazard, 9.9)); + SC_EXPECT_TRUE(environmentHazardActiveAt(hazard, 10.0)); + SC_EXPECT_TRUE(environmentHazardActiveAt(hazard, 120.0)); + + hazard.endSeconds = 20.0; + SC_EXPECT_TRUE(!environmentHazardHasOpenEndedSchedule(hazard)); +} + +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, 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); +} + +SC_TEST(environmentHazardFloorId_FallsBackToAffectedZoneFloor) { + FacilityLayout2D layout; + layout.zones.push_back({ + .id = "zone-a", + .floorId = "L2", + }); + auto hazard = makeSmokeHazard(); + hazard.floorId.clear(); + + SC_EXPECT_EQ(environmentHazardFloorId(layout, hazard), std::string{"L2"}); + + hazard.floorId = "Manual"; + SC_EXPECT_EQ(environmentHazardFloorId(layout, hazard), std::string{"Manual"}); +} + 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 9612dab..fcd4e2b 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -357,6 +357,45 @@ safecrowd::domain::FacilityLayout2D twoExitGuidanceDetourLayout() { return layout; } +safecrowd::domain::FacilityLayout2D wideTwoExitHazardRouteLayout() { + safecrowd::domain::FacilityLayout2D layout; + layout.zones.push_back({ + .id = "room", + .kind = safecrowd::domain::ZoneKind::Room, + .label = "Room", + .area = {.outline = {{0.0, 0.0}, {10.0, 0.0}, {10.0, 10.0}, {0.0, 10.0}}}, + }); + layout.zones.push_back({ + .id = "near-exit", + .kind = safecrowd::domain::ZoneKind::Exit, + .label = "Near Exit", + .area = {.outline = {{10.0, 0.0}, {12.0, 0.0}, {12.0, 2.0}, {10.0, 2.0}}}, + }); + layout.zones.push_back({ + .id = "far-exit", + .kind = safecrowd::domain::ZoneKind::Exit, + .label = "Far Exit", + .area = {.outline = {{10.0, 8.0}, {12.0, 8.0}, {12.0, 10.0}, {10.0, 10.0}}}, + }); + layout.connections.push_back({ + .id = "room-near-exit", + .kind = safecrowd::domain::ConnectionKind::Exit, + .fromZoneId = "room", + .toZoneId = "near-exit", + .effectiveWidth = 0.8, + .centerSpan = {{10.0, 0.7}, {10.0, 1.3}}, + }); + layout.connections.push_back({ + .id = "room-far-exit", + .kind = safecrowd::domain::ConnectionKind::Exit, + .fromZoneId = "room", + .toZoneId = "far-exit", + .effectiveWidth = 0.8, + .centerSpan = {{10.0, 8.7}, {10.0, 9.3}}, + }); + return layout; +} + safecrowd::domain::ScenarioAgentSeed straightRouteSeed( safecrowd::domain::Point2D start, double maxSpeed = 1.0, @@ -988,14 +1027,17 @@ SC_TEST(ScenarioEnvironmentHazardSystem_SmokeSlowsButDoesNotStopAgent) { const auto smokeVelocity = smokeRuntime.world().resources().get().frame.agents.front().velocity; const auto smokeSpeed = std::hypot(smokeVelocity.x, smokeVelocity.y); + const auto& smokeReaction = + smokeRuntime.world().resources().get().agentsById.at(0); SC_EXPECT_TRUE(smokeSpeed < baselineSpeed); SC_EXPECT_TRUE(smokeSpeed > 0.1); + SC_EXPECT_NEAR(smokeReaction.hazardSpeedFactor, 0.2, 1e-9); } SC_TEST(ScenarioEnvironmentHazardSystem_ReroutesOnlyAfterHazardAwareness) { safecrowd::domain::ScenarioAgentSeed seed; seed.position = {.value = {.x = 1.2, .y = 0.5}}; - seed.agent = {.radius = 0.25f, .maxSpeed = 1.0f, .reactionDelaySeconds = 0.2}; + seed.agent = {.radius = 0.25f, .maxSpeed = 1.0f, .hazardSensitivity = 2.0, .reactionDelaySeconds = 0.2}; seed.velocity = {}; seed.route = { .waypoints = {{.x = 2.0, .y = 0.5}, {.x = 3.0, .y = 0.5}}, @@ -1015,7 +1057,7 @@ SC_TEST(ScenarioEnvironmentHazardSystem_ReroutesOnlyAfterHazardAwareness) { "near-exit-fire", safecrowd::domain::EnvironmentHazardKind::Fire, safecrowd::domain::ScenarioElementSeverity::Low, - {.x = 3.0, .y = 0.5}, + {.x = 3.6, .y = 0.5}, "near-exit"); safecrowd::engine::EngineRuntime runtime({ @@ -1045,6 +1087,93 @@ SC_TEST(ScenarioEnvironmentHazardSystem_ReroutesOnlyAfterHazardAwareness) { SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); } +SC_TEST(ScenarioEnvironmentHazardSystem_RoutePenaltyUsesPathGeometryNotOnlyAffectedZone) { + safecrowd::domain::ScenarioAgentSeed seed; + seed.position = {.value = {.x = 5.0, .y = 5.0}}; + seed.agent = {.radius = 0.25f, .maxSpeed = 1.0f, .hazardSensitivity = 4.0}; + seed.velocity = {}; + seed.route = { + .waypoints = {{.x = 10.0, .y = 1.0}, {.x = 11.0, .y = 1.0}}, + .waypointPassages = { + {{.x = 10.0, .y = 0.7}, {.x = 10.0, .y = 1.3}}, + {{.x = 11.0, .y = 1.0}, {.x = 11.0, .y = 1.0}}, + }, + .waypointFromZoneIds = {"room", ""}, + .waypointZoneIds = {"near-exit", "near-exit"}, + .waypointConnectionIds = {"room-near-exit", ""}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 5.0, .y = 5.0}, + .previousDistanceToWaypoint = 6.4, + .destinationZoneId = "near-exit", + }; + + auto fire = hazardDraft( + "room-fire-near-exit", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::Low, + {.x = 9.5, .y = 1.0}, + "room"); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 48, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + addHazardMotionSystems(runtime, wideTwoExitHazardRouteLayout(), {fire}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + + const auto entities = runtime.world().query().view(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); +} + +SC_TEST(ScenarioEnvironmentHazardSystem_ExposureCountsPhysicalFootprintIndependentOfDetectionSensitivity) { + auto seed = straightRouteSeed({.x = 0.0, .y = 0.0}, 1.0); + seed.agent.hazardSensitivity = 0.0; + + auto fire = hazardDraft( + "physical-fire", + safecrowd::domain::EnvironmentHazardKind::Fire, + safecrowd::domain::ScenarioElementSeverity::Low, + {.x = 0.0, .y = 0.0}); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 49, + }); + runtime.addSystem(std::make_unique( + std::vector{seed}, + 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioEnvironmentHazardSystem(straightExitLayout(), {fire}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .order = -20, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 1.0}); + runtime.stepFrame(0.0); + + const auto& exposure = + runtime.world().resources().get(); + const auto& metric = exposure.hazardsById.at("physical-fire"); + SC_EXPECT_NEAR(metric.exposedAgentSeconds, 1.0, 1e-9); + SC_EXPECT_EQ(metric.peakExposedAgentCount, std::size_t{1}); + + const auto& reactions = + runtime.world().resources().get(); + const auto reactionIt = reactions.agentsById.find(0); + SC_EXPECT_TRUE(reactionIt == reactions.agentsById.end() || !reactionIt->second.hazardInRange); +} + SC_TEST(ScenarioEnvironmentHazardSystem_IgnoresInactiveAndDifferentFloorHazards) { auto layout = overlappingFloorBottleneckLayout(); auto seed = straightRouteSeed({.x = 0.25, .y = 0.0}, 1.0, 0.0, "exit-l1");