diff --git a/src/application/ProjectPersistence.cpp b/src/application/ProjectPersistence.cpp index 7448820..8300481 100644 --- a/src/application/ProjectPersistence.cpp +++ b/src/application/ProjectPersistence.cpp @@ -905,6 +905,10 @@ QJsonObject routeGuidanceToJson(const safecrowd::domain::RouteGuidanceDraft& gui object["periods"] = periods; object["guidedExitZoneId"] = QString::fromStdString(guidance.guidedExitZoneId); object["installConnectionId"] = QString::fromStdString(guidance.installConnectionId); + object["installFloorId"] = QString::fromStdString(guidance.installFloorId); + object["installZoneId"] = QString::fromStdString(guidance.installZoneId); + object["installPositionX"] = guidance.installPosition.x; + object["installPositionY"] = guidance.installPosition.y; object["baseComplianceRate"] = guidance.baseComplianceRate; object["guidanceStrength"] = guidance.guidanceStrength; object["maxDetourMeters"] = guidance.maxDetourMeters; @@ -933,6 +937,12 @@ safecrowd::domain::RouteGuidanceDraft routeGuidanceFromJson(const QJsonObject& o } guidance.guidedExitZoneId = object.value("guidedExitZoneId").toString().toStdString(); guidance.installConnectionId = object.value("installConnectionId").toString().toStdString(); + guidance.installFloorId = object.value("installFloorId").toString().toStdString(); + guidance.installZoneId = object.value("installZoneId").toString().toStdString(); + guidance.installPosition = { + .x = object.value("installPositionX").toDouble(0.0), + .y = object.value("installPositionY").toDouble(0.0), + }; guidance.baseComplianceRate = object.value("baseComplianceRate").toDouble(0.5); guidance.guidanceStrength = object.value("guidanceStrength").toDouble(0.55); guidance.maxDetourMeters = object.value("maxDetourMeters").toDouble(20.0); diff --git a/src/application/ScenarioAuthoringWidget.cpp b/src/application/ScenarioAuthoringWidget.cpp index 893f560..5447a96 100644 --- a/src/application/ScenarioAuthoringWidget.cpp +++ b/src/application/ScenarioAuthoringWidget.cpp @@ -193,6 +193,29 @@ QString hazardPositionSummary(const safecrowd::domain::EnvironmentHazardDraft& h return QString("(%1, %2)").arg(hazard.position.x, 0, 'f', 1).arg(hazard.position.y, 0, 'f', 1); } +QString routeGuidancePositionSummary(const safecrowd::domain::RouteGuidanceDraft& guidance) { + if (guidance.installFloorId.empty() && guidance.installZoneId.empty()) { + return guidance.installConnectionId.empty() + ? QStringLiteral("Derived from target") + : QStringLiteral("Derived from install anchor"); + } + return QString("(%1, %2)") + .arg(guidance.installPosition.x, 0, 'f', 1) + .arg(guidance.installPosition.y, 0, 'f', 1); +} + +QString routeGuidanceInstallLabel( + const safecrowd::domain::FacilityLayout2D& layout, + const safecrowd::domain::RouteGuidanceDraft& guidance) { + if (!guidance.installConnectionId.empty()) { + return connectionLabelForId(layout, guidance.installConnectionId); + } + if (!guidance.installZoneId.empty()) { + return zoneName(layout, guidance.installZoneId); + } + return QStringLiteral("Unassigned"); +} + bool hasSmokeHazard(const safecrowd::domain::EnvironmentState& environment) { return std::any_of(environment.hazards.begin(), environment.hazards.end(), [](const auto& hazard) { return hazard.kind == safecrowd::domain::EnvironmentHazardKind::Smoke; @@ -958,12 +981,11 @@ std::vector buildEventsTree( nodes.reserve(routeGuidances.size()); for (const auto& guidance : routeGuidances) { const auto guidanceId = QString::fromStdString(guidance.id); - const auto doorLabel = guidance.installConnectionId.empty() - ? QString{} - : connectionLabelForId(layout, guidance.installConnectionId); + const auto installLabel = routeGuidanceInstallLabel(layout, guidance); const auto exitLabel = guidance.guidedExitZoneId.empty() ? QStringLiteral("Nearest exit") : zoneName(layout, guidance.guidedExitZoneId); + const auto locationLabel = routeGuidancePositionSummary(guidance); QString periodSummary = QStringLiteral("Always"); if (!guidance.periods.empty()) { periodSummary = blockScheduleSummary(safecrowd::domain::ConnectionBlockDraft{ @@ -982,26 +1004,28 @@ std::vector buildEventsTree( } std::vector children; - children.reserve(doorLabel.isEmpty() ? 2u : 3u); + children.reserve(4u); children.push_back({ .label = QString("Exit - %1").arg(exitLabel), .id = QString("%1/exit").arg(guidanceId), }); - if (!doorLabel.isEmpty()) { - children.push_back({ - .label = QString("Door - %1").arg(doorLabel), - .id = QString("%1/door").arg(guidanceId), - }); - } + children.push_back({ + .label = QString("Install - %1").arg(installLabel), + .id = QString("%1/install").arg(guidanceId), + }); + children.push_back({ + .label = QString("Location - %1").arg(locationLabel), + .id = QString("%1/location").arg(guidanceId), + }); children.push_back({ .label = QString("Period - %1").arg(periodSummary), .id = QString("%1/period").arg(guidanceId), }); nodes.push_back({ - .label = QString("Guidance - %1").arg(doorLabel.isEmpty() ? exitLabel : doorLabel), + .label = QString("Guidance - %1").arg(installLabel), .id = guidanceId, - .detail = QString("Period: %1").arg(periodSummary), + .detail = QString("Location: %1 / Period: %2").arg(locationLabel, periodSummary), .children = std::move(children), .expanded = true, }); diff --git a/src/application/ScenarioCanvasWidget.cpp b/src/application/ScenarioCanvasWidget.cpp index 961c394..5fbe614 100644 --- a/src/application/ScenarioCanvasWidget.cpp +++ b/src/application/ScenarioCanvasWidget.cpp @@ -49,6 +49,7 @@ constexpr double kDefaultInitialSpeed = 1.3; constexpr double kOccupantMarkerRadius = 5.0; constexpr double kOccupantWorldRadius = 0.25; constexpr double kOccupantMinSpacing = kOccupantWorldRadius * 2.0; +constexpr double kGuidancePlacementBarrierClearance = 0.35; constexpr int kMaxSourceOccupantCount = 5000; constexpr int kDefaultSourceAgentsPerSpawn = 1; constexpr double kDefaultSourceStartSeconds = 0.0; @@ -171,28 +172,14 @@ safecrowd::domain::Point2D connectionMarkerCenter(const safecrowd::domain::Conne }; } -std::string pickNearestExitZoneIdForConnection( - const safecrowd::domain::FacilityLayout2D& layout, - const safecrowd::domain::Connection2D& connection) { - const auto pickAdjacentExit = [&]() -> std::string { - if (connection.fromZoneId.empty() && connection.toZoneId.empty()) { - return {}; - } - const auto exitIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { - if (zone.kind != safecrowd::domain::ZoneKind::Exit) { - return false; - } - return zone.id == connection.fromZoneId || zone.id == connection.toZoneId; - }); - return exitIt == layout.zones.end() ? std::string{} : exitIt->id; - }; - - if (auto adjacent = pickAdjacentExit(); !adjacent.empty()) { - return adjacent; - } - - const auto doorCenter = connectionMarkerCenter(connection); +bool hasExplicitGuidanceInstallPosition(const safecrowd::domain::RouteGuidanceDraft& guidance) { + return !guidance.installFloorId.empty() || !guidance.installZoneId.empty(); +} +std::string pickNearestExitZoneIdForPoint( + const safecrowd::domain::FacilityLayout2D& layout, + const safecrowd::domain::Point2D& point, + const std::string& preferredFloorId) { const auto pickNearest = [&](bool sameFloorOnly) -> std::string { double bestDistanceSq = std::numeric_limits::infinity(); const safecrowd::domain::Zone2D* bestZone = nullptr; @@ -200,16 +187,16 @@ std::string pickNearestExitZoneIdForConnection( if (zone.kind != safecrowd::domain::ZoneKind::Exit) { continue; } - if (sameFloorOnly && !connection.floorId.empty() && !zone.floorId.empty() && zone.floorId != connection.floorId) { + if (sameFloorOnly && !preferredFloorId.empty() && !zone.floorId.empty() && zone.floorId != preferredFloorId) { continue; } const auto exitCenter = polygonCenter(zone.area); - const auto dx = exitCenter.x - doorCenter.x; - const auto dy = exitCenter.y - doorCenter.y; - const auto d2 = (dx * dx) + (dy * dy); - if (d2 < bestDistanceSq) { - bestDistanceSq = d2; + const auto dx = exitCenter.x - point.x; + const auto dy = exitCenter.y - point.y; + const auto distanceSq = (dx * dx) + (dy * dy); + if (distanceSq < bestDistanceSq) { + bestDistanceSq = distanceSq; bestZone = &zone; } } @@ -222,6 +209,29 @@ std::string pickNearestExitZoneIdForConnection( return pickNearest(false); } +std::string pickNearestExitZoneIdForConnection( + const safecrowd::domain::FacilityLayout2D& layout, + const safecrowd::domain::Connection2D& connection) { + const auto pickAdjacentExit = [&]() -> std::string { + if (connection.fromZoneId.empty() && connection.toZoneId.empty()) { + return {}; + } + const auto exitIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { + if (zone.kind != safecrowd::domain::ZoneKind::Exit) { + return false; + } + return zone.id == connection.fromZoneId || zone.id == connection.toZoneId; + }); + return exitIt == layout.zones.end() ? std::string{} : exitIt->id; + }; + + if (auto adjacent = pickAdjacentExit(); !adjacent.empty()) { + return adjacent; + } + + return pickNearestExitZoneIdForPoint(layout, connectionMarkerCenter(connection), connection.floorId); +} + QString formatConnectionBlockTooltip(const safecrowd::domain::ConnectionBlockDraft& block) { if (block.connectionId.empty()) { return {}; @@ -361,6 +371,11 @@ QString formatRouteGuidanceTooltip( text.append(QString("\n %1s~%2s").arg(start, 0, 'f', 1).arg(end, 0, 'f', 1)); } } + if (hasExplicitGuidanceInstallPosition(guidance)) { + text.append(QString("\n Location: (%1, %2)") + .arg(guidance.installPosition.x, 0, 'f', 1) + .arg(guidance.installPosition.y, 0, 'f', 1)); + } text.append(QString("\n Base compliance: %1").arg(std::clamp(guidance.baseComplianceRate, 0.0, 1.0), 0, 'f', 2)); text.append(QString("\n Strength: %1").arg(std::clamp(guidance.guidanceStrength, 0.0, 1.0), 0, 'f', 2)); text.append(QString("\n Max detour:%1m").arg(std::max(0.0, guidance.maxDetourMeters), 0, 'f', 1)); @@ -374,7 +389,12 @@ std::optional routeGuidanceMarkerCenter( const LayoutCanvasTransform& transform, const QString& currentFloorId) { std::optional center; - if (!guidance.installConnectionId.empty()) { + if (hasExplicitGuidanceInstallPosition(guidance)) { + if (!matchesFloor(guidance.installFloorId, currentFloorId)) { + return std::nullopt; + } + center = transform.map(guidance.installPosition); + } else if (!guidance.installConnectionId.empty()) { const auto it = std::find_if(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { return connection.id == guidance.installConnectionId; }); @@ -2871,16 +2891,19 @@ void ScenarioCanvasWidget::addEnvironmentHazardForZone( void ScenarioCanvasWidget::addRouteGuidance(const QPointF& position) { const auto point = unmapPoint(position); const auto zoneId = zoneAt(point); + const safecrowd::domain::Zone2D* clickedZone = nullptr; if (!zoneId.isEmpty()) { const auto zoneIdStd = zoneId.toStdString(); const auto it = std::find_if(layout_.zones.begin(), layout_.zones.end(), [&](const auto& zone) { return zone.id == zoneIdStd; }); - if (it != layout_.zones.end() && it->kind == safecrowd::domain::ZoneKind::Exit) { - addRouteGuidanceForExitZone(*it); - return; + if (it != layout_.zones.end()) { + clickedZone = &(*it); + if (it->kind == safecrowd::domain::ZoneKind::Exit) { + addRouteGuidanceForExitZone(*it); + return; + } } - // If the user clicked inside a non-exit zone, still allow installing guidance by selecting a nearby door. } constexpr double kPickRadiusPixels = 18.0; @@ -2909,12 +2932,69 @@ void ScenarioCanvasWidget::addRouteGuidance(const QPointF& position) { } } - if (connection == nullptr) { - QMessageBox::information(this, "Route guidance", "Click an exit zone or a door to install guidance."); + if (connection != nullptr) { + addRouteGuidanceForConnection(*connection); + return; + } + + if (clickedZone != nullptr) { + addRouteGuidanceForZonePosition(*clickedZone, point); return; } - addRouteGuidanceForConnection(*connection); + QMessageBox::information(this, "Route guidance", "Click a walkable room area, exit zone, or a door to install guidance."); +} + +void ScenarioCanvasWidget::addRouteGuidanceForZonePosition( + const safecrowd::domain::Zone2D& zone, + safecrowd::domain::Point2D position) { + if (!matchesFloor(zone.floorId, currentFloorId_)) { + return; + } + if (zone.kind == safecrowd::domain::ZoneKind::Exit) { + addRouteGuidanceForExitZone(zone); + return; + } + if (!pointInPolygon(zone.area, position)) { + QMessageBox::information(this, "Route guidance", "Click inside a walkable room area to place guidance."); + return; + } + + const auto floorId = zone.floorId.empty() ? currentFloorId_.toStdString() : zone.floorId; + if (!safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout_, + position, + floorId, + kGuidancePlacementBarrierClearance)) { + QMessageBox::information( + this, + "Route guidance", + "Guidance must be placed inside walkable room space and not too close to walls."); + return; + } + + const auto exitZoneId = pickNearestExitZoneIdForPoint(layout_, position, floorId); + if (exitZoneId.empty()) { + QMessageBox::information(this, "Route guidance", "Could not find a reachable exit target for this guidance."); + return; + } + + safecrowd::domain::RouteGuidanceDraft draft; + draft.id = nextRouteGuidanceId().toStdString(); + draft.startSeconds = 0.0; + draft.endSeconds = 0.0; + draft.periods.clear(); + draft.guidedExitZoneId = exitZoneId; + draft.installConnectionId.clear(); + draft.installFloorId = floorId; + draft.installZoneId = zone.id; + draft.installPosition = position; + draft.baseComplianceRate = 0.5; + draft.guidanceStrength = 0.55; + draft.maxDetourMeters = 20.0; + routeGuidances_.push_back(std::move(draft)); + emitRouteGuidancesChanged(); + update(); } void ScenarioCanvasWidget::addRouteGuidanceForExitZone(const safecrowd::domain::Zone2D& zone) { @@ -2924,7 +3004,9 @@ void ScenarioCanvasWidget::addRouteGuidanceForExitZone(const safecrowd::domain:: } for (const auto& existing : routeGuidances_) { - if (existing.installConnectionId.empty() && existing.guidedExitZoneId == zone.id) { + if (existing.installConnectionId.empty() + && existing.guidedExitZoneId == zone.id + && (existing.installZoneId.empty() || existing.installZoneId == zone.id)) { QMessageBox::information(this, "Route guidance", "Guidance is already installed on this exit."); return; } @@ -2937,6 +3019,9 @@ void ScenarioCanvasWidget::addRouteGuidanceForExitZone(const safecrowd::domain:: draft.periods.clear(); draft.guidedExitZoneId = zone.id; draft.installConnectionId.clear(); + draft.installFloorId = zone.floorId.empty() ? currentFloorId_.toStdString() : zone.floorId; + draft.installZoneId = zone.id; + draft.installPosition = representativePointInPolygon(zone.area).value_or(polygonCenter(zone.area)); draft.baseComplianceRate = 0.5; draft.guidanceStrength = 0.55; draft.maxDetourMeters = 20.0; @@ -2968,6 +3053,9 @@ void ScenarioCanvasWidget::addRouteGuidanceForConnection(const safecrowd::domain draft.periods.clear(); draft.guidedExitZoneId = exitZoneId; draft.installConnectionId = connection.id; + draft.installFloorId = connection.floorId.empty() ? currentFloorId_.toStdString() : connection.floorId; + draft.installZoneId.clear(); + draft.installPosition = connectionMarkerCenter(connection); draft.baseComplianceRate = 0.5; draft.guidanceStrength = 0.55; draft.maxDetourMeters = 20.0; diff --git a/src/application/ScenarioCanvasWidget.h b/src/application/ScenarioCanvasWidget.h index 5bba600..2866073 100644 --- a/src/application/ScenarioCanvasWidget.h +++ b/src/application/ScenarioCanvasWidget.h @@ -133,6 +133,9 @@ class ScenarioCanvasWidget : public QWidget { safecrowd::domain::Point2D position, safecrowd::domain::EnvironmentHazardKind kind); void addRouteGuidance(const QPointF& position); + void addRouteGuidanceForZonePosition( + const safecrowd::domain::Zone2D& zone, + safecrowd::domain::Point2D position); void addRouteGuidanceForExitZone(const safecrowd::domain::Zone2D& zone); void addRouteGuidanceForConnection(const safecrowd::domain::Connection2D& connection); void openRouteGuidanceEditor(const QString& guidanceId, const QPoint& screenPosition); diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index 2f50fdf..ef92777 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -151,6 +151,10 @@ safecrowd::domain::Point2D polygonCenter(const safecrowd::domain::Polygon2D& pol return {.x = x / count, .y = y / count}; } +bool hasExplicitGuidanceInstallPosition(const safecrowd::domain::RouteGuidanceDraft& guidance) { + return !guidance.installFloorId.empty() || !guidance.installZoneId.empty(); +} + QColor densityHeatmapColor(double ratio, int alpha) { const auto t = std::clamp(ratio, 0.0, 1.0); if (t < 0.22) { @@ -244,6 +248,11 @@ QString formatRouteGuidanceTooltip(const safecrowd::domain::RouteGuidanceDraft& text.append(QString("\n %1s~%2s").arg(start, 0, 'f', 1).arg(end, 0, 'f', 1)); } } + if (hasExplicitGuidanceInstallPosition(guidance)) { + text.append(QString("\n Location: (%1, %2)") + .arg(guidance.installPosition.x, 0, 'f', 1) + .arg(guidance.installPosition.y, 0, 'f', 1)); + } text.append(QString("\n Base compliance: %1").arg(std::clamp(guidance.baseComplianceRate, 0.0, 1.0), 0, 'f', 2)); text.append(QString("\n Strength: %1").arg(std::clamp(guidance.guidanceStrength, 0.0, 1.0), 0, 'f', 2)); text.append(QString("\n Max detour:%1m").arg(std::max(0.0, guidance.maxDetourMeters), 0, 'f', 1)); @@ -329,12 +338,10 @@ struct ActiveRouteGuidanceSelection { double endSeconds{0.0}; }; -std::optional activeRouteGuidanceSelection( +std::vector activeRouteGuidanceSelections( const std::vector& guidances, double elapsedSeconds) { - std::optional best; - double bestStart = -1.0; - + std::vector active; for (std::size_t guidanceIndex = 0; guidanceIndex < guidances.size(); ++guidanceIndex) { const auto& guidance = guidances[guidanceIndex]; if (guidance.periods.empty()) { @@ -343,10 +350,12 @@ std::optional activeRouteGuidanceSelection( if (elapsedSeconds + 1e-9 < start || elapsedSeconds > end + 1e-9) { continue; } - if (!best.has_value() || start >= bestStart) { - bestStart = start; - best = ActiveRouteGuidanceSelection{.guidanceIndex = guidanceIndex, .periodIndex = 0, .startSeconds = start, .endSeconds = end}; - } + active.push_back(ActiveRouteGuidanceSelection{ + .guidanceIndex = guidanceIndex, + .periodIndex = 0, + .startSeconds = start, + .endSeconds = end, + }); continue; } @@ -360,14 +369,15 @@ std::optional activeRouteGuidanceSelection( if (elapsedSeconds > end + 1e-9) { continue; } - if (!best.has_value() || start >= bestStart) { - bestStart = start; - best = ActiveRouteGuidanceSelection{.guidanceIndex = guidanceIndex, .periodIndex = periodIndex, .startSeconds = start, .endSeconds = end}; - } + active.push_back(ActiveRouteGuidanceSelection{ + .guidanceIndex = guidanceIndex, + .periodIndex = periodIndex, + .startSeconds = start, + .endSeconds = end, + }); } } - - return best; + return active; } std::optional routeGuidanceMarkerCenter( @@ -378,7 +388,12 @@ std::optional routeGuidanceMarkerCenter( const std::string& currentFloorId, double elapsedSeconds) { QPointF center; - if (!guidance.installConnectionId.empty()) { + if (hasExplicitGuidanceInstallPosition(guidance)) { + if (!matchesFloor(guidance.installFloorId, currentFloorId)) { + return std::nullopt; + } + center = transform.map(guidance.installPosition); + } else if (!guidance.installConnectionId.empty()) { const auto it = std::find_if(layout.connections.begin(), layout.connections.end(), [&](const auto& connection) { return connection.id == guidance.installConnectionId; }); @@ -458,29 +473,31 @@ std::optional hoveredActiveRouteGuidanceIndex( const QPointF& screenPosition) { constexpr double kHoverRadiusPixels = 14.0; - const auto active = activeRouteGuidanceSelection(guidances, elapsedSeconds); - if (!active.has_value()) { - return std::nullopt; - } - const auto& guidance = guidances[active->guidanceIndex]; - const auto center = routeGuidanceMarkerCenter( - layout, - guidance, - blocks, - transform, - currentFloorId, - elapsedSeconds); - if (!center.has_value()) { - return std::nullopt; - } + const auto activeSelections = activeRouteGuidanceSelections(guidances, elapsedSeconds); + std::optional closestIndex; + double closestDistanceSq = kHoverRadiusPixels * kHoverRadiusPixels; + for (const auto& active : activeSelections) { + const auto& guidance = guidances[active.guidanceIndex]; + const auto center = routeGuidanceMarkerCenter( + layout, + guidance, + blocks, + transform, + currentFloorId, + elapsedSeconds); + if (!center.has_value()) { + continue; + } - const auto dx = center->x() - screenPosition.x(); - const auto dy = center->y() - screenPosition.y(); - const auto distanceSq = (dx * dx) + (dy * dy); - if (distanceSq <= kHoverRadiusPixels * kHoverRadiusPixels) { - return active->guidanceIndex; + 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 = active.guidanceIndex; + } } - return std::nullopt; + return closestIndex; } } // namespace @@ -1146,43 +1163,47 @@ void SimulationCanvasWidget::drawEnvironmentHazardOverlay(QPainter& painter, con void SimulationCanvasWidget::drawRouteGuidanceOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const { const auto elapsedSeconds = std::max(0.0, frame_.elapsedSeconds); - const auto active = activeRouteGuidanceSelection(routeGuidances_, elapsedSeconds); - if (!active.has_value()) { - return; - } - - const auto& guidance = routeGuidances_[active->guidanceIndex]; - const auto center = routeGuidanceMarkerCenter( - layout_, - guidance, - connectionBlocks_, - transform, - currentFloorId_, - elapsedSeconds); - if (!center.has_value()) { + const auto activeSelections = activeRouteGuidanceSelections(routeGuidances_, elapsedSeconds); + if (activeSelections.empty()) { return; } painter.save(); painter.setPen(Qt::NoPen); - painter.setBrush(QColor("#1f5fae")); - const double r = 10.0; - painter.drawEllipse(*center, r, r); + for (const auto& active : activeSelections) { + const auto& guidance = routeGuidances_[active.guidanceIndex]; + const auto center = routeGuidanceMarkerCenter( + layout_, + guidance, + connectionBlocks_, + transform, + currentFloorId_, + elapsedSeconds); + if (!center.has_value()) { + continue; + } - painter.save(); - painter.translate(*center); - painter.rotate(-25.0); - painter.translate(-(*center)); - painter.setBrush(Qt::white); - painter.drawRoundedRect(QRectF(center->x() - 1.8, center->y() - 7.0, 3.6, 10.5), 1.4, 1.4); - painter.drawRoundedRect(QRectF(center->x() - 1.5, center->y() + 2.2, 3.0, 5.2), 1.2, 1.2); - painter.restore(); + painter.setBrush(QColor("#1f5fae")); - painter.setPen(QPen(Qt::white, 1.7, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); - painter.drawLine(QPointF(center->x() + 5.3, center->y() - 5.0), QPointF(center->x() + 8.2, center->y() - 7.7)); - painter.drawLine(QPointF(center->x() + 6.3, center->y() - 2.0), QPointF(center->x() + 9.2, center->y() - 2.8)); - painter.drawLine(QPointF(center->x() + 3.7, center->y() - 7.2), QPointF(center->x() + 4.8, center->y() - 9.8)); + const double r = 10.0; + painter.drawEllipse(*center, r, r); + + painter.save(); + painter.translate(*center); + painter.rotate(-25.0); + painter.translate(-(*center)); + painter.setBrush(Qt::white); + painter.drawRoundedRect(QRectF(center->x() - 1.8, center->y() - 7.0, 3.6, 10.5), 1.4, 1.4); + painter.drawRoundedRect(QRectF(center->x() - 1.5, center->y() + 2.2, 3.0, 5.2), 1.2, 1.2); + painter.restore(); + + painter.setPen(QPen(Qt::white, 1.7, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + painter.drawLine(QPointF(center->x() + 5.3, center->y() - 5.0), QPointF(center->x() + 8.2, center->y() - 7.7)); + painter.drawLine(QPointF(center->x() + 6.3, center->y() - 2.0), QPointF(center->x() + 9.2, center->y() - 2.8)); + painter.drawLine(QPointF(center->x() + 3.7, center->y() - 7.2), QPointF(center->x() + 4.8, center->y() - 9.8)); + painter.setPen(Qt::NoPen); + } painter.restore(); } diff --git a/src/domain/DemoFixtureService.cpp b/src/domain/DemoFixtureService.cpp index 1292090..feca3fe 100644 --- a/src/domain/DemoFixtureService.cpp +++ b/src/domain/DemoFixtureService.cpp @@ -29,11 +29,14 @@ ScenarioDraft makeTwoFloorEastExitGuidanceAlternative(const ScenarioDraft& basel baseline, "two-floor-guidance", "East exit guidance alternative"); + const auto guidanceEndSeconds = baseline.execution.timeLimitSeconds > 0.0 + ? baseline.execution.timeLimitSeconds + : 180.0; alternative.control.routeGuidances.push_back({ .id = "guidance-east-exit", .startSeconds = 0.0, - .endSeconds = 180.0, - .periods = {{.startSeconds = 0.0, .endSeconds = 180.0}}, + .endSeconds = guidanceEndSeconds, + .periods = {{.startSeconds = 0.0, .endSeconds = guidanceEndSeconds}}, .guidedExitZoneId = Ids::EastExitZoneId, .installConnectionId = Ids::UpperWestTrainingToCorridorConnectionId, .baseComplianceRate = 0.95, diff --git a/src/domain/GeometryQueries.cpp b/src/domain/GeometryQueries.cpp index ac1b6e4..e202b07 100644 --- a/src/domain/GeometryQueries.cpp +++ b/src/domain/GeometryQueries.cpp @@ -50,6 +50,18 @@ Point2D polygonCenter(const Polygon2D& polygon) { return {.x = x / count, .y = y / count}; } +bool pointWithinSegmentBounds( + const Point2D& point, + const Point2D& start, + const Point2D& end, + double margin) { + const auto minX = std::min(start.x, end.x) - margin; + const auto maxX = std::max(start.x, end.x) + margin; + const auto minY = std::min(start.y, end.y) - margin; + const auto maxY = std::max(start.y, end.y) + margin; + return point.x >= minX && point.x <= maxX && point.y >= minY && point.y <= maxY; +} + void appendRingYValues(const std::vector& ring, std::vector& values) { for (const auto& point : ring) { values.push_back(point.y); @@ -242,4 +254,58 @@ std::optional representativePointInPolygon(const Polygon2D& polygon) { return gridRepresentativePoint(polygon); } +bool pointHasBarrierClearance( + const FacilityLayout2D& layout, + const Point2D& point, + const std::string& floorId, + double clearance) { + if (clearance <= kGeometryEpsilon) { + return true; + } + + for (const auto& barrier : layout.barriers) { + if (!floorId.empty() && !barrier.floorId.empty() && barrier.floorId != floorId) { + continue; + } + if (!barrier.blocksMovement || barrier.geometry.vertices.size() < 2) { + continue; + } + + const auto& vertices = barrier.geometry.vertices; + for (std::size_t index = 0; index + 1 < vertices.size(); ++index) { + if (pointWithinSegmentBounds(point, vertices[index], vertices[index + 1], clearance) + && distancePointToSegment(point, vertices[index], vertices[index + 1]) < clearance) { + return false; + } + } + if (barrier.geometry.closed + && pointWithinSegmentBounds(point, vertices.back(), vertices.front(), clearance) + && distancePointToSegment(point, vertices.back(), vertices.front()) < clearance) { + return false; + } + } + return true; +} + +bool pointInsideWalkableZoneWithClearance( + const FacilityLayout2D& layout, + const Point2D& point, + const std::string& floorId, + double clearance) { + const auto zoneIt = std::find_if(layout.zones.begin(), layout.zones.end(), [&](const auto& zone) { + if (!floorId.empty() && !zone.floorId.empty() && zone.floorId != floorId) { + return false; + } + return pointInPolygon(zone.area, point); + }); + if (zoneIt == layout.zones.end()) { + return false; + } + if (clearance > kGeometryEpsilon + && distanceToPolygonBoundary(zoneIt->area, point) < clearance) { + return false; + } + return pointHasBarrierClearance(layout, point, floorId, clearance); +} + } // namespace safecrowd::domain diff --git a/src/domain/GeometryQueries.h b/src/domain/GeometryQueries.h index 0a0d4e0..e9d4a37 100644 --- a/src/domain/GeometryQueries.h +++ b/src/domain/GeometryQueries.h @@ -3,6 +3,7 @@ #include #include +#include "domain/FacilityLayout2D.h" #include "domain/Geometry2D.h" namespace safecrowd::domain { @@ -12,5 +13,15 @@ bool pointInPolygon(const Polygon2D& polygon, const Point2D& point); double distancePointToSegment(const Point2D& point, const Point2D& start, const Point2D& end); double distanceToPolygonBoundary(const Polygon2D& polygon, const Point2D& point); std::optional representativePointInPolygon(const Polygon2D& polygon); +bool pointHasBarrierClearance( + const FacilityLayout2D& layout, + const Point2D& point, + const std::string& floorId, + double clearance); +bool pointInsideWalkableZoneWithClearance( + const FacilityLayout2D& layout, + const Point2D& point, + const std::string& floorId, + double clearance); } // namespace safecrowd::domain diff --git a/src/domain/ScenarioAuthoring.cpp b/src/domain/ScenarioAuthoring.cpp index 1a0fa0d..dd57d41 100644 --- a/src/domain/ScenarioAuthoring.cpp +++ b/src/domain/ScenarioAuthoring.cpp @@ -163,6 +163,9 @@ bool routeGuidancesEqual(const std::vector& lhs, || lhs[i].endSeconds != rhs[i].endSeconds || lhs[i].guidedExitZoneId != rhs[i].guidedExitZoneId || lhs[i].installConnectionId != rhs[i].installConnectionId + || lhs[i].installFloorId != rhs[i].installFloorId + || lhs[i].installZoneId != rhs[i].installZoneId + || !pointsEqual(lhs[i].installPosition, rhs[i].installPosition) || lhs[i].baseComplianceRate != rhs[i].baseComplianceRate || lhs[i].guidanceStrength != rhs[i].guidanceStrength || lhs[i].maxDetourMeters != rhs[i].maxDetourMeters diff --git a/src/domain/ScenarioAuthoring.h b/src/domain/ScenarioAuthoring.h index e1a3837..8155f6c 100644 --- a/src/domain/ScenarioAuthoring.h +++ b/src/domain/ScenarioAuthoring.h @@ -72,6 +72,9 @@ struct RouteGuidanceDraft { std::vector periods{}; std::string guidedExitZoneId{}; std::string installConnectionId{}; + std::string installFloorId{}; + std::string installZoneId{}; + Point2D installPosition{}; double baseComplianceRate{0.5}; double guidanceStrength{0.55}; double maxDetourMeters{20.0}; diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index e8ae4b6..a24a80d 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -78,7 +78,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { ? &resources.get() : nullptr; - applyRouteGuidance(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed); + applyRouteGuidance(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed, sharedSpatialIndex); advanceRoutesForCurrentZones(query, activeEntities_, layoutCache); replanBlockedExitRoutes(query, activeEntities_, layoutCache, clock.elapsedSeconds, layoutRevision, reactions); advanceRoutesForWaypointProgress(query, 0.0, activeEntities_, layoutCache); @@ -560,6 +560,10 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return sameFloor(fromFloorId, floorId) || sameFloor(toFloorId, floorId); } + bool guidanceHasInstallPosition(const RouteGuidanceDraft& guidance) const { + return !guidance.installFloorId.empty() || !guidance.installZoneId.empty(); + } + bool agentCanSeeGuidanceAtInstallConnection( const ScenarioLayoutCacheResource& layoutCache, const RouteGuidanceDraft& guidance, @@ -592,6 +596,31 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return distanceBetween(position.value, closestOnInstall) <= visibilityDistance; } + bool agentCanSeeGuidanceAtInstallPosition( + const ScenarioLayoutCacheResource& layoutCache, + const RouteGuidanceDraft& guidance, + const Position& position, + const Agent& agent, + const EvacuationRoute& route) const { + if (!guidanceHasInstallPosition(guidance) || !sameFloor(guidance.installFloorId, route.currentFloorId)) { + return false; + } + + const auto currentZoneId = zoneAt(layoutCache, position.value, route.currentFloorId); + if (!guidance.installZoneId.empty() && currentZoneId != guidance.installZoneId) { + return false; + } + + constexpr double kGuidanceVisibilityDistanceMeters = 2.5; + if (distanceBetween(position.value, guidance.installPosition) > kGuidanceVisibilityDistanceMeters) { + return false; + } + + const auto& floorLayout = cachedLayoutForFloor(layoutCache, route.currentFloorId); + const auto clearance = std::max(0.08, static_cast(agent.radius) + 0.05); + return lineOfSightClear(floorLayout, position.value, guidance.installPosition, clearance); + } + const Connection2D* nextBlockedConnection( const ScenarioLayoutCacheResource& layoutCache, const EvacuationRoute& route) const { @@ -1028,29 +1057,23 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { double endSeconds{0.0}; }; - std::optional activeRouteGuidance(double elapsedSeconds) const { - std::optional best; - double bestStart = -1.0; - + std::vector activeRouteGuidances(double elapsedSeconds) const { + std::vector active; for (std::size_t guidanceIndex = 0; guidanceIndex < routeGuidances_.size(); ++guidanceIndex) { const auto& guidance = routeGuidances_[guidanceIndex]; if (guidance.periods.empty()) { - // No periods configured => always active (like connection blocks with no intervals). const double start = 0.0; const double end = 1e18; if (elapsedSeconds + 1e-9 < start || elapsedSeconds > end + 1e-9) { continue; } - if (!best.has_value() || start >= bestStart) { - bestStart = start; - best = ActiveRouteGuidance{ - .guidance = &guidance, - .guidanceIndex = guidanceIndex, - .periodIndex = 0, - .startSeconds = start, - .endSeconds = end, - }; - } + active.push_back(ActiveRouteGuidance{ + .guidance = &guidance, + .guidanceIndex = guidanceIndex, + .periodIndex = 0, + .startSeconds = start, + .endSeconds = end, + }); continue; } @@ -1064,19 +1087,86 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { if (elapsedSeconds > end + 1e-9) { continue; } - if (!best.has_value() || start >= bestStart) { - bestStart = start; - best = ActiveRouteGuidance{ - .guidance = &guidance, - .guidanceIndex = guidanceIndex, - .periodIndex = index, - .startSeconds = start, - .endSeconds = end, - }; - } + active.push_back(ActiveRouteGuidance{ + .guidance = &guidance, + .guidanceIndex = guidanceIndex, + .periodIndex = index, + .startSeconds = start, + .endSeconds = end, + }); } } + return active; + } + std::string activeRouteGuidanceKey(const ActiveRouteGuidance& active) const { + if (active.guidance == nullptr) { + return {}; + } + auto id = active.guidance->id; + if (id.empty()) { + id = "route-guidance:"; + id.append(std::to_string(active.guidanceIndex)); + } + if (!active.guidance->periods.empty()) { + id.append(":p"); + id.append(std::to_string(active.periodIndex)); + } + return id; + } + + std::string activeRouteGuidanceSignature(const std::vector& activeGuidances) const { + std::string signature; + for (const auto& active : activeGuidances) { + if (!signature.empty()) { + signature.push_back('\x1f'); + } + signature.append(activeRouteGuidanceKey(active)); + } + return signature; + } + + const ActiveRouteGuidance* matchingActiveGuidance( + const std::vector& activeGuidances, + const std::string& guidanceEventId) const { + if (guidanceEventId.empty()) { + return nullptr; + } + for (const auto& active : activeGuidances) { + if (activeRouteGuidanceKey(active) == guidanceEventId) { + return &active; + } + } + return nullptr; + } + + const ActiveRouteGuidance* applicableActiveGuidance( + const std::vector& activeGuidances, + const ScenarioLayoutCacheResource& layoutCache, + const Position& position, + const Agent& agent, + const EvacuationRoute& route) const { + const ActiveRouteGuidance* best = nullptr; + double bestStart = -1.0; + for (const auto& active : activeGuidances) { + if (active.guidance == nullptr) { + continue; + } + const auto& guidance = *active.guidance; + bool applicable = true; + if (!guidance.installConnectionId.empty()) { + applicable = agentCanSeeGuidanceAtInstallConnection(layoutCache, guidance, position, agent, route); + } else if (guidanceHasInstallPosition(guidance)) { + applicable = agentCanSeeGuidanceAtInstallPosition(layoutCache, guidance, position, agent, route); + } + if (!applicable) { + continue; + } + if (best == nullptr || active.startSeconds >= bestStart) { + bestStart = active.startSeconds; + best = &active; + } + } return best; } @@ -1092,6 +1182,87 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return result->distance; } + double guidancePriorityRadiusMeters( + const ScenarioLayoutCacheResource& layoutCache, + const RouteGuidanceDraft& guidance) const { + if (!guidance.installConnectionId.empty()) { + if (const auto* connection = findConnectionById(layoutCache, guidance.installConnectionId); + connection != nullptr) { + return std::max(2.0, (connection->effectiveWidth * 0.5) + 0.5); + } + return 2.0; + } + if (guidanceHasInstallPosition(guidance)) { + return 2.5; + } + return 0.0; + } + + std::vector prioritizedGuidanceEntities( + engine::WorldQuery& query, + const std::vector& activeGuidances, + const ScenarioLayoutCacheResource& layoutCache, + const ScenarioAgentSpatialIndexResource* sharedSpatialIndex, + const AgentSpatialIndex* localSpatialIndex) const { + std::vector prioritized; + + auto appendNearby = [&](const Point2D& anchor, const std::string& floorId, double radius) { + if (radius <= 0.0) { + return; + } + + std::vector nearby; + if (sharedSpatialIndex != nullptr) { + if (floorId.empty()) { + for (const auto& [candidateFloorId, _] : sharedSpatialIndex->cellsByFloor) { + const auto floorNearby = scenarioNearbyAgents(query, *sharedSpatialIndex, anchor, candidateFloorId, radius); + nearby.insert(nearby.end(), floorNearby.begin(), floorNearby.end()); + } + } else { + nearby = scenarioNearbyAgents(query, *sharedSpatialIndex, anchor, floorId, radius); + } + } else if (localSpatialIndex != nullptr) { + if (floorId.empty()) { + nearby = nearbyAgents(query, *localSpatialIndex, anchor, radius); + } else { + nearby = nearbyAgents(query, *localSpatialIndex, anchor, floorId, radius); + } + } + prioritized.insert(prioritized.end(), nearby.begin(), nearby.end()); + }; + + for (const auto& active : activeGuidances) { + if (active.guidance == nullptr) { + continue; + } + + const auto& guidance = *active.guidance; + const auto radius = guidancePriorityRadiusMeters(layoutCache, guidance); + if (!guidance.installConnectionId.empty()) { + const auto* connection = findConnectionById(layoutCache, guidance.installConnectionId); + if (connection == nullptr) { + continue; + } + const auto floorId = connection->floorId.empty() ? guidance.installFloorId : connection->floorId; + appendNearby(midpoint(connection->centerSpan), floorId, radius); + continue; + } + + if (guidanceHasInstallPosition(guidance)) { + appendNearby(guidance.installPosition, guidance.installFloorId, radius); + } + } + + std::sort(prioritized.begin(), prioritized.end(), [](const auto& lhs, const auto& rhs) { + if (lhs.index != rhs.index) { + return lhs.index < rhs.index; + } + return lhs.generation < rhs.generation; + }); + prioritized.erase(std::unique(prioritized.begin(), prioritized.end()), prioritized.end()); + return prioritized; + } + double complianceProbability( const RouteGuidanceDraft& guidance, const Agent& agent, @@ -1113,169 +1284,214 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return clamp01(sigmoid(score)); } - void applyRouteGuidance( + void applyRouteGuidanceToEntity( engine::WorldQuery& query, - const std::vector& entities, + engine::Entity entity, const ScenarioLayoutCacheResource& layoutCache, + const std::vector& activeGuidances, double elapsedSeconds, - std::uint64_t derivedSeed) { - // Keep this small to avoid frame spikes when guidance toggles. - // Higher values converge faster but may cause noticeable hitching with many agents. - constexpr std::size_t kGuidanceReplanBudgetPerFrame = 50; - - const auto active = activeRouteGuidance(elapsedSeconds); - std::string activeId; - if (active.has_value() && active->guidance != nullptr) { - activeId = active->guidance->id; - if (activeId.empty()) { - activeId = "route-guidance:"; - activeId.append(std::to_string(active->guidanceIndex)); - } - if (!active->guidance->periods.empty()) { - activeId.append(":p"); - activeId.append(std::to_string(active->periodIndex)); - } + std::uint64_t stableSeed) { + const auto& status = query.get(entity); + if (status.evacuated) { + return; } - if (activeId != activeRouteGuidanceId_) { - activeRouteGuidanceId_ = activeId; - guidanceReplanCursor_ = 0; - guidanceReplanSeed_ = derivedSeed; - if (active.has_value() && active->guidance != nullptr) { - guidanceReplanGuidance_ = *active->guidance; - } else { - guidanceReplanGuidance_.reset(); - } - guidanceReplanIdHash_ = fnv1a64(activeId); - guidanceReplanPending_ = true; - } else if (active.has_value() - && active->guidance != nullptr - && !active->guidance->installConnectionId.empty() - && !guidanceReplanPending_) { - guidanceReplanCursor_ = 0; - guidanceReplanPending_ = true; + const auto& position = query.get(entity); + const auto& agent = query.get(entity); + auto& route = query.get(entity); + if (route.originalDestinationZoneId.empty()) { + route.originalDestinationZoneId = route.destinationZoneId; } - if (!guidanceReplanPending_ || guidanceReplanCursor_ >= entities.size()) { + const auto startZoneId = zoneAt(layoutCache, position.value, route.currentFloorId); + if (startZoneId.empty()) { return; } - const auto endIndex = std::min(entities.size(), guidanceReplanCursor_ + kGuidanceReplanBudgetPerFrame); + if (activeGuidances.empty()) { + route.guidanceEventId.clear(); + route.followsGuidance = false; - const RouteGuidanceDraft* activeGuidance = guidanceReplanGuidance_.has_value() ? &*guidanceReplanGuidance_ : nullptr; - const auto activeIdHash = guidanceReplanIdHash_; - const auto stableSeed = guidanceReplanSeed_; + std::string desiredExit = route.originalDestinationZoneId; + if (desiredExit.empty()) { + return; + } - for (std::size_t i = guidanceReplanCursor_; i < endIndex; ++i) { - const auto entity = entities[i]; - const auto& status = query.get(entity); - if (status.evacuated) { - continue; + if (route.destinationZoneId == desiredExit && !route.waypoints.empty()) { + return; } - const auto& position = query.get(entity); - const auto& agent = query.get(entity); - auto& route = query.get(entity); - if (route.originalDestinationZoneId.empty()) { - route.originalDestinationZoneId = route.destinationZoneId; + RoutePlan plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); + if (plan.destinationZoneId.empty()) { + plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); + } + if (plan.destinationZoneId.empty()) { + return; } + replaceRouteWithPlan(route, plan, position.value); + route.nextExitReplanSeconds = elapsedSeconds + 0.25; + return; + } - const auto startZoneId = zoneAt(layoutCache, position.value, route.currentFloorId); - if (startZoneId.empty()) { - continue; + const auto* visibleGuidance = applicableActiveGuidance(activeGuidances, layoutCache, position, agent, route); + if (visibleGuidance != nullptr && visibleGuidance->guidance != nullptr + && (!visibleGuidance->guidance->installConnectionId.empty() + || guidanceHasInstallPosition(*visibleGuidance->guidance))) { + if (route.guidanceEventId == activeRouteGuidanceKey(*visibleGuidance)) { + return; } + } + + const auto* activeGuidance = visibleGuidance; + if (activeGuidance == nullptr) { + if (const auto* retained = matchingActiveGuidance(activeGuidances, route.guidanceEventId); + retained != nullptr + && retained->guidance != nullptr + && (!retained->guidance->installConnectionId.empty() || guidanceHasInstallPosition(*retained->guidance))) { + return; + } + } - if (activeGuidance == nullptr) { + if (activeGuidance == nullptr || activeGuidance->guidance == nullptr) { + if (!route.guidanceEventId.empty() || route.followsGuidance) { route.guidanceEventId.clear(); route.followsGuidance = false; - - std::string desiredExit = route.originalDestinationZoneId; - if (desiredExit.empty()) { - continue; + const auto& desiredExit = route.originalDestinationZoneId; + if (!desiredExit.empty() && (route.destinationZoneId != desiredExit || route.waypoints.empty())) { + auto plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); + if (plan.destinationZoneId.empty()) { + plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); + } + if (!plan.destinationZoneId.empty()) { + replaceRouteWithPlan(route, plan, position.value); + route.nextExitReplanSeconds = elapsedSeconds + 0.25; + } } + } + return; + } - if (route.destinationZoneId == desiredExit && !route.waypoints.empty()) { - continue; - } + const auto& selectedGuidance = *activeGuidance->guidance; + const auto activeId = activeRouteGuidanceKey(*activeGuidance); + const auto activeIdHash = fnv1a64(activeId); - RoutePlan plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); - if (plan.destinationZoneId.empty()) { - plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); - } - if (plan.destinationZoneId.empty()) { - continue; - } - replaceRouteWithPlan(route, plan, position.value); - route.nextExitReplanSeconds = elapsedSeconds + 0.25; - continue; + bool guidedExitValid = false; + if (!selectedGuidance.guidedExitZoneId.empty()) { + if (const auto* exitZone = findCachedZone(layoutCache, selectedGuidance.guidedExitZoneId); + exitZone != nullptr && exitZone->kind == ZoneKind::Exit) { + guidedExitValid = true; } + } - if (!activeGuidance->installConnectionId.empty()) { - if (route.guidanceEventId == activeId) { - continue; - } - if (!agentCanSeeGuidanceAtInstallConnection(layoutCache, *activeGuidance, position, agent, route)) { - continue; - } + double detourMeters = 0.0; + if (guidedExitValid && !route.originalDestinationZoneId.empty()) { + const auto* originalExit = findCachedZone(layoutCache, route.originalDestinationZoneId); + const auto* guidedExit = findCachedZone(layoutCache, selectedGuidance.guidedExitZoneId); + if (originalExit != nullptr && guidedExit != nullptr && originalExit->kind == ZoneKind::Exit + && guidedExit->kind == ZoneKind::Exit) { + const auto originalDistance = distanceBetween(position.value, polygonCenter(originalExit->area)); + const auto guidedDistance = distanceBetween(position.value, polygonCenter(guidedExit->area)); + detourMeters = std::max(0.0, guidedDistance - originalDistance); } + } - bool guidedExitValid = false; - if (!activeGuidance->guidedExitZoneId.empty()) { - if (const auto* exitZone = findCachedZone(layoutCache, activeGuidance->guidedExitZoneId); - exitZone != nullptr && exitZone->kind == ZoneKind::Exit) { - guidedExitValid = true; - } - } + const auto pFollow = complianceProbability(selectedGuidance, agent, detourMeters); + const auto u = uniform01( + stableSeed + ^ activeIdHash + ^ (static_cast(entity.index) << 1U) + ^ static_cast(entity.generation)); + const bool follows = u < pFollow; - // Detour estimation can be expensive; skip it unless guidance has a detour limit. - double detourMeters = 0.0; - if (guidedExitValid && !route.originalDestinationZoneId.empty()) { - // Use a cheap approximation for detour to avoid expensive graph searches when guidance toggles. - // This detour is only used for compliance probability; the actual route still uses full planning. - const auto* originalExit = findCachedZone(layoutCache, route.originalDestinationZoneId); - const auto* guidedExit = findCachedZone(layoutCache, activeGuidance->guidedExitZoneId); - if (originalExit != nullptr && guidedExit != nullptr && originalExit->kind == ZoneKind::Exit - && guidedExit->kind == ZoneKind::Exit) { - const auto originalDistance = distanceBetween(position.value, polygonCenter(originalExit->area)); - const auto guidedDistance = distanceBetween(position.value, polygonCenter(guidedExit->area)); - detourMeters = std::max(0.0, guidedDistance - originalDistance); - } - } + route.guidanceEventId = activeId; + route.followsGuidance = follows; - const auto pFollow = complianceProbability(*activeGuidance, agent, detourMeters); - const auto u = uniform01( - stableSeed - ^ activeIdHash - ^ (static_cast(entity.index) << 1U) - ^ static_cast(entity.generation)); - const bool follows = u < pFollow; + std::string desiredExit; + if (follows && guidedExitValid) { + desiredExit = selectedGuidance.guidedExitZoneId; + } else if (!follows) { + desiredExit = route.originalDestinationZoneId; + } - route.guidanceEventId = activeId; - route.followsGuidance = follows; + if (!desiredExit.empty() && route.destinationZoneId == desiredExit && !route.waypoints.empty()) { + return; + } - std::string desiredExit; - if (follows && guidedExitValid) { - desiredExit = activeGuidance->guidedExitZoneId; - } else if (!follows) { - desiredExit = route.originalDestinationZoneId; - } + RoutePlan plan; + if (!desiredExit.empty()) { + plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); + } + if (plan.destinationZoneId.empty()) { + plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); + } + if (plan.destinationZoneId.empty()) { + return; + } + replaceRouteWithPlan(route, plan, position.value); + route.nextExitReplanSeconds = elapsedSeconds + 0.25; + } - if (!desiredExit.empty() && route.destinationZoneId == desiredExit && !route.waypoints.empty()) { - continue; - } + void applyRouteGuidance( + engine::WorldQuery& query, + const std::vector& entities, + const ScenarioLayoutCacheResource& layoutCache, + double elapsedSeconds, + std::uint64_t derivedSeed, + const ScenarioAgentSpatialIndexResource* sharedSpatialIndex) { + // Keep this small to avoid frame spikes when guidance toggles. + // Higher values converge faster but may cause noticeable hitching with many agents. + constexpr std::size_t kGuidanceReplanBudgetPerFrame = 50; - RoutePlan plan; - if (!desiredExit.empty()) { - plan = routePlanToExit(layoutCache, position.value, startZoneId, desiredExit); - } - if (plan.destinationZoneId.empty()) { - plan = routePlanToNearestExit(layoutCache, position.value, startZoneId); - } - if (plan.destinationZoneId.empty()) { - continue; + const auto activeGuidances = activeRouteGuidances(elapsedSeconds); + const auto activeSignature = activeRouteGuidanceSignature(activeGuidances); + const auto hasVisibilityAnchoredGuidance = std::any_of(activeGuidances.begin(), activeGuidances.end(), [&](const auto& active) { + return active.guidance != nullptr + && (!active.guidance->installConnectionId.empty() || guidanceHasInstallPosition(*active.guidance)); + }); + + if (activeSignature != activeRouteGuidanceId_) { + activeRouteGuidanceId_ = activeSignature; + guidanceReplanCursor_ = 0; + guidanceReplanSeed_ = derivedSeed; + guidanceReplanPending_ = true; + } else if (hasVisibilityAnchoredGuidance && !guidanceReplanPending_) { + guidanceReplanCursor_ = 0; + guidanceReplanSeed_ = derivedSeed; + guidanceReplanPending_ = true; + } + + std::optional localGuidanceIndex; + if (hasVisibilityAnchoredGuidance && sharedSpatialIndex == nullptr) { + localGuidanceIndex = buildAgentSpatialIndex(query, entities, 1.0); + } + + if (hasVisibilityAnchoredGuidance) { + const auto prioritizedEntities = prioritizedGuidanceEntities( + query, + activeGuidances, + layoutCache, + sharedSpatialIndex, + localGuidanceIndex.has_value() ? &(*localGuidanceIndex) : nullptr); + for (const auto entity : prioritizedEntities) { + applyRouteGuidanceToEntity( + query, + entity, + layoutCache, + activeGuidances, + elapsedSeconds, + guidanceReplanSeed_); } - replaceRouteWithPlan(route, plan, position.value); - route.nextExitReplanSeconds = elapsedSeconds + 0.25; + } + + if (!guidanceReplanPending_ || guidanceReplanCursor_ >= entities.size()) { + return; + } + + const auto endIndex = std::min(entities.size(), guidanceReplanCursor_ + kGuidanceReplanBudgetPerFrame); + const auto stableSeed = guidanceReplanSeed_; + + for (std::size_t i = guidanceReplanCursor_; i < endIndex; ++i) { + applyRouteGuidanceToEntity(query, entities[i], layoutCache, activeGuidances, elapsedSeconds, stableSeed); } guidanceReplanCursor_ = endIndex; @@ -2405,9 +2621,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { std::string activeRouteGuidanceId_{}; bool guidanceReplanPending_{false}; std::size_t guidanceReplanCursor_{0}; - std::optional guidanceReplanGuidance_{}; std::uint64_t guidanceReplanSeed_{0U}; - std::uint64_t guidanceReplanIdHash_{0U}; std::vector activeEntities_{}; mutable std::uint64_t planningCacheRevision_{std::numeric_limits::max()}; mutable std::unordered_map> nearestExitRouteCache_{}; diff --git a/tests/GeometryQueriesTests.cpp b/tests/GeometryQueriesTests.cpp index 425b301..8ce5b27 100644 --- a/tests/GeometryQueriesTests.cpp +++ b/tests/GeometryQueriesTests.cpp @@ -94,3 +94,36 @@ SC_TEST(GeometryQueries_RepresentativePointHandlesThinDiagonalPolygon) { SC_EXPECT_TRUE(point.has_value()); SC_EXPECT_TRUE(safecrowd::domain::pointInPolygon(polygon, *point)); } + +SC_TEST(GeometryQueries_PointInsideWalkableZoneWithClearanceRejectsWallsAndOutsideSpace) { + safecrowd::domain::FacilityLayout2D layout; + layout.zones.push_back({ + .id = "room", + .floorId = "L1", + .kind = safecrowd::domain::ZoneKind::Room, + .area = rectangle(0.0, 0.0, 10.0, 10.0), + }); + layout.zones.front().area.holes.push_back({ + {.x = 7.0, .y = 7.0}, + {.x = 8.0, .y = 7.0}, + {.x = 8.0, .y = 8.0}, + {.x = 7.0, .y = 8.0}, + }); + layout.barriers.push_back({ + .id = "wall-1", + .floorId = "L1", + .geometry = {.vertices = {{5.0, 0.0}, {5.0, 10.0}}}, + .blocksMovement = true, + }); + + SC_EXPECT_TRUE(safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout, {.x = 2.0, .y = 2.0}, "L1", 0.35)); + SC_EXPECT_TRUE(!safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout, {.x = 5.1, .y = 5.0}, "L1", 0.35)); + SC_EXPECT_TRUE(!safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout, {.x = 0.1, .y = 2.0}, "L1", 0.35)); + SC_EXPECT_TRUE(!safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout, {.x = 6.8, .y = 7.5}, "L1", 0.35)); + SC_EXPECT_TRUE(!safecrowd::domain::pointInsideWalkableZoneWithClearance( + layout, {.x = 11.0, .y = 2.0}, "L1", 0.35)); +} diff --git a/tests/ScenarioAuthoringTests.cpp b/tests/ScenarioAuthoringTests.cpp index 5b650a7..a0e13c8 100644 --- a/tests/ScenarioAuthoringTests.cpp +++ b/tests/ScenarioAuthoringTests.cpp @@ -351,3 +351,22 @@ SC_TEST(computeScenarioDiffKeys_detectsRouteGuidanceChange) { SC_EXPECT_EQ(keys.size(), std::size_t{1}); SC_EXPECT_TRUE(containsKey(keys, "control.routeGuidances")); } + +SC_TEST(computeScenarioDiffKeys_detectsRouteGuidancePositionChange) { + auto baseline = makeBaselineDraft(); + RouteGuidanceDraft guidance; + guidance.id = "guidance-1"; + guidance.guidedExitZoneId = "exit-east"; + guidance.installZoneId = "room-a"; + guidance.installFloorId = "L1"; + guidance.installPosition = {.x = 1.0, .y = 1.0}; + baseline.control.routeGuidances.push_back(guidance); + + auto variant = duplicateScenarioDraft(baseline, "scenario-2", "Variant"); + variant.control.routeGuidances.front().installPosition = {.x = 1.5, .y = 1.0}; + + const auto keys = computeScenarioDiffKeys(baseline, variant); + + SC_EXPECT_EQ(keys.size(), std::size_t{1}); + SC_EXPECT_TRUE(containsKey(keys, "control.routeGuidances")); +} diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index 6465a7b..d2dcd23 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -1790,6 +1790,347 @@ SC_TEST(ScenarioSimulationMotionSystem_RechecksInstalledGuidanceAsAgentApproache SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); } +SC_TEST(ScenarioSimulationMotionSystem_AppliesRoomGuidanceOnlyNearInstallPosition) { + auto runFromPosition = [](const safecrowd::domain::Point2D& start) { + std::vector seeds; + seeds.push_back({ + .position = {.value = start}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f, .guidancePropensity = 1.0}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = start, + .previousDistanceToWaypoint = 1.5, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "room-guidance"; + guidance.guidedExitZoneId = "far-exit"; + guidance.installZoneId = "room"; + guidance.installPosition = {.x = 0.8, .y = 3.5}; + guidance.baseComplianceRate = 1.0; + guidance.guidanceStrength = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 13, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem( + twoExitGuidanceDetourLayout(), + std::vector{guidance}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + return runtime.world().query().get(entities.front()); + }; + + const auto farRoute = runFromPosition({.x = 0.5, .y = 0.5}); + SC_EXPECT_EQ(farRoute.destinationZoneId, std::string{"near-exit"}); + SC_EXPECT_TRUE(!farRoute.followsGuidance); + SC_EXPECT_TRUE(farRoute.guidanceEventId.empty()); + + const auto nearRoute = runFromPosition({.x = 0.8, .y = 3.1}); + SC_EXPECT_EQ(nearRoute.destinationZoneId, std::string{"far-exit"}); + SC_EXPECT_TRUE(nearRoute.followsGuidance); + SC_EXPECT_EQ(nearRoute.guidanceEventId, std::string{"room-guidance"}); +} + +SC_TEST(ScenarioSimulationMotionSystem_AppliesMultipleActiveGuidancesIndependently) { + std::vector seeds; + seeds.push_back({ + .position = {.value = {.x = 1.6, .y = 0.5}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f, .guidancePropensity = 1.0}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 1.6, .y = 0.5}, + .previousDistanceToWaypoint = 0.4, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + seeds.push_back({ + .position = {.value = {.x = 0.8, .y = 3.1}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f, .guidancePropensity = 1.0}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 3.5}}, + .waypointPassages = {{{.x = 2.0, .y = 3.3}, {.x = 2.0, .y = 3.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"far-exit"}, + .waypointConnectionIds = {"room-far-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 0.8, .y = 3.1}, + .previousDistanceToWaypoint = 1.2, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + + safecrowd::domain::RouteGuidanceDraft doorGuidance; + doorGuidance.id = "door-guidance"; + doorGuidance.guidedExitZoneId = "far-exit"; + doorGuidance.installConnectionId = "room-near-exit"; + doorGuidance.baseComplianceRate = 1.0; + doorGuidance.guidanceStrength = 1.0; + doorGuidance.maxDetourMeters = 100.0; + + safecrowd::domain::RouteGuidanceDraft roomGuidance; + roomGuidance.id = "room-guidance"; + roomGuidance.guidedExitZoneId = "far-exit"; + roomGuidance.installZoneId = "room"; + roomGuidance.installPosition = {.x = 0.8, .y = 3.5}; + roomGuidance.baseComplianceRate = 1.0; + roomGuidance.guidanceStrength = 1.0; + roomGuidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 13, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem( + twoExitGuidanceDetourLayout(), + std::vector{doorGuidance, roomGuidance}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{2}); + + int doorGuidanceCount = 0; + int roomGuidanceCount = 0; + for (const auto entity : entities) { + const auto& route = runtime.world().query().get(entity); + if (route.guidanceEventId == "door-guidance") { + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); + doorGuidanceCount += 1; + } else if (route.guidanceEventId == "room-guidance") { + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); + roomGuidanceCount += 1; + } + } + + SC_EXPECT_EQ(doorGuidanceCount, 1); + SC_EXPECT_EQ(roomGuidanceCount, 1); +} + +SC_TEST(ScenarioSimulationMotionSystem_LaterApplicableGuidanceReplacesRetainedInstalledGuidance) { + std::vector seeds; + seeds.push_back({ + .position = {.value = {.x = 1.6, .y = 0.5}}, + .agent = {.radius = 0.25f, .maxSpeed = 1.0f, .guidancePropensity = 1.0}, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 1.6, .y = 0.5}, + .previousDistanceToWaypoint = 0.4, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + + safecrowd::domain::RouteGuidanceDraft doorGuidance; + doorGuidance.id = "door-guidance"; + doorGuidance.guidedExitZoneId = "far-exit"; + doorGuidance.installConnectionId = "room-near-exit"; + doorGuidance.baseComplianceRate = 1.0; + doorGuidance.guidanceStrength = 1.0; + doorGuidance.maxDetourMeters = 100.0; + + safecrowd::domain::RouteGuidanceDraft roomGuidance; + roomGuidance.id = "room-guidance"; + roomGuidance.guidedExitZoneId = "near-exit"; + roomGuidance.installZoneId = "room"; + roomGuidance.installPosition = {.x = 1.6, .y = 0.5}; + roomGuidance.periods.push_back({.startSeconds = 0.2, .endSeconds = 10.0}); + roomGuidance.baseComplianceRate = 1.0; + roomGuidance.guidanceStrength = 1.0; + roomGuidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 13, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem( + twoExitGuidanceDetourLayout(), + std::vector{doorGuidance, roomGuidance}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + for (int step = 0; step < 4; ++step) { + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + } + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + SC_EXPECT_EQ(entities.size(), std::size_t{1}); + const auto& route = runtime.world().query().get(entities.front()); + SC_EXPECT_EQ(route.guidanceEventId, std::string{"room-guidance:p0"}); + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"near-exit"}); +} + +SC_TEST(ScenarioSimulationMotionSystem_PrioritizesAgentsNearInstalledGuidanceBeforeGlobalBudgetSweep) { + std::vector seeds; + for (int index = 0; index < 60; ++index) { + const auto y = 3.5 + (0.02 * static_cast(index)); + seeds.push_back({ + .position = {.value = {.x = 0.5, .y = y}}, + .agent = { + .radius = 0.25f, + .maxSpeed = 1.0f, + .sourcePlacementId = "far-agent", + .guidancePropensity = 1.0, + }, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 0.5, .y = y}, + .previousDistanceToWaypoint = std::sqrt((1.5 * 1.5) + ((y - 0.5) * (y - 0.5))), + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + } + + seeds.push_back({ + .position = {.value = {.x = 1.6, .y = 0.5}}, + .agent = { + .radius = 0.25f, + .maxSpeed = 1.0f, + .sourcePlacementId = "priority-near", + .guidancePropensity = 1.0, + }, + .velocity = {.value = {}}, + .route = { + .waypoints = {{.x = 2.0, .y = 0.5}}, + .waypointPassages = {{{.x = 2.0, .y = 0.3}, {.x = 2.0, .y = 0.7}}}, + .waypointFromZoneIds = {"room"}, + .waypointZoneIds = {"near-exit"}, + .waypointConnectionIds = {"room-near-exit"}, + .nextWaypointIndex = 0, + .currentSegmentStart = {.x = 1.6, .y = 0.5}, + .previousDistanceToWaypoint = 0.4, + .destinationZoneId = "near-exit", + .originalDestinationZoneId = "near-exit", + }, + .status = {}, + }); + + safecrowd::domain::RouteGuidanceDraft guidance; + guidance.id = "installed-guidance"; + guidance.guidedExitZoneId = "far-exit"; + guidance.installConnectionId = "room-near-exit"; + guidance.baseComplianceRate = 1.0; + guidance.guidanceStrength = 1.0; + guidance.maxDetourMeters = 100.0; + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 13, + }); + runtime.addSystem(std::make_unique(std::move(seeds), 10.0)); + runtime.addSystem( + safecrowd::domain::makeScenarioSimulationMotionSystem( + twoExitGuidanceDetourLayout(), + std::vector{guidance}), + {.phase = safecrowd::engine::UpdatePhase::PostSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.world().resources().set(safecrowd::domain::ScenarioSimulationStepResource{.deltaSeconds = 0.1}); + runtime.stepFrame(0.0); + + const auto entities = runtime.world().query().view< + safecrowd::domain::Position, + safecrowd::domain::Agent, + safecrowd::domain::Velocity, + safecrowd::domain::AvoidanceState, + safecrowd::domain::EvacuationRoute, + safecrowd::domain::EvacuationStatus>(); + + const auto nearIt = std::find_if(entities.begin(), entities.end(), [&](const auto entity) { + const auto& agent = runtime.world().query().get(entity); + return agent.sourcePlacementId == "priority-near"; + }); + SC_EXPECT_TRUE(nearIt != entities.end()); + + const auto& route = runtime.world().query().get(*nearIt); + SC_EXPECT_EQ(route.guidanceEventId, std::string{"installed-guidance"}); + SC_EXPECT_TRUE(route.followsGuidance); + SC_EXPECT_EQ(route.destinationZoneId, std::string{"far-exit"}); +} + SC_TEST(ScenarioSimulationMotionSystem_SkipsIntermediateWaypointWhenCrowdPushesAgentPastApproachArea) { std::vector seeds; seeds.push_back({