diff --git a/src/application/ScenarioAuthoringWidget.cpp b/src/application/ScenarioAuthoringWidget.cpp index 1294310..c4d0a8c 100644 --- a/src/application/ScenarioAuthoringWidget.cpp +++ b/src/application/ScenarioAuthoringWidget.cpp @@ -406,6 +406,18 @@ void ScenarioAuthoringWidget::refreshCanvas() { canvas_->setPlacementsChangedHandler([this](const std::vector& placements) { updateCurrentScenarioPlacements(placements); }); + canvas_->setConnectionBlocks(scenario->draft.control.connectionBlocks); + canvas_->setConnectionBlocksChangedHandler([this](const std::vector& blocks) { + auto* current = currentScenario(); + if (current == nullptr) { + return; + } + current->draft.control.connectionBlocks = blocks; + refreshInspector(); + if (rightPanelMode_ == RightPanelMode::Run) { + refreshRightPanel(); + } + }); shell_->setCanvas(canvas_); } @@ -475,7 +487,7 @@ void ScenarioAuthoringWidget::refreshNavigationPanel() { &layout_, [this](const QString& elementId) { if (canvas_ != nullptr) { - canvas_->focusLayoutElement(elementId); + canvas_->activateLayoutElement(elementId); } }, shell_, diff --git a/src/application/ScenarioCanvasWidget.cpp b/src/application/ScenarioCanvasWidget.cpp index 3653ef6..d0a39f9 100644 --- a/src/application/ScenarioCanvasWidget.cpp +++ b/src/application/ScenarioCanvasWidget.cpp @@ -5,16 +5,23 @@ #include #include +#include +#include +#include #include #include #include #include #include #include +#include +#include #include #include +#include #include #include +#include #include namespace safecrowd::application { @@ -171,6 +178,18 @@ QIcon makeToolIcon(const QString& type, const QColor& color) { return QIcon(pixmap); } + if (type == "block") { + painter.setPen(QPen(color, 3.0, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.setBrush(Qt::NoBrush); + painter.drawEllipse(QPointF(22, 22), 11.5, 11.5); + painter.drawLine(QPointF(14.5, 29.5), QPointF(29.5, 14.5)); + return QIcon(pixmap); + } + + if (type != "group") { + return QIcon(pixmap); + } + painter.setBrush(color); painter.setPen(Qt::NoPen); painter.drawEllipse(QPointF(17, 15), 4.0, 4.0); @@ -180,6 +199,177 @@ QIcon makeToolIcon(const QString& type, const QColor& color) { return QIcon(pixmap); } +double intervalSecondsFrom(int value, const QString& unit) { + if (unit == "hour") { + return static_cast(value) * 3600.0; + } + if (unit == "min") { + return static_cast(value) * 60.0; + } + return static_cast(value); +} + +QStringList intervalUnitOptions() { + return {"sec", "min", "hour"}; +} + +class ConnectionBlockScheduleDialog final : public QDialog { +public: + explicit ConnectionBlockScheduleDialog( + std::vector intervals, + QWidget* parent = nullptr) + : QDialog(parent), + intervals_(std::move(intervals)) { + setWindowTitle("Block door schedule"); + setModal(true); + + auto* root = new QVBoxLayout(this); + root->setContentsMargins(12, 12, 12, 12); + root->setSpacing(10); + + auto* caption = new QLabel("Add one or more block/unblock intervals.", this); + caption->setWordWrap(true); + root->addWidget(caption); + + rowsContainer_ = new QWidget(this); + rowsLayout_ = new QVBoxLayout(rowsContainer_); + rowsLayout_->setContentsMargins(0, 0, 0, 0); + rowsLayout_->setSpacing(8); + root->addWidget(rowsContainer_); + + auto* addRowButton = new QPushButton("+", this); + addRowButton->setToolTip("Add interval"); + addRowButton->setFixedSize(36, 32); + root->addWidget(addRowButton, 0, Qt::AlignLeft); + connect(addRowButton, &QPushButton::clicked, this, [this]() { + addRow({}); + }); + + auto* buttons = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel, this); + root->addWidget(buttons); + connect(buttons, &QDialogButtonBox::accepted, this, [this]() { + if (!applyFromUi()) { + return; + } + accept(); + }); + connect(buttons, &QDialogButtonBox::rejected, this, [this]() { + reject(); + }); + + if (intervals_.empty()) { + addRow({}); + } else { + for (const auto& interval : intervals_) { + addRow(interval); + } + } + } + + std::vector intervals() const { + return intervals_; + } + +private: + struct Row { + QWidget* container{nullptr}; + QSpinBox* startValue{nullptr}; + QComboBox* startUnit{nullptr}; + QSpinBox* endValue{nullptr}; + QComboBox* endUnit{nullptr}; + QPushButton* removeButton{nullptr}; + }; + + static int clampIntSeconds(double seconds) { + if (!std::isfinite(seconds) || seconds < 0.0) { + return 0; + } + const auto value = static_cast(std::llround(seconds)); + return static_cast(std::clamp(value, 0, 1'000'000'000LL)); + } + + void addRow(const safecrowd::domain::ConnectionBlockIntervalDraft& interval) { + auto* row = new QWidget(rowsContainer_); + auto* layout = new QHBoxLayout(row); + layout->setContentsMargins(0, 0, 0, 0); + layout->setSpacing(8); + + auto* startLabel = new QLabel("Start", row); + layout->addWidget(startLabel); + + auto* startValue = new QSpinBox(row); + startValue->setRange(0, 1'000'000'000); + startValue->setValue(clampIntSeconds(interval.startSeconds)); + layout->addWidget(startValue); + + auto* startUnit = new QComboBox(row); + startUnit->addItems(intervalUnitOptions()); + startUnit->setCurrentText("sec"); + layout->addWidget(startUnit); + + auto* endLabel = new QLabel("End", row); + layout->addWidget(endLabel); + + auto* endValue = new QSpinBox(row); + endValue->setRange(0, 1'000'000'000); + endValue->setValue(clampIntSeconds(interval.endSeconds)); + layout->addWidget(endValue); + + auto* endUnit = new QComboBox(row); + endUnit->addItems(intervalUnitOptions()); + endUnit->setCurrentText("sec"); + layout->addWidget(endUnit); + + auto* remove = new QPushButton("-", row); + remove->setToolTip("Remove interval"); + remove->setFixedSize(32, 32); + layout->addWidget(remove); + + Row widgets{ + .container = row, + .startValue = startValue, + .startUnit = startUnit, + .endValue = endValue, + .endUnit = endUnit, + .removeButton = remove, + }; + rows_.push_back(widgets); + rowsLayout_->addWidget(row); + + connect(remove, &QPushButton::clicked, this, [this, row]() { + rows_.erase(std::remove_if(rows_.begin(), rows_.end(), [&](const Row& r) { return r.container == row; }), rows_.end()); + row->deleteLater(); + }); + } + + bool applyFromUi() { + std::vector intervals; + intervals.reserve(rows_.size()); + for (const auto& row : rows_) { + if (row.startValue == nullptr || row.startUnit == nullptr || row.endValue == nullptr || row.endUnit == nullptr) { + continue; + } + const auto startSeconds = intervalSecondsFrom(row.startValue->value(), row.startUnit->currentText()); + const auto endSeconds = intervalSecondsFrom(row.endValue->value(), row.endUnit->currentText()); + if (endSeconds < startSeconds) { + QMessageBox::warning(this, "Invalid interval", "End time must be greater than or equal to start time."); + return false; + } + intervals.push_back({ + .startSeconds = startSeconds, + .endSeconds = endSeconds, + }); + } + intervals_ = std::move(intervals); + return true; + } + + QWidget* rowsContainer_{nullptr}; + QVBoxLayout* rowsLayout_{nullptr}; + std::vector rows_{}; + std::vector intervals_{}; +}; + } // namespace ScenarioCanvasWidget::ScenarioCanvasWidget( @@ -217,6 +407,15 @@ void ScenarioCanvasWidget::setPlacementsChangedHandler(std::function blocks) { + connectionBlocks_ = std::move(blocks); + update(); +} + +void ScenarioCanvasWidget::setConnectionBlocksChangedHandler(std::function&)> handler) { + connectionBlocksChangedHandler_ = std::move(handler); +} + void ScenarioCanvasWidget::focusLayoutElement(const QString& elementId) { if (elementId.startsWith("floor:")) { currentFloorId_ = elementId.mid(QString("floor:").size()); @@ -233,6 +432,24 @@ void ScenarioCanvasWidget::focusLayoutElement(const QString& elementId) { update(); } +void ScenarioCanvasWidget::activateLayoutElement(const QString& elementId) { + focusLayoutElement(elementId); + + if (toolMode_ != ToolMode::BlockDoor) { + return; + } + + const auto targetId = elementId.toStdString(); + const auto it = std::find_if(layout_.connections.begin(), layout_.connections.end(), [&](const auto& connection) { + return connection.id == targetId; + }); + if (it == layout_.connections.end()) { + return; + } + + addConnectionBlockForConnection(*it); +} + void ScenarioCanvasWidget::focusPlacement(const QString& placementId) { focusedPlacementId_ = placementId.section('/', 0, 0); focusedLayoutElementId_.clear(); @@ -298,6 +515,38 @@ void ScenarioCanvasWidget::mousePressEvent(QMouseEvent* event) { return; } + if (event->button() == Qt::RightButton) { + const auto point = unmapPoint(event->position()); + constexpr double kPickRadiusPixels = 18.0; + const auto offsetPoint = unmapPoint(event->position() + QPointF(kPickRadiusPixels, 0.0)); + const auto dx = offsetPoint.x - point.x; + const auto dy = offsetPoint.y - point.y; + const auto hitTolerance = std::max(1.2, std::hypot(dx, dy)); + for (const auto& block : connectionBlocks_) { + if (block.connectionId.empty()) { + continue; + } + const auto it = std::find_if(layout_.connections.begin(), layout_.connections.end(), [&](const auto& connection) { + return connection.id == block.connectionId; + }); + if (it == layout_.connections.end()) { + continue; + } + const auto halfWidth = std::max(0.0, it->effectiveWidth * 0.5); + const auto distance = std::max( + 0.0, + distancePointToSegment(point, it->centerSpan.start, it->centerSpan.end) - halfWidth); + if (distance <= hitTolerance) { + openConnectionBlockScheduleEditor(QString::fromStdString(block.id), event->globalPosition().toPoint()); + event->accept(); + return; + } + } + + QWidget::mousePressEvent(event); + return; + } + if (event->button() != Qt::LeftButton || event->position().y() < kTopToolbarHeight + kPropertyPanelHeight) { QWidget::mousePressEvent(event); return; @@ -318,6 +567,12 @@ void ScenarioCanvasWidget::mousePressEvent(QMouseEvent* event) { return; } + if (toolMode_ == ToolMode::BlockDoor) { + addConnectionBlock(event->position()); + event->accept(); + return; + } + QWidget::mousePressEvent(event); } @@ -415,6 +670,7 @@ void ScenarioCanvasWidget::paintEvent(QPaintEvent* event) { } } drawFocusedPlacement(painter, transform); + drawConnectionBlocks(painter, transform); if (dragging_) { painter.setPen(QPen(QColor("#1f5fae"), 1.5, Qt::DashLine)); @@ -523,6 +779,28 @@ void ScenarioCanvasWidget::drawFocusedPlacement(QPainter& painter, const LayoutC painter.drawPath(path); } +void ScenarioCanvasWidget::drawConnectionBlocks(QPainter& painter, const LayoutCanvasTransform& transform) const { + painter.setBrush(Qt::NoBrush); + painter.setPen(QPen(QColor("#c0392b"), 2.8, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + for (const auto& block : connectionBlocks_) { + if (block.connectionId.empty()) { + continue; + } + const auto it = std::find_if(layout_.connections.begin(), layout_.connections.end(), [&](const auto& connection) { + return connection.id == block.connectionId; + }); + if (it == layout_.connections.end()) { + continue; + } + + const auto center = transform.map(connectionCenter(*it)); + const double r = 10.0; + painter.drawEllipse(center, r, r); + painter.drawLine(QPointF(center.x() - 6.5, center.y() + 6.5), QPointF(center.x() + 6.5, center.y() - 6.5)); + } +} + std::optional ScenarioCanvasWidget::collectBounds() const { return collectLayoutCanvasBounds(layout_, currentFloorId_.toStdString()); } @@ -592,6 +870,28 @@ QString ScenarioCanvasWidget::zoneAt(const safecrowd::domain::Point2D& point) co return {}; } +const safecrowd::domain::Connection2D* ScenarioCanvasWidget::connectionAt( + const safecrowd::domain::Point2D& point, + double toleranceWorldUnits) const { + const safecrowd::domain::Connection2D* best = nullptr; + double bestDistance = std::max(0.0, toleranceWorldUnits); + for (const auto& connection : layout_.connections) { + const auto distance = distancePointToSegment(point, connection.centerSpan.start, connection.centerSpan.end); + if (distance <= bestDistance) { + bestDistance = distance; + best = &connection; + } + } + return best; +} + +safecrowd::domain::Point2D ScenarioCanvasWidget::connectionCenter(const safecrowd::domain::Connection2D& connection) const { + return { + .x = (connection.centerSpan.start.x + connection.centerSpan.end.x) * 0.5, + .y = (connection.centerSpan.start.y + connection.centerSpan.end.y) * 0.5, + }; +} + bool ScenarioCanvasWidget::placementPointBlocked(const safecrowd::domain::Point2D& point) const { for (const auto& barrier : layout_.barriers) { if (!matchesFloor(barrier.floorId, currentFloorId_)) { @@ -721,6 +1021,10 @@ QString ScenarioCanvasWidget::nextPlacementId(ScenarioCrowdPlacementKind kind) c return QString("%1-%2").arg(prefix).arg(static_cast(placements_.size()) + 1); } +QString ScenarioCanvasWidget::nextConnectionBlockId() const { + return QString("block-%1").arg(static_cast(connectionBlocks_.size()) + 1); +} + void ScenarioCanvasWidget::addGroupPlacement(const QPointF& start, const QPointF& end) { if ((QLineF(start, end).length()) < 8.0) { return; @@ -779,12 +1083,104 @@ void ScenarioCanvasWidget::addIndividualPlacement(const QPointF& position) { update(); } +void ScenarioCanvasWidget::addConnectionBlock(const QPointF& position) { + const auto point = unmapPoint(position); + constexpr double kPickRadiusPixels = 18.0; + const auto offsetPoint = unmapPoint(position + QPointF(kPickRadiusPixels, 0.0)); + const auto dx = offsetPoint.x - point.x; + const auto dy = offsetPoint.y - point.y; + const auto pixelToleranceWorldUnits = std::hypot(dx, dy); + + const auto toleranceWorldUnits = std::max(1.2, pixelToleranceWorldUnits); + const safecrowd::domain::Connection2D* connection = nullptr; + double bestDistance = toleranceWorldUnits; + for (const auto& candidate : layout_.connections) { + if (candidate.kind != safecrowd::domain::ConnectionKind::Doorway + && candidate.kind != safecrowd::domain::ConnectionKind::Exit) { + continue; + } + const auto halfWidth = std::max(0.0, candidate.effectiveWidth * 0.5); + const auto distance = + std::max(0.0, distancePointToSegment(point, candidate.centerSpan.start, candidate.centerSpan.end) - halfWidth); + if (distance <= bestDistance) { + bestDistance = distance; + connection = &candidate; + } + } + if (connection == nullptr) { + QMessageBox::information(this, "Block door", "This tool can only be used on exits or doors."); + return; + } + + addConnectionBlockForConnection(*connection); +} + +void ScenarioCanvasWidget::addConnectionBlockForConnection(const safecrowd::domain::Connection2D& connection) { + if (connection.kind != safecrowd::domain::ConnectionKind::Doorway + && connection.kind != safecrowd::domain::ConnectionKind::Exit) { + QMessageBox::information(this, "Block door", "This tool can only be used on exits or doors."); + return; + } + + for (const auto& existing : connectionBlocks_) { + if (existing.connectionId == connection.id) { + QMessageBox::information(this, "Block door", "This door or exit is already blocked."); + return; + } + } + + safecrowd::domain::ConnectionBlockDraft draft; + draft.id = nextConnectionBlockId().toStdString(); + draft.connectionId = connection.id; + connectionBlocks_.push_back(std::move(draft)); + emitConnectionBlocksChanged(); + update(); +} + +void ScenarioCanvasWidget::openConnectionBlockScheduleEditor(const QString& blockId, const QPoint& screenPosition) { + QMenu menu(this); + auto* editAction = menu.addAction("Set schedule..."); + auto* deleteAction = menu.addAction("Delete"); + const auto* selected = menu.exec(screenPosition); + if (selected != editAction && selected != deleteAction) { + return; + } + + auto it = std::find_if(connectionBlocks_.begin(), connectionBlocks_.end(), [&](const auto& block) { + return QString::fromStdString(block.id) == blockId; + }); + if (it == connectionBlocks_.end()) { + return; + } + + if (selected == deleteAction) { + connectionBlocks_.erase(it); + emitConnectionBlocksChanged(); + update(); + return; + } + + ConnectionBlockScheduleDialog dialog(it->intervals, this); + if (dialog.exec() != QDialog::Accepted) { + return; + } + it->intervals = dialog.intervals(); + emitConnectionBlocksChanged(); + update(); +} + void ScenarioCanvasWidget::emitPlacementsChanged() { if (placementsChangedHandler_) { placementsChangedHandler_(placements_); } } +void ScenarioCanvasWidget::emitConnectionBlocksChanged() { + if (connectionBlocksChangedHandler_) { + connectionBlocksChangedHandler_(connectionBlocks_); + } +} + void ScenarioCanvasWidget::repositionToolbars() { if (topToolbar_ != nullptr) { topToolbar_->setGeometry(0, 0, width(), kTopToolbarHeight); @@ -807,6 +1203,9 @@ void ScenarioCanvasWidget::setToolMode(ToolMode mode) { if (groupToolButton_ != nullptr) { groupToolButton_->setChecked(mode == ToolMode::GroupPlacement); } + if (blockDoorToolButton_ != nullptr) { + blockDoorToolButton_->setChecked(mode == ToolMode::BlockDoor); + } if (groupCountLabel_ != nullptr) { groupCountLabel_->setVisible(mode == ToolMode::GroupPlacement); } @@ -851,6 +1250,7 @@ void ScenarioCanvasWidget::setupToolbars() { selectToolButton_ = makeButton(makeToolIcon("select", QColor("#16202b")), "Select"); individualToolButton_ = makeButton(makeToolIcon("individual", QColor("#1f5fae")), "Add Individual Occupant"); groupToolButton_ = makeButton(makeToolIcon("group", QColor("#1f5fae")), "Add Occupant Group"); + blockDoorToolButton_ = makeButton(makeToolIcon("block", QColor("#c0392b")), "block door"); topLayout->addStretch(1); groupCountLabel_ = new QLabel("Group count", propertyPanel_); @@ -866,6 +1266,7 @@ void ScenarioCanvasWidget::setupToolbars() { connect(selectToolButton_, &QToolButton::clicked, this, [this]() { setToolMode(ToolMode::Select); }); connect(individualToolButton_, &QToolButton::clicked, this, [this]() { setToolMode(ToolMode::IndividualPlacement); }); connect(groupToolButton_, &QToolButton::clicked, this, [this]() { setToolMode(ToolMode::GroupPlacement); }); + connect(blockDoorToolButton_, &QToolButton::clicked, this, [this]() { setToolMode(ToolMode::BlockDoor); }); setToolMode(ToolMode::Select); repositionToolbars(); diff --git a/src/application/ScenarioCanvasWidget.h b/src/application/ScenarioCanvasWidget.h index 3cdd953..6a93ff9 100644 --- a/src/application/ScenarioCanvasWidget.h +++ b/src/application/ScenarioCanvasWidget.h @@ -10,6 +10,7 @@ #include "application/LayoutCanvasRendering.h" #include "domain/FacilityLayout2D.h" +#include "domain/ScenarioAuthoring.h" class QFrame; class QEvent; @@ -49,7 +50,10 @@ class ScenarioCanvasWidget : public QWidget { void setPlacements(std::vector placements); void setPlacementsChangedHandler(std::function&)> handler); + void setConnectionBlocks(std::vector blocks); + void setConnectionBlocksChangedHandler(std::function&)> handler); void focusLayoutElement(const QString& elementId); + void activateLayoutElement(const QString& elementId); void focusPlacement(const QString& placementId); protected: @@ -69,6 +73,7 @@ class ScenarioCanvasWidget : public QWidget { Select, IndividualPlacement, GroupPlacement, + BlockDoor, }; std::optional collectBounds() const; @@ -77,21 +82,30 @@ class ScenarioCanvasWidget : public QWidget { void selectFloorForElement(const QString& elementId); safecrowd::domain::Point2D unmapPoint(const QPointF& point) const; QString zoneAt(const safecrowd::domain::Point2D& point) const; + const safecrowd::domain::Connection2D* connectionAt(const safecrowd::domain::Point2D& point, double toleranceWorldUnits) const; + safecrowd::domain::Point2D connectionCenter(const safecrowd::domain::Connection2D& connection) const; bool placementAreaBlocked(const std::vector& area, int occupantCount) const; bool placementPointBlocked(const safecrowd::domain::Point2D& point) const; safecrowd::domain::Point2D defaultVelocityFrom(const safecrowd::domain::Point2D& point) const; QString nextPlacementId(ScenarioCrowdPlacementKind kind) const; + QString nextConnectionBlockId() const; void addGroupPlacement(const QPointF& start, const QPointF& end); void addIndividualPlacement(const QPointF& position); + void addConnectionBlock(const QPointF& position); + void addConnectionBlockForConnection(const safecrowd::domain::Connection2D& connection); + void openConnectionBlockScheduleEditor(const QString& blockId, const QPoint& screenPosition); void drawFocusedLayoutElement(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawFocusedPlacement(QPainter& painter, const LayoutCanvasTransform& transform) const; + void drawConnectionBlocks(QPainter& painter, const LayoutCanvasTransform& transform) const; void emitPlacementsChanged(); + void emitConnectionBlocksChanged(); void repositionToolbars(); void setToolMode(ToolMode mode); void setupToolbars(); safecrowd::domain::FacilityLayout2D layout_{}; std::vector placements_{}; + std::vector connectionBlocks_{}; QString currentFloorId_{}; QString focusedLayoutElementId_{}; QString focusedPlacementId_{}; @@ -105,9 +119,11 @@ class ScenarioCanvasWidget : public QWidget { QToolButton* selectToolButton_{nullptr}; QToolButton* individualToolButton_{nullptr}; QToolButton* groupToolButton_{nullptr}; + QToolButton* blockDoorToolButton_{nullptr}; QLabel* groupCountLabel_{nullptr}; QSpinBox* groupCountSpinBox_{nullptr}; std::function&)> placementsChangedHandler_{}; + std::function&)> connectionBlocksChangedHandler_{}; }; } // namespace safecrowd::application diff --git a/src/application/ScenarioRunWidget.cpp b/src/application/ScenarioRunWidget.cpp index 805ba48..65decb8 100644 --- a/src/application/ScenarioRunWidget.cpp +++ b/src/application/ScenarioRunWidget.cpp @@ -196,6 +196,7 @@ ScenarioRunWidget::ScenarioRunWidget( returnToAuthoring(); }); canvas_ = new SimulationCanvasWidget(layout_, shell_); + canvas_->setConnectionBlocks(scenario_.control.connectionBlocks); canvas_->setFrame(runner_.frame()); shell_->setCanvas(canvas_); shell_->setReviewPanel(createRunPanel()); diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index 9180e7f..97c89ca 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -42,6 +42,34 @@ bool matchesFloor(const std::string& elementFloorId, const std::string& floorId) return floorId.empty() || elementFloorId.empty() || elementFloorId == floorId; } +bool intervalContains(const safecrowd::domain::ConnectionBlockIntervalDraft& interval, double timeSeconds) { + const auto start = std::max(0.0, interval.startSeconds); + const auto end = std::max(start, interval.endSeconds); + return timeSeconds + 1e-9 >= start && timeSeconds <= end + 1e-9; +} + +bool connectionShouldBeBlocked(const safecrowd::domain::ConnectionBlockDraft& block, double timeSeconds) { + if (block.connectionId.empty()) { + return false; + } + if (block.intervals.empty()) { + return true; + } + for (const auto& interval : block.intervals) { + if (intervalContains(interval, timeSeconds)) { + return true; + } + } + return false; +} + +safecrowd::domain::Point2D connectionCenter(const safecrowd::domain::Connection2D& connection) { + return { + .x = (connection.centerSpan.start.x + connection.centerSpan.end.x) * 0.5, + .y = (connection.centerSpan.start.y + connection.centerSpan.end.y) * 0.5, + }; +} + } // namespace SimulationCanvasWidget::SimulationCanvasWidget(safecrowd::domain::FacilityLayout2D layout, QWidget* parent) @@ -80,6 +108,11 @@ void SimulationCanvasWidget::setFrame(safecrowd::domain::SimulationFrame frame) update(); } +void SimulationCanvasWidget::setConnectionBlocks(std::vector blocks) { + connectionBlocks_ = std::move(blocks); + update(); +} + void SimulationCanvasWidget::setHotspotOverlay(std::vector hotspots) { hotspotOverlay_ = std::move(hotspots); if (focusedHotspotIndex_.has_value() && *focusedHotspotIndex_ >= hotspotOverlay_.size()) { @@ -192,6 +225,7 @@ void SimulationCanvasWidget::paintEvent(QPaintEvent* event) { painter.drawPixmap(0, 0, layoutCache_); const auto transform = currentTransform(*bounds); + drawConnectionBlockOverlay(painter, transform); drawHotspotOverlay(painter, transform); drawBottleneckOverlay(painter, transform); for (const auto& agent : frame_.agents) { @@ -286,6 +320,38 @@ void SimulationCanvasWidget::focusWorldPoint(const safecrowd::domain::Point2D& p update(); } +void SimulationCanvasWidget::drawConnectionBlockOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const { + if (connectionBlocks_.empty()) { + return; + } + + const auto elapsedSeconds = std::max(0.0, frame_.elapsedSeconds); + + painter.save(); + painter.setBrush(Qt::NoBrush); + painter.setPen(QPen(QColor("#c0392b"), 2.8, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + + for (const auto& block : connectionBlocks_) { + if (!connectionShouldBeBlocked(block, elapsedSeconds)) { + continue; + } + + const auto it = std::find_if(layout_.connections.begin(), layout_.connections.end(), [&](const auto& connection) { + return connection.id == block.connectionId; + }); + if (it == layout_.connections.end()) { + continue; + } + + const auto center = transform.map(connectionCenter(*it)); + const double r = 10.0; + painter.drawEllipse(center, r, r); + painter.drawLine(QPointF(center.x() - 6.5, center.y() + 6.5), QPointF(center.x() + 6.5, center.y() - 6.5)); + } + + painter.restore(); +} + void SimulationCanvasWidget::drawHotspotOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const { if (hotspotOverlay_.empty()) { return; diff --git a/src/application/SimulationCanvasWidget.h b/src/application/SimulationCanvasWidget.h index 9ff5aef..b1be2c4 100644 --- a/src/application/SimulationCanvasWidget.h +++ b/src/application/SimulationCanvasWidget.h @@ -31,6 +31,7 @@ class SimulationCanvasWidget : public QWidget { ~SimulationCanvasWidget() override; void setFrame(safecrowd::domain::SimulationFrame frame); + void setConnectionBlocks(std::vector blocks); void setHotspotOverlay(std::vector hotspots); void setBottleneckOverlay(std::vector bottlenecks); void focusHotspot(std::size_t index); @@ -54,6 +55,7 @@ class SimulationCanvasWidget : public QWidget { void refreshLayoutCache(const LayoutCanvasBounds& bounds); QRectF previewViewport() const; void focusWorldPoint(const safecrowd::domain::Point2D& point, double zoom); + void drawConnectionBlockOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawHotspotOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void drawBottleneckOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; void setCurrentFloorId(std::string floorId, bool manualSelection); @@ -62,6 +64,7 @@ class SimulationCanvasWidget : public QWidget { safecrowd::domain::FacilityLayout2D layout_{}; safecrowd::domain::SimulationFrame frame_{}; + std::vector connectionBlocks_{}; std::vector hotspotOverlay_{}; std::vector bottleneckOverlay_{}; std::optional focusedHotspotIndex_{}; diff --git a/src/domain/ScenarioAuthoring.h b/src/domain/ScenarioAuthoring.h index 28be1b0..d10bace 100644 --- a/src/domain/ScenarioAuthoring.h +++ b/src/domain/ScenarioAuthoring.h @@ -28,8 +28,20 @@ struct OperationalEventDraft { std::string targetSummary{}; }; +struct ConnectionBlockIntervalDraft { + double startSeconds{0.0}; + double endSeconds{0.0}; +}; + +struct ConnectionBlockDraft { + std::string id{}; + std::string connectionId{}; + std::vector intervals{}; +}; + struct ControlPlan { std::vector events{}; + std::vector connectionBlocks{}; }; struct ExecutionConfig { diff --git a/src/domain/ScenarioRiskMetricsSystem.cpp b/src/domain/ScenarioRiskMetricsSystem.cpp index 0ab0aa6..89e2ef5 100644 --- a/src/domain/ScenarioRiskMetricsSystem.cpp +++ b/src/domain/ScenarioRiskMetricsSystem.cpp @@ -150,6 +150,9 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { auto& query = world.query(); auto& resources = world.resources(); + activeLayout_ = resources.contains() + ? &resources.get().layout + : &layout_; ScenarioRiskSnapshot snapshot; const auto entities = query.view(); @@ -206,9 +209,14 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { .snapshot = std::move(snapshot), .peakSnapshot = std::move(peakSnapshot), }); + activeLayout_ = nullptr; } private: + const FacilityLayout2D& layout() const { + return activeLayout_ == nullptr ? layout_ : *activeLayout_; + } + void mergePeakSnapshot(ScenarioRiskSnapshot& peak, const ScenarioRiskSnapshot& current) const { if (riskSeverity(current.completionRisk) > riskSeverity(peak.completionRisk)) { peak.completionRisk = current.completionRisk; @@ -248,7 +256,7 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { } std::string zoneDisplayName(const std::string& zoneId) const { - const auto* zone = findZone(layout_, zoneId); + const auto* zone = findZone(layout(), zoneId); if (zone == nullptr) { return zoneId; } @@ -268,7 +276,7 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { ScenarioRiskSnapshot& snapshot, engine::WorldQuery& query, const std::vector& entities) const { - for (const auto& connection : layout_.connections) { + for (const auto& connection : layout().connections) { if (connection.directionality == TravelDirection::Closed) { continue; } @@ -323,6 +331,7 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { } FacilityLayout2D layout_{}; + const FacilityLayout2D* activeLayout_{nullptr}; }; } // namespace diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 05534d8..93a0ff9 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -12,90 +12,96 @@ namespace { using namespace simulation_internal; -class ScenarioSimulationMotionSystem final : public engine::EngineSystem { -public: - explicit ScenarioSimulationMotionSystem(FacilityLayout2D layout) - : layout_(std::move(layout)) { - } - - void update(engine::EngineWorld& world, const engine::EngineStepContext& step) override { - (void)step; - - auto& resources = world.resources(); - if (!resources.contains()) { - return; - } - - auto& clock = resources.get(); - if (clock.complete) { - return; - } - - const auto clampedDelta = std::max(0.0, resources.get().deltaSeconds); - if (clampedDelta <= 0.0) { - return; - } - - auto& query = world.query(); - const auto entities = simulationEntities(query); - const auto localNeighborIndex = resources.contains() - ? AgentSpatialIndex{} - : buildAgentSpatialIndex(query, entities, 1.0); - std::vector plans; - plans.reserve(entities.size()); - - advanceRoutesForCurrentZones(query, entities); - advanceRoutesForWaypointProgress(query, 0.0, entities); - replanBlockedRouteSegments(query, entities); - - for (const auto entity : entities) { - auto& position = query.get(entity); - const auto& agent = query.get(entity); - auto& velocity = query.get(entity); - auto& route = query.get(entity); - auto& status = query.get(entity); +class ScenarioSimulationMotionSystem final : public engine::EngineSystem { +public: + explicit ScenarioSimulationMotionSystem(FacilityLayout2D layout) + : layout_(std::move(layout)) { + } + + void update(engine::EngineWorld& world, const engine::EngineStepContext& step) override { + (void)step; + + auto& resources = world.resources(); + activeLayout_ = resources.contains() + ? &resources.get().layout + : nullptr; + if (!resources.contains()) { + activeLayout_ = nullptr; + return; + } + + auto& clock = resources.get(); + if (clock.complete) { + activeLayout_ = nullptr; + return; + } + + const auto clampedDelta = std::max(0.0, resources.get().deltaSeconds); + if (clampedDelta <= 0.0) { + activeLayout_ = nullptr; + return; + } + + auto& query = world.query(); + const auto entities = simulationEntities(query); + const auto localNeighborIndex = resources.contains() + ? AgentSpatialIndex{} + : buildAgentSpatialIndex(query, entities, 1.0); + std::vector plans; + plans.reserve(entities.size()); + + advanceRoutesForCurrentZones(query, entities); + advanceRoutesForWaypointProgress(query, 0.0, entities); + replanBlockedRouteSegments(query, entities); + + for (const auto entity : entities) { + auto& position = query.get(entity); + const auto& agent = query.get(entity); + auto& velocity = query.get(entity); + auto& route = query.get(entity); + auto& status = query.get(entity); if (status.evacuated) { continue; } - const auto floorLayout = layoutForFloor(layout_, route.currentFloorId); + const auto floorLayout = layoutForFloor(layout(), route.currentFloorId); const auto* destinationZone = findZone(floorLayout, route.destinationZoneId); if (destinationZone != nullptr && pointInRing(destinationZone->area.outline, position.value)) { status.evacuated = true; - status.completionTimeSeconds = clock.elapsedSeconds; - velocity.value = {}; - continue; - } - - if (route.nextWaypointIndex >= route.waypoints.size()) { - velocity.value = {}; - continue; - } - - const auto target = routeWaypointTarget(route, position.value); - const auto distance = distanceBetween(position.value, target); - if (distance <= kArrivalEpsilon) { - position.value = target; - advanceRouteWaypoint(route, target); - velocity.value = {}; - continue; - } - + status.completionTimeSeconds = clock.elapsedSeconds; + velocity.value = {}; + continue; + } + + if (route.nextWaypointIndex >= route.waypoints.size()) { + velocity.value = {}; + continue; + } + + const auto target = routeWaypointTarget(route, position.value); + const auto distance = distanceBetween(position.value, target); + if (distance <= kArrivalEpsilon) { + position.value = target; + advanceRouteWaypoint(route, target); + velocity.value = {}; + continue; + } + const auto routeDirection = (target - position.value) * (1.0 / distance); const auto maxSpeed = effectiveMaxSpeed(agent, route, position.value); const auto desiredVelocity = routeDirection * maxSpeed; - double speedScale = 1.0; - const auto neighborRadius = static_cast(agent.radius) + kDefaultAgentRadius + kPersonalSpaceBuffer; - const auto neighborCandidates = resources.contains() - ? scenarioNearbyAgents(query, resources.get(), position.value, neighborRadius) - : nearbyAgents(query, localNeighborIndex, position.value, neighborRadius); + double speedScale = 1.0; + const auto neighborRadius = static_cast(agent.radius) + kDefaultAgentRadius + kPersonalSpaceBuffer; + const auto neighborCandidates = resources.contains() + ? scenarioNearbyAgents(query, resources.get(), position.value, neighborRadius) + : nearbyAgents(query, localNeighborIndex, position.value, neighborRadius); const auto avoidanceVelocity = forwardPreservingAgentAvoidanceVelocity(query, entity, neighborCandidates, desiredVelocity, speedScale); const auto barrierVelocity = barrierSeparationVelocity(floorLayout, position, agent); - auto finalVelocity = (desiredVelocity * speedScale) + avoidanceVelocity + barrierVelocity; - if (dot(finalVelocity, routeDirection) < 0.0) { - const auto lateral = perpendicularLeft(routeDirection); - finalVelocity = (routeDirection * (static_cast(agent.maxSpeed) * 0.15)) + auto finalVelocity = (desiredVelocity * speedScale) + avoidanceVelocity + barrierVelocity; + if (dot(finalVelocity, routeDirection) < 0.0) { + const auto lateral = perpendicularLeft(routeDirection); + finalVelocity = (routeDirection * (static_cast(agent.maxSpeed) * 0.15)) + (lateral * dot(finalVelocity, lateral)); } plans.push_back({ @@ -103,16 +109,16 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { .velocity = clampedToLength(finalVelocity, maxSpeed), }); } - - for (const auto& plan : plans) { - auto& position = query.get(plan.entity); - auto& velocity = query.get(plan.entity); - auto& route = query.get(plan.entity); - const auto& agent = query.get(plan.entity); - if (route.nextWaypointIndex >= route.waypoints.size()) { - continue; - } - + + for (const auto& plan : plans) { + auto& position = query.get(plan.entity); + auto& velocity = query.get(plan.entity); + auto& route = query.get(plan.entity); + const auto& agent = query.get(plan.entity); + if (route.nextWaypointIndex >= route.waypoints.size()) { + continue; + } + const auto target = routeWaypointTarget(route, position.value); const auto remainingDistance = distanceBetween(position.value, target); const auto maxSpeed = effectiveMaxSpeed(agent, route, position.value); @@ -120,22 +126,27 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { clampedToLength(plan.velocity, std::min(maxSpeed, remainingDistance / clampedDelta)); const auto previousPosition = position.value; const auto nextPosition = constrainedMove( - layoutForFloor(layout_, route.currentFloorId), + layoutForFloor(layout(), route.currentFloorId), previousPosition, previousPosition + (stepVelocity * clampedDelta)); position.value = nextPosition; velocity.value = (nextPosition - previousPosition) * (1.0 / clampedDelta); updateDisplayFloor(route, nextPosition); } - - resolveAgentOverlaps(query, entities); - advanceRoutesForCurrentZones(query, entities); - advanceRoutesForWaypointProgress(query, clampedDelta, entities); - advanceClock(query, clock, entities, clampedDelta); - resources.set(ScenarioSimulationStepResource{}); - } - -private: + + resolveAgentOverlaps(query, entities); + advanceRoutesForCurrentZones(query, entities); + advanceRoutesForWaypointProgress(query, clampedDelta, entities); + advanceClock(query, clock, entities, clampedDelta); + resources.set(ScenarioSimulationStepResource{}); + activeLayout_ = nullptr; + } + +private: + const FacilityLayout2D& layout() const { + return activeLayout_ == nullptr ? layout_ : *activeLayout_; + } + void advanceRouteWaypoint(EvacuationRoute& route, const Point2D& reachedPoint) const { if (route.nextWaypointIndex >= route.waypoints.size()) { return; @@ -148,141 +159,141 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { route.displayFloorId = route.currentFloorId; route.currentSegmentStart = reachedPoint; ++route.nextWaypointIndex; - if (route.nextWaypointIndex < route.waypoints.size()) { - route.previousDistanceToWaypoint = - distanceToRouteWaypoint(route, route.currentSegmentStart); - } else { - route.previousDistanceToWaypoint = 0.0; - } - route.stalledSeconds = 0.0; - } - - void advanceRoutesForWaypointProgress( - engine::WorldQuery& query, - double deltaSeconds, - const std::vector& entities) const { - for (const auto entity : entities) { - const auto& status = query.get(entity); - if (status.evacuated) { - continue; - } - - const auto& position = query.get(entity); + if (route.nextWaypointIndex < route.waypoints.size()) { + route.previousDistanceToWaypoint = + distanceToRouteWaypoint(route, route.currentSegmentStart); + } else { + route.previousDistanceToWaypoint = 0.0; + } + route.stalledSeconds = 0.0; + } + + void advanceRoutesForWaypointProgress( + engine::WorldQuery& query, + double deltaSeconds, + const std::vector& entities) const { + 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); auto& route = query.get(entity); while (route.nextWaypointIndex < route.waypoints.size()) { - const auto floorLayout = layoutForFloor(layout_, route.currentFloorId); + const auto floorLayout = layoutForFloor(layout(), route.currentFloorId); if (routePassageCrossed(floorLayout, route, position.value, agent.radius)) { advanceRouteWaypoint(route, position.value); continue; } - - const auto target = routeWaypointTarget(route, position.value); - const auto segment = target - route.currentSegmentStart; - const auto segmentLengthSquared = dot(segment, segment); - const auto distance = distanceToRouteWaypoint(route, position.value); - - if (distance <= kArrivalEpsilon) { - advanceRouteWaypoint(route, target); - continue; - } - - if (segmentLengthSquared > 1e-9) { - const auto projection = dot(position.value - route.currentSegmentStart, segment); - if (projection >= segmentLengthSquared - kWaypointCrossingEpsilon) { - advanceRouteWaypoint(route, target); - continue; - } - } - - if (route.previousDistanceToWaypoint <= 0.0 - || distance < route.previousDistanceToWaypoint - kWaypointProgressEpsilon) { - route.previousDistanceToWaypoint = distance; - route.stalledSeconds = 0.0; - break; - } - - if (deltaSeconds > 0.0) { - route.stalledSeconds += deltaSeconds; - } - - if (route.stalledSeconds >= kWaypointStallSeconds - && route.nextWaypointIndex + 1 < route.waypoints.size() - && segmentLengthSquared > 1e-9) { - const auto projection = dot(position.value - route.currentSegmentStart, segment); - if (projection > segmentLengthSquared * 0.45) { - advanceRouteWaypoint(route, target); - continue; - } - } - - route.previousDistanceToWaypoint = std::min(route.previousDistanceToWaypoint, distance); - break; - } - } - } - - void advanceRoutesForCurrentZones(engine::WorldQuery& query, const std::vector& entities) const { - for (const auto entity : entities) { - const auto& status = query.get(entity); - if (status.evacuated) { - continue; - } - - const auto& position = query.get(entity); - auto& route = query.get(entity); + + const auto target = routeWaypointTarget(route, position.value); + const auto segment = target - route.currentSegmentStart; + const auto segmentLengthSquared = dot(segment, segment); + const auto distance = distanceToRouteWaypoint(route, position.value); + + if (distance <= kArrivalEpsilon) { + advanceRouteWaypoint(route, target); + continue; + } + + if (segmentLengthSquared > 1e-9) { + const auto projection = dot(position.value - route.currentSegmentStart, segment); + if (projection >= segmentLengthSquared - kWaypointCrossingEpsilon) { + advanceRouteWaypoint(route, target); + continue; + } + } + + if (route.previousDistanceToWaypoint <= 0.0 + || distance < route.previousDistanceToWaypoint - kWaypointProgressEpsilon) { + route.previousDistanceToWaypoint = distance; + route.stalledSeconds = 0.0; + break; + } + + if (deltaSeconds > 0.0) { + route.stalledSeconds += deltaSeconds; + } + + if (route.stalledSeconds >= kWaypointStallSeconds + && route.nextWaypointIndex + 1 < route.waypoints.size() + && segmentLengthSquared > 1e-9) { + const auto projection = dot(position.value - route.currentSegmentStart, segment); + if (projection > segmentLengthSquared * 0.45) { + advanceRouteWaypoint(route, target); + continue; + } + } + + route.previousDistanceToWaypoint = std::min(route.previousDistanceToWaypoint, distance); + break; + } + } + } + + void advanceRoutesForCurrentZones(engine::WorldQuery& query, const std::vector& entities) const { + for (const auto entity : entities) { + const auto& status = query.get(entity); + if (status.evacuated) { + continue; + } + + const auto& position = query.get(entity); + auto& route = query.get(entity); const auto currentZoneId = zoneAt(position.value, route.currentFloorId); - while (!currentZoneId.empty() && route.nextWaypointIndex < route.waypointZoneIds.size()) { - auto matchedIndex = route.waypointZoneIds.size(); - for (auto index = route.nextWaypointIndex; index < route.waypointZoneIds.size(); ++index) { - if (route.waypointZoneIds[index] == currentZoneId) { - matchedIndex = index; - break; - } - } - if (matchedIndex == route.waypointZoneIds.size()) { - break; - } - - while (route.nextWaypointIndex <= matchedIndex && route.nextWaypointIndex < route.waypoints.size()) { - advanceRouteWaypoint(route, position.value); - } - } - } - } - - void replanBlockedRouteSegments(engine::WorldQuery& query, const std::vector& entities) const { - 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); - auto& route = query.get(entity); - if (route.nextWaypointIndex >= route.waypoints.size()) { - continue; - } - - const auto target = routeWaypointTarget(route, position.value); - const auto clearance = static_cast(agent.radius) + kPathClearance; - const auto floorLayout = layoutForFloor(layout_, route.currentFloorId); + while (!currentZoneId.empty() && route.nextWaypointIndex < route.waypointZoneIds.size()) { + auto matchedIndex = route.waypointZoneIds.size(); + for (auto index = route.nextWaypointIndex; index < route.waypointZoneIds.size(); ++index) { + if (route.waypointZoneIds[index] == currentZoneId) { + matchedIndex = index; + break; + } + } + if (matchedIndex == route.waypointZoneIds.size()) { + break; + } + + while (route.nextWaypointIndex <= matchedIndex && route.nextWaypointIndex < route.waypoints.size()) { + advanceRouteWaypoint(route, position.value); + } + } + } + } + + void replanBlockedRouteSegments(engine::WorldQuery& query, const std::vector& entities) const { + 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); + auto& route = query.get(entity); + if (route.nextWaypointIndex >= route.waypoints.size()) { + continue; + } + + const auto target = routeWaypointTarget(route, position.value); + const auto clearance = static_cast(agent.radius) + kPathClearance; + const auto floorLayout = layoutForFloor(layout(), route.currentFloorId); if (lineOfSightClear(floorLayout, position.value, target, clearance)) { continue; } const auto replacement = buildPath(floorLayout, position.value, target, clearance); - if (replacement.size() <= 1) { - continue; - } - - const auto originalTargetZoneId = route.nextWaypointIndex < route.waypointZoneIds.size() - ? route.waypointZoneIds[route.nextWaypointIndex] - : std::string{}; - const auto originalTargetPassage = route.nextWaypointIndex < route.waypointPassages.size() - ? route.waypointPassages[route.nextWaypointIndex] - : pointPassage(target); + if (replacement.size() <= 1) { + continue; + } + + const auto originalTargetZoneId = route.nextWaypointIndex < route.waypointZoneIds.size() + ? route.waypointZoneIds[route.nextWaypointIndex] + : std::string{}; + const auto originalTargetPassage = route.nextWaypointIndex < route.waypointPassages.size() + ? route.waypointPassages[route.nextWaypointIndex] + : pointPassage(target); const auto originalFromZoneId = route.nextWaypointIndex < route.waypointFromZoneIds.size() ? route.waypointFromZoneIds[route.nextWaypointIndex] : std::string{}; @@ -292,34 +303,27 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto originalConnectionId = route.nextWaypointIndex < route.waypointConnectionIds.size() ? route.waypointConnectionIds[route.nextWaypointIndex] : std::string{}; - const auto originalVerticalTransition = route.nextWaypointIndex < route.waypointVerticalTransitions.size() - ? route.waypointVerticalTransitions[route.nextWaypointIndex] - : false; route.waypoints.erase(route.waypoints.begin() + static_cast(route.nextWaypointIndex)); route.waypointPassages.erase(route.waypointPassages.begin() + static_cast(route.nextWaypointIndex)); route.waypointFromZoneIds.erase(route.waypointFromZoneIds.begin() + static_cast(route.nextWaypointIndex)); route.waypointZoneIds.erase(route.waypointZoneIds.begin() + static_cast(route.nextWaypointIndex)); - if (route.nextWaypointIndex < route.waypointConnectionIds.size()) { - route.waypointConnectionIds.erase( - route.waypointConnectionIds.begin() + static_cast(route.nextWaypointIndex)); - } + route.waypointFloorIds.erase(route.waypointFloorIds.begin() + static_cast(route.nextWaypointIndex)); + route.waypointConnectionIds.erase( + route.waypointConnectionIds.begin() + static_cast(route.nextWaypointIndex)); if (route.nextWaypointIndex < route.waypointVerticalTransitions.size()) { route.waypointVerticalTransitions.erase( route.waypointVerticalTransitions.begin() + static_cast(route.nextWaypointIndex)); } - if (route.nextWaypointIndex < route.waypointFloorIds.size()) { - route.waypointFloorIds.erase(route.waypointFloorIds.begin() + static_cast(route.nextWaypointIndex)); + + std::vector replacementPassages; + replacementPassages.reserve(replacement.size()); + for (const auto& waypoint : replacement) { + replacementPassages.push_back(pointPassage(waypoint)); } - - std::vector replacementPassages; - replacementPassages.reserve(replacement.size()); - for (const auto& waypoint : replacement) { - replacementPassages.push_back(pointPassage(waypoint)); - } - replacementPassages.back() = originalTargetPassage; - - std::vector replacementZoneIds(replacement.size(), std::string{}); - replacementZoneIds.back() = originalTargetZoneId; + replacementPassages.back() = originalTargetPassage; + + std::vector replacementZoneIds(replacement.size(), std::string{}); + replacementZoneIds.back() = originalTargetZoneId; std::vector replacementFromZoneIds(replacement.size(), std::string{}); replacementFromZoneIds.back() = originalFromZoneId; std::vector replacementFloorIds(replacement.size(), std::string{}); @@ -327,143 +331,137 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { std::vector replacementConnectionIds(replacement.size(), std::string{}); replacementConnectionIds.back() = originalConnectionId; std::vector replacementVerticalTransitions(replacement.size(), false); - replacementVerticalTransitions.back() = originalVerticalTransition; + if (route.nextWaypointIndex < route.waypointVerticalTransitions.size()) { + replacementVerticalTransitions.back() = route.waypointVerticalTransitions[route.nextWaypointIndex]; + } + route.waypoints.insert( route.waypoints.begin() + static_cast(route.nextWaypointIndex), replacement.begin(), - replacement.end()); - route.waypointPassages.insert( - route.waypointPassages.begin() + static_cast(route.nextWaypointIndex), - replacementPassages.begin(), - replacementPassages.end()); - route.waypointFromZoneIds.insert( - route.waypointFromZoneIds.begin() + static_cast(route.nextWaypointIndex), - replacementFromZoneIds.begin(), - replacementFromZoneIds.end()); + replacement.end()); + route.waypointPassages.insert( + route.waypointPassages.begin() + static_cast(route.nextWaypointIndex), + replacementPassages.begin(), + replacementPassages.end()); + route.waypointFromZoneIds.insert( + route.waypointFromZoneIds.begin() + static_cast(route.nextWaypointIndex), + replacementFromZoneIds.begin(), + replacementFromZoneIds.end()); route.waypointZoneIds.insert( route.waypointZoneIds.begin() + static_cast(route.nextWaypointIndex), replacementZoneIds.begin(), replacementZoneIds.end()); - if (route.waypointConnectionIds.size() < route.nextWaypointIndex) { - route.waypointConnectionIds.resize(route.nextWaypointIndex); - } + route.waypointFloorIds.insert( + route.waypointFloorIds.begin() + static_cast(route.nextWaypointIndex), + replacementFloorIds.begin(), + replacementFloorIds.end()); route.waypointConnectionIds.insert( route.waypointConnectionIds.begin() + static_cast(route.nextWaypointIndex), replacementConnectionIds.begin(), replacementConnectionIds.end()); - if (route.waypointVerticalTransitions.size() < route.nextWaypointIndex) { - route.waypointVerticalTransitions.resize(route.nextWaypointIndex); - } route.waypointVerticalTransitions.insert( route.waypointVerticalTransitions.begin() + static_cast(route.nextWaypointIndex), replacementVerticalTransitions.begin(), replacementVerticalTransitions.end()); - if (route.waypointFloorIds.size() < route.nextWaypointIndex) { - route.waypointFloorIds.resize(route.nextWaypointIndex); - } - route.waypointFloorIds.insert( - route.waypointFloorIds.begin() + static_cast(route.nextWaypointIndex), - replacementFloorIds.begin(), - replacementFloorIds.end()); - route.currentSegmentStart = position.value; - route.previousDistanceToWaypoint = distanceToRouteWaypoint(route, position.value); - route.stalledSeconds = 0.0; - } - } - - void resolveAgentOverlaps(engine::WorldQuery& query, const std::vector& entities) const { - for (int iteration = 0; iteration < kOverlapRelaxationIterations; ++iteration) { - const auto spatialIndex = buildAgentSpatialIndex(query, entities, 1.0); - std::unordered_set checkedPairs; - checkedPairs.reserve(entities.size() * 4); - for (const auto first : entities) { - auto& firstStatus = query.get(first); - if (firstStatus.evacuated) { - continue; - } - - auto& firstPosition = query.get(first); - const auto& firstAgent = query.get(first); - const auto candidates = nearbyAgents( - query, - spatialIndex, - firstPosition.value, - static_cast(firstAgent.radius) + kDefaultAgentRadius); - for (const auto second : candidates) { - if (first == second) { - continue; - } - const auto minIndex = std::min(first.index, second.index); - const auto maxIndex = std::max(first.index, second.index); - const auto pairKey = (static_cast(minIndex) << 32) - ^ static_cast(maxIndex); - if (!checkedPairs.insert(pairKey).second) { - continue; - } - - auto& secondStatus = query.get(second); - if (firstStatus.evacuated || secondStatus.evacuated) { - continue; - } - - auto& secondPosition = query.get(second); - const auto& secondAgent = query.get(second); - const auto delta = firstPosition.value - secondPosition.value; - const auto distance = lengthOf(delta); - const auto minimumDistance = static_cast(firstAgent.radius + secondAgent.radius); - if (distance >= minimumDistance) { - continue; - } - - const auto direction = normalizedOr(delta, deterministicFallbackDirection(first)); - const auto push = std::min(0.08, (minimumDistance - distance) * 0.35); + route.currentSegmentStart = position.value; + route.previousDistanceToWaypoint = distanceToRouteWaypoint(route, position.value); + route.stalledSeconds = 0.0; + } + } + + void resolveAgentOverlaps(engine::WorldQuery& query, const std::vector& entities) const { + for (int iteration = 0; iteration < kOverlapRelaxationIterations; ++iteration) { + const auto spatialIndex = buildAgentSpatialIndex(query, entities, 1.0); + std::unordered_set checkedPairs; + checkedPairs.reserve(entities.size() * 4); + for (const auto first : entities) { + auto& firstStatus = query.get(first); + if (firstStatus.evacuated) { + continue; + } + + auto& firstPosition = query.get(first); + const auto& firstAgent = query.get(first); + const auto candidates = nearbyAgents( + query, + spatialIndex, + firstPosition.value, + static_cast(firstAgent.radius) + kDefaultAgentRadius); + for (const auto second : candidates) { + if (first == second) { + continue; + } + const auto minIndex = std::min(first.index, second.index); + const auto maxIndex = std::max(first.index, second.index); + const auto pairKey = (static_cast(minIndex) << 32) + ^ static_cast(maxIndex); + if (!checkedPairs.insert(pairKey).second) { + continue; + } + + auto& secondStatus = query.get(second); + if (firstStatus.evacuated || secondStatus.evacuated) { + continue; + } + + auto& secondPosition = query.get(second); + const auto& secondAgent = query.get(second); + const auto delta = firstPosition.value - secondPosition.value; + const auto distance = lengthOf(delta); + const auto minimumDistance = static_cast(firstAgent.radius + secondAgent.radius); + if (distance >= minimumDistance) { + continue; + } + + const auto direction = normalizedOr(delta, deterministicFallbackDirection(first)); + const auto push = std::min(0.08, (minimumDistance - distance) * 0.35); const auto& firstRoute = query.get(first); const auto& secondRoute = query.get(second); firstPosition.value = constrainedMove( - layoutForFloor(layout_, firstRoute.currentFloorId), + layoutForFloor(layout(), firstRoute.currentFloorId), firstPosition.value, firstPosition.value + (direction * push)); secondPosition.value = constrainedMove( - layoutForFloor(layout_, secondRoute.currentFloorId), + layoutForFloor(layout(), secondRoute.currentFloorId), secondPosition.value, secondPosition.value - (direction * push)); - } - } - } - } - - void advanceClock( - engine::WorldQuery& query, - ScenarioSimulationClockResource& clock, - const std::vector& entities, - double deltaSeconds) const { - clock.elapsedSeconds += deltaSeconds; - clock.complete = clock.elapsedSeconds >= clock.timeLimitSeconds; - if (clock.complete) { - return; - } - - std::size_t totalAgentCount = 0; - std::size_t evacuatedAgentCount = 0; - for (const auto entity : entities) { - ++totalAgentCount; - const auto& status = query.get(entity); - if (status.evacuated) { - ++evacuatedAgentCount; - } - } - clock.complete = totalAgentCount > 0 && evacuatedAgentCount >= totalAgentCount; - } - + } + } + } + } + + void advanceClock( + engine::WorldQuery& query, + ScenarioSimulationClockResource& clock, + const std::vector& entities, + double deltaSeconds) const { + clock.elapsedSeconds += deltaSeconds; + clock.complete = clock.elapsedSeconds >= clock.timeLimitSeconds; + if (clock.complete) { + return; + } + + std::size_t totalAgentCount = 0; + std::size_t evacuatedAgentCount = 0; + for (const auto entity : entities) { + ++totalAgentCount; + const auto& status = query.get(entity); + if (status.evacuated) { + ++evacuatedAgentCount; + } + } + clock.complete = totalAgentCount > 0 && evacuatedAgentCount >= totalAgentCount; + } + std::string zoneAt(const Point2D& point, const std::string& floorId) const { - for (const auto& zone : layout_.zones) { + for (const auto& zone : layout().zones) { if (!floorId.empty() && !zone.floorId.empty() && zone.floorId != floorId) { continue; } if (pointInRing(zone.area.outline, point)) { return zone.id; - } - } + } + } return {}; } @@ -474,7 +472,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { double effectiveMaxSpeed(const Agent& agent, const EvacuationRoute& route, const Point2D& position) const { const auto currentZoneId = zoneAt(position, route.currentFloorId); - const auto* zone = findZone(layout_, currentZoneId); + const auto* zone = findZone(layout(), currentZoneId); const bool inStairZone = zone != nullptr && (zone->kind == ZoneKind::Stair || zone->isStair || zone->isRamp); const bool onVerticalTransition = currentWaypointIsVertical(route); @@ -506,9 +504,8 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } FacilityLayout2D layout_{}; + const FacilityLayout2D* activeLayout_{nullptr}; }; - - } // namespace @@ -516,4 +513,5 @@ std::unique_ptr makeScenarioSimulationMotionSystem(Facilit return std::make_unique(std::move(layout)); } -} // namespace safecrowd::domain +} // namespace safecrowd::domain + diff --git a/src/domain/ScenarioSimulationRunner.cpp b/src/domain/ScenarioSimulationRunner.cpp index 1b82505..1210518 100644 --- a/src/domain/ScenarioSimulationRunner.cpp +++ b/src/domain/ScenarioSimulationRunner.cpp @@ -142,6 +142,10 @@ void ScenarioSimulationRunner::initializeRuntime() { .baseSeed = 1, }); runtime_->addSystem(std::make_unique(createAgentSeeds(), timeLimitSeconds_)); + runtime_->addSystem( + makeScenarioControlSystem(layout_, scenario_.control.connectionBlocks), + {.phase = engine::UpdatePhase::PreSimulation, + .triggerPolicy = engine::TriggerPolicy::EveryFrame}); runtime_->addSystem( std::make_unique(1.0), {.phase = engine::UpdatePhase::PreSimulation, diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 0dc2847..dd52a8b 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -2,7 +2,10 @@ #include #include +#include #include +#include +#include #include #include "engine/EngineWorld.h" @@ -49,6 +52,110 @@ std::optional percentileCompletionTime( return completionTimes[targetCount - 1]; } +bool intervalContains(const ConnectionBlockIntervalDraft& interval, double timeSeconds) { + const auto start = std::max(0.0, interval.startSeconds); + const auto end = std::max(start, interval.endSeconds); + return timeSeconds + 1e-9 >= start && timeSeconds <= end + 1e-9; +} + +bool connectionShouldBeBlocked(const ConnectionBlockDraft& block, double timeSeconds) { + if (block.intervals.empty()) { + return true; + } + + for (const auto& interval : block.intervals) { + if (intervalContains(interval, timeSeconds)) { + return true; + } + } + return false; +} + +const Connection2D* findConnectionById(const 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; + }); + return it == layout.connections.end() ? nullptr : &(*it); +} + +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; + }); + return it == layout.connections.end() ? nullptr : &(*it); +} + +class ScenarioControlSystem final : public engine::EngineSystem { +public: + ScenarioControlSystem(FacilityLayout2D baseLayout, std::vector blocks) + : baseLayout_(std::move(baseLayout)), + blocks_(std::move(blocks)) { + } + + void configure(engine::EngineWorld& world) override { + world.resources().set(ScenarioLayoutResource{.layout = baseLayout_}); + world.resources().set(ScenarioLayoutRevisionResource{.revision = revision_}); + } + + void update(engine::EngineWorld& world, const engine::EngineStepContext& step) override { + (void)step; + + auto& resources = world.resources(); + if (!resources.contains()) { + resources.set(ScenarioLayoutResource{.layout = baseLayout_}); + } + + double elapsedSeconds = 0.0; + if (resources.contains()) { + elapsedSeconds = std::max(0.0, resources.get().elapsedSeconds); + } + + auto& layout = resources.get().layout; + layout = baseLayout_; + + std::unordered_set blockedConnectionIds; + blockedConnectionIds.reserve(blocks_.size()); + for (const auto& block : blocks_) { + if (block.connectionId.empty()) { + continue; + } + + auto* connection = findConnectionById(layout, block.connectionId); + if (connection == nullptr) { + continue; + } + + if (connectionShouldBeBlocked(block, elapsedSeconds)) { + connection->directionality = TravelDirection::Closed; + blockedConnectionIds.insert(block.connectionId); + layout.barriers.push_back(Barrier2D{ + .id = "control-block-" + block.connectionId, + .floorId = connection->floorId, + .geometry = Polyline2D{.vertices = {connection->centerSpan.start, connection->centerSpan.end}, .closed = false}, + .blocksMovement = true, + }); + } else { + // Restored by layout reset from baseLayout_. + } + } + + if (blockedConnectionIds != previousBlockedConnectionIds_) { + ++revision_; + previousBlockedConnectionIds_ = std::move(blockedConnectionIds); + resources.set(ScenarioLayoutRevisionResource{.revision = revision_}); + } else if (!resources.contains() + || resources.get().revision != revision_) { + resources.set(ScenarioLayoutRevisionResource{.revision = revision_}); + } + } + +private: + FacilityLayout2D baseLayout_{}; + std::vector blocks_{}; + std::unordered_set previousBlockedConnectionIds_{}; + std::uint64_t revision_{0}; +}; + } // namespace ScenarioAgentSpawnSystem::ScenarioAgentSpawnSystem(std::vector seeds, double timeLimitSeconds) @@ -275,4 +382,10 @@ void ScenarioResultArtifactsSystem::update(engine::EngineWorld& world, const eng } } +std::unique_ptr makeScenarioControlSystem( + FacilityLayout2D baseLayout, + std::vector blocks) { + return std::make_unique(std::move(baseLayout), std::move(blocks)); +} + } // namespace safecrowd::domain diff --git a/src/domain/ScenarioSimulationSystems.h b/src/domain/ScenarioSimulationSystems.h index 3034888..077feb1 100644 --- a/src/domain/ScenarioSimulationSystems.h +++ b/src/domain/ScenarioSimulationSystems.h @@ -1,9 +1,11 @@ #pragma once +#include #include #include #include +#include "domain/ScenarioAuthoring.h" #include "domain/AgentComponents.h" #include "domain/FacilityLayout2D.h" #include "domain/ScenarioResultArtifacts.h" @@ -28,6 +30,14 @@ struct ScenarioSimulationStepResource { double deltaSeconds{0.0}; }; +struct ScenarioLayoutResource { + FacilityLayout2D layout{}; +}; + +struct ScenarioLayoutRevisionResource { + std::uint64_t revision{0}; +}; + struct ScenarioAgentSpatialIndexResource { double cellSize{1.0}; std::unordered_map> cells{}; @@ -59,6 +69,9 @@ std::vector scenarioNearbyAgents( const Point2D& point, double radius); +std::unique_ptr makeScenarioControlSystem( + FacilityLayout2D baseLayout, + std::vector blocks); std::unique_ptr makeScenarioSimulationMotionSystem(FacilityLayout2D layout); std::unique_ptr makeScenarioRiskMetricsSystem(FacilityLayout2D layout); diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index 56b22f1..4f69d3b 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -231,6 +231,53 @@ SC_TEST(ScenarioSimulationMotionSystem_AdvancesAgentsFromStepResource) { SC_EXPECT_NEAR(frame.agents.front().velocity.x, 1.0, 1e-9); } +SC_TEST(ScenarioControlSystem_BlocksConnectionsUsingScenarioClock) { + auto layout = straightExitLayout(); + safecrowd::domain::ConnectionBlockDraft block; + block.id = "block-1"; + block.connectionId = "room-exit"; + block.intervals = { + {.startSeconds = 0.0, .endSeconds = 2.0}, + }; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 21, + }); + runtime.addSystem( + safecrowd::domain::makeScenarioControlSystem(layout, {block}), + {.phase = safecrowd::engine::UpdatePhase::PreSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationClockResource{ + .elapsedSeconds = 1.0, + .timeLimitSeconds = 10.0, + .complete = false, + }); + runtime.stepFrame(0.0); + + { + const auto& scenarioLayout = runtime.world().resources().get().layout; + SC_EXPECT_EQ(scenarioLayout.connections.size(), std::size_t{1}); + SC_EXPECT_EQ(scenarioLayout.connections.front().directionality, safecrowd::domain::TravelDirection::Closed); + SC_EXPECT_EQ(scenarioLayout.barriers.size(), std::size_t{1}); + } + + auto& clock = runtime.world().resources().get(); + clock.elapsedSeconds = 3.0; + runtime.stepFrame(0.0); + + { + const auto& scenarioLayout = runtime.world().resources().get().layout; + SC_EXPECT_EQ(scenarioLayout.connections.size(), std::size_t{1}); + SC_EXPECT_EQ(scenarioLayout.connections.front().directionality, safecrowd::domain::TravelDirection::Bidirectional); + SC_EXPECT_EQ(scenarioLayout.barriers.size(), std::size_t{0}); + } +} + SC_TEST(ScenarioRiskMetricsSystem_PublishesStalledHotspotAndBottleneckMetrics) { std::vector seeds; for (int index = 0; index < 5; ++index) {