diff --git a/src/application/ScenarioBatchResultWidget.cpp b/src/application/ScenarioBatchResultWidget.cpp index 21e90a6..758a2bc 100644 --- a/src/application/ScenarioBatchResultWidget.cpp +++ b/src/application/ScenarioBatchResultWidget.cpp @@ -990,28 +990,12 @@ void ScenarioBatchResultWidget::applyReplayFrameData(const safecrowd::domain::Si if (results_.empty() || currentResultIndex_ < 0 || currentResultIndex_ >= static_cast(results_.size())) { return; } - const auto& result = results_[static_cast(currentResultIndex_)]; replayFrameIndex_ = replayFrames_.empty() ? 0 : std::clamp(sliderIndex, 0, static_cast(replayFrames_.size()) - 1); if (canvas_ != nullptr) { - canvas_->setConnectionBlocks(result.scenario.control.connectionBlocks); - canvas_->setEnvironmentHazards(result.scenario.environment.hazards); canvas_->setFrame(frame); - canvas_->setHotspotOverlay(result.risk.hotspots); - canvas_->setBottleneckOverlay(result.risk.bottlenecks); - canvas_->setDensityOverlay(result.artifacts.densitySummary.peakField.cells.empty() - ? result.artifacts.densitySummary.peakCells - : result.artifacts.densitySummary.peakField.cells, - result.artifacts.densitySummary.highDensityThresholdPeoplePerSquareMeter); - canvas_->setPressureOverlay(result.artifacts.pressureSummary.peakField.cells.empty() - ? result.artifacts.pressureSummary.peakCells - : result.artifacts.pressureSummary.peakField.cells, - std::max( - result.artifacts.pressureSummary.hotspotScoreThreshold, - result.artifacts.pressureSummary.peakPressureScore)); - applyOverlayModeToCanvas(); } if (replaySlider_ != nullptr && replaySlider_->value() != replayFrameIndex_) { const QSignalBlocker blocker(replaySlider_); @@ -1031,6 +1015,34 @@ void ScenarioBatchResultWidget::applyReplayFrameData(const safecrowd::domain::Si } } +void ScenarioBatchResultWidget::applySelectedResultStaticCanvasState() { + if (canvas_ == nullptr + || results_.empty() + || currentResultIndex_ < 0 + || currentResultIndex_ >= static_cast(results_.size())) { + return; + } + + const auto& result = results_[static_cast(currentResultIndex_)]; + canvas_->setConnectionBlocks(result.scenario.control.connectionBlocks); + canvas_->setEnvironmentHazards(result.scenario.environment.hazards); + canvas_->setHotspotOverlay(result.risk.hotspots); + canvas_->setBottleneckOverlay(result.risk.bottlenecks); + canvas_->setDensityOverlay( + result.artifacts.densitySummary.peakField.cells.empty() + ? result.artifacts.densitySummary.peakCells + : result.artifacts.densitySummary.peakField.cells, + result.artifacts.densitySummary.highDensityThresholdPeoplePerSquareMeter); + canvas_->setPressureOverlay( + result.artifacts.pressureSummary.peakField.cells.empty() + ? result.artifacts.pressureSummary.peakCells + : result.artifacts.pressureSummary.peakField.cells, + std::max( + result.artifacts.pressureSummary.hotspotScoreThreshold, + result.artifacts.pressureSummary.peakPressureScore)); + applyOverlayModeToCanvas(); +} + void ScenarioBatchResultWidget::applyOverlayModeToCanvas() { if (canvas_ == nullptr) { return; @@ -1125,6 +1137,7 @@ void ScenarioBatchResultWidget::loadReplayForSelectedResult() { replayFrames_.clear(); return; } + applySelectedResultStaticCanvasState(); replayFrames_ = replayFramesForResult(results_[static_cast(currentResultIndex_)]); replayFrameIndex_ = replayFrames_.empty() ? 0 : static_cast(replayFrames_.size()) - 1; if (replaySlider_ != nullptr) { diff --git a/src/application/ScenarioBatchResultWidget.h b/src/application/ScenarioBatchResultWidget.h index 163f184..b46ada1 100644 --- a/src/application/ScenarioBatchResultWidget.h +++ b/src/application/ScenarioBatchResultWidget.h @@ -59,6 +59,7 @@ class ScenarioBatchResultWidget : public QWidget { void applyReplayFrame(int frameIndex); void applyReplayFrameData(const safecrowd::domain::SimulationFrame& frame, int sliderIndex); void applyOverlayModeToCanvas(); + void applySelectedResultStaticCanvasState(); void loadReplayForSelectedResult(); int nearestReplayFrameIndex(double seconds) const; void navigateToAuthoring(); diff --git a/src/application/SimulationCanvasWidget.cpp b/src/application/SimulationCanvasWidget.cpp index be9ae61..2f50fdf 100644 --- a/src/application/SimulationCanvasWidget.cpp +++ b/src/application/SimulationCanvasWidget.cpp @@ -544,6 +544,7 @@ void SimulationCanvasWidget::setDensityOverlay( std::isfinite(scaleMaxPeoplePerSquareMeter) && scaleMaxPeoplePerSquareMeter > 0.0 ? scaleMaxPeoplePerSquareMeter : kDefaultDensityScaleMaxPeoplePerSquareMeter; + invalidateOverlayCache(); update(); } @@ -555,6 +556,7 @@ void SimulationCanvasWidget::setPressureOverlay( std::isfinite(scaleMaxPressureScore) && scaleMaxPressureScore > 0.0 ? scaleMaxPressureScore : kDefaultPressureScaleMaxScore; + invalidateOverlayCache(); update(); } @@ -563,6 +565,7 @@ void SimulationCanvasWidget::setHotspotOverlay(std::vector= hotspotOverlay_.size()) { focusedHotspotIndex_.reset(); } + invalidateOverlayCache(); update(); } @@ -571,11 +574,16 @@ void SimulationCanvasWidget::setBottleneckOverlay(std::vector= bottleneckOverlay_.size()) { focusedBottleneckIndex_.reset(); } + invalidateOverlayCache(); update(); } void SimulationCanvasWidget::setResultOverlayMode(ResultOverlayMode mode) { + if (overlayMode_ == mode) { + return; + } overlayMode_ = mode; + invalidateOverlayCache(); update(); } @@ -589,6 +597,7 @@ void SimulationCanvasWidget::focusHotspot(std::size_t index) { } focusedHotspotIndex_ = index; focusedBottleneckIndex_.reset(); + invalidateOverlayCache(); focusWorldPoint(hotspotOverlay_[index].center, std::max(camera_.zoom(), kHotspotFocusZoom)); } @@ -603,6 +612,7 @@ void SimulationCanvasWidget::focusBottleneck(std::size_t index) { } focusedBottleneckIndex_ = index; focusedHotspotIndex_.reset(); + invalidateOverlayCache(); focusWorldPoint( {.x = (passage.start.x + passage.end.x) / 2.0, .y = (passage.start.y + passage.end.y) / 2.0}, std::max(camera_.zoom(), kBottleneckFocusZoom)); @@ -640,6 +650,7 @@ void SimulationCanvasWidget::mouseDoubleClickEvent(QMouseEvent* event) { if (event->button() == Qt::LeftButton) { camera_.reset(); layoutCacheValid_ = false; + invalidateOverlayCache(); update(); event->accept(); return; @@ -658,6 +669,7 @@ void SimulationCanvasWidget::mouseMoveEvent(QMouseEvent* event) { QToolTip::hideText(); } layoutCacheValid_ = false; + invalidateOverlayCache(); update(); return; } @@ -807,14 +819,9 @@ void SimulationCanvasWidget::paintEvent(QPaintEvent* event) { painter.drawPixmap(0, 0, layoutCache_); const auto transform = currentTransform(*bounds); - if (overlayMode_ == ResultOverlayMode::Density) { - drawDensityOverlay(painter, transform); - } else if (overlayMode_ == ResultOverlayMode::Pressure) { - drawPressureOverlay(painter, transform); - } else if (overlayMode_ == ResultOverlayMode::Hotspots) { - drawHotspotOverlay(painter, transform); - } else if (overlayMode_ == ResultOverlayMode::Bottlenecks) { - drawBottleneckOverlay(painter, transform); + refreshOverlayCache(*bounds); + if (!overlayCache_.isNull()) { + painter.drawPixmap(0, 0, overlayCache_); } drawEnvironmentHazardOverlay(painter, transform); drawConnectionBlockOverlay(painter, transform); @@ -847,6 +854,7 @@ void SimulationCanvasWidget::wheelEvent(QWheelEvent* event) { } if (camera_.zoomAt(event, *bounds, previewViewport())) { layoutCacheValid_ = false; + invalidateOverlayCache(); update(); return; } @@ -901,6 +909,69 @@ void SimulationCanvasWidget::refreshLayoutCache(const LayoutCanvasBounds& bounds layoutCacheValid_ = true; } +void SimulationCanvasWidget::refreshOverlayCache(const LayoutCanvasBounds& bounds) { + if (overlayMode_ == ResultOverlayMode::None) { + overlayCache_ = QPixmap(); + overlayCacheValid_ = true; + overlayCacheMode_ = overlayMode_; + overlayCacheFloorId_ = currentFloorId_; + return; + } + + const auto currentSize = size(); + if (currentSize.isEmpty()) { + overlayCache_ = QPixmap(); + overlayCacheSize_ = currentSize; + overlayCacheDevicePixelRatio_ = 0.0; + overlayCacheValid_ = false; + return; + } + + const auto devicePixelRatio = devicePixelRatioF(); + if (overlayCacheValid_ + && overlayCacheSize_ == currentSize + && overlayCacheDevicePixelRatio_ == devicePixelRatio + && overlayCacheZoom_ == camera_.zoom() + && overlayCachePan_ == camera_.panOffset() + && overlayCacheMode_ == overlayMode_ + && overlayCacheFloorId_ == currentFloorId_) { + return; + } + + const QSize physicalSize{ + std::max(1, static_cast(std::ceil(currentSize.width() * devicePixelRatio))), + std::max(1, static_cast(std::ceil(currentSize.height() * devicePixelRatio))), + }; + overlayCache_ = QPixmap(physicalSize); + overlayCache_.setDevicePixelRatio(devicePixelRatio); + overlayCache_.fill(Qt::transparent); + + QPainter painter(&overlayCache_); + painter.setRenderHint(QPainter::Antialiasing, true); + const auto transform = currentTransform(bounds); + if (overlayMode_ == ResultOverlayMode::Density) { + drawDensityOverlay(painter, transform); + } else if (overlayMode_ == ResultOverlayMode::Pressure) { + drawPressureOverlay(painter, transform); + } else if (overlayMode_ == ResultOverlayMode::Hotspots) { + drawHotspotOverlay(painter, transform); + } else if (overlayMode_ == ResultOverlayMode::Bottlenecks) { + drawBottleneckOverlay(painter, transform); + } + + overlayCacheSize_ = currentSize; + overlayCacheZoom_ = camera_.zoom(); + overlayCachePan_ = camera_.panOffset(); + overlayCacheDevicePixelRatio_ = devicePixelRatio; + overlayCacheMode_ = overlayMode_; + overlayCacheFloorId_ = currentFloorId_; + overlayCacheValid_ = true; +} + +void SimulationCanvasWidget::invalidateOverlayCache() { + overlayCacheValid_ = false; +} + QRectF SimulationCanvasWidget::previewViewport() const { return QRectF(rect()).adjusted(kViewportPadding, kViewportPadding, -kViewportPadding, -kViewportPadding); } @@ -922,6 +993,7 @@ void SimulationCanvasWidget::focusWorldPoint(const safecrowd::domain::Point2D& p const LayoutCanvasTransform transform(*bounds, viewport, camera_.zoom(), {}); camera_.setPanOffset(viewport.center() - transform.map(point)); layoutCacheValid_ = false; + invalidateOverlayCache(); update(); } @@ -1404,6 +1476,7 @@ void SimulationCanvasWidget::setCurrentFloorId(std::string floorId, bool manualS manualFloorSelection_ = manualSelection; layoutBounds_ = collectLayoutCanvasBounds(layout_, currentFloorId_); layoutCacheValid_ = false; + invalidateOverlayCache(); camera_.reset(); if (floorComboBox_ != nullptr) { diff --git a/src/application/SimulationCanvasWidget.h b/src/application/SimulationCanvasWidget.h index 4a9c7a9..398d513 100644 --- a/src/application/SimulationCanvasWidget.h +++ b/src/application/SimulationCanvasWidget.h @@ -73,6 +73,8 @@ class SimulationCanvasWidget : public QWidget { std::optional collectBounds() const; LayoutCanvasTransform currentTransform(const LayoutCanvasBounds& bounds) const; void refreshLayoutCache(const LayoutCanvasBounds& bounds); + void refreshOverlayCache(const LayoutCanvasBounds& bounds); + void invalidateOverlayCache(); QRectF previewViewport() const; void focusWorldPoint(const safecrowd::domain::Point2D& point, double zoom); void drawConnectionBlockOverlay(QPainter& painter, const LayoutCanvasTransform& transform) const; @@ -113,6 +115,14 @@ class SimulationCanvasWidget : public QWidget { double layoutCacheZoom_{0.0}; double layoutCacheDevicePixelRatio_{0.0}; bool layoutCacheValid_{false}; + QPixmap overlayCache_{}; + QSize overlayCacheSize_{}; + QPointF overlayCachePan_{}; + double overlayCacheZoom_{0.0}; + double overlayCacheDevicePixelRatio_{0.0}; + ResultOverlayMode overlayCacheMode_{ResultOverlayMode::None}; + std::string overlayCacheFloorId_{}; + bool overlayCacheValid_{false}; std::string hoveredConnectionBlockId_{}; std::string hoveredEnvironmentHazardId_{}; diff --git a/src/domain/CompressionSystem.cpp b/src/domain/CompressionSystem.cpp index 5b9b651..ed54ae3 100644 --- a/src/domain/CompressionSystem.cpp +++ b/src/domain/CompressionSystem.cpp @@ -4,9 +4,13 @@ #include "domain/FacilityLayout2D.h" #include "domain/Metrics.h" #include "domain/PressureTuning.h" +#include "domain/ScenarioSimulationInternal.h" +#include "domain/ScenarioSimulationSystems.h" #include #include +#include +#include namespace safecrowd::domain { namespace { @@ -14,6 +18,11 @@ namespace { constexpr double kPi = 3.14159265358979323846; constexpr double kReferenceDistanceMeters = kPressureReferenceDistanceMeters; +struct SpatialCell { + int x{0}; + int y{0}; +}; + double distanceBetween(const Point2D& lhs, const Point2D& rhs) { const double dx = lhs.x - rhs.x; const double dy = lhs.y - rhs.y; @@ -72,6 +81,18 @@ double localDensityRatio(std::size_t nearbyCount) { return densityPeoplePerSquareMeter / kPressureHighDensityThresholdPeoplePerSquareMeter; } +long long spatialKey(const SpatialCell& cell) { + return (static_cast(cell.x) << 32) + ^ static_cast(cell.y); +} + +SpatialCell spatialCellFor(const Point2D& point, double cellSize) { + return { + .x = static_cast(std::floor(point.x / cellSize)), + .y = static_cast(std::floor(point.y / cellSize)), + }; +} + } // namespace CompressionSystem::CompressionSystem(double timeStepSeconds) @@ -83,8 +104,20 @@ void CompressionSystem::update(engine::EngineWorld& world, (void)step; auto& query = world.query(); + auto& resources = world.resources(); const auto agentEntities = query.view(); const auto barrierEntities = query.view(); + const auto* spatialIndex = resources.contains() + ? &resources.get() + : nullptr; + std::unordered_map> agentCells; + if (spatialIndex == nullptr) { + agentCells.reserve(agentEntities.size()); + for (const auto entity : agentEntities) { + const auto& position = query.get(entity); + agentCells[spatialKey(spatialCellFor(position.value, kReferenceDistanceMeters))].push_back(entity); + } + } for (const auto entity : agentEntities) { const auto& position = query.get(entity); @@ -92,17 +125,52 @@ void CompressionSystem::update(engine::EngineWorld& world, std::size_t nearbyCount = 0; double proximityScore = 0.0; - - for (const auto otherEntity : agentEntities) { - if (otherEntity == entity) { - continue; + if (spatialIndex != nullptr) { + const auto floorId = query.contains(entity) + ? simulation_internal::agentCollisionFloorId(query.get(entity)) + : std::string{}; + const auto nearbyAgents = scenarioNearbyAgents( + query, + *spatialIndex, + position.value, + floorId, + kReferenceDistanceMeters); + for (const auto otherEntity : nearbyAgents) { + if (otherEntity == entity) { + continue; + } + + const auto& otherPosition = query.get(otherEntity); + const double distance = distanceBetween(position.value, otherPosition.value); + if (distance < kReferenceDistanceMeters) { + proximityScore += (kReferenceDistanceMeters - distance) / kReferenceDistanceMeters; + ++nearbyCount; + } } - - const auto& otherPosition = query.get(otherEntity); - const double distance = distanceBetween(position.value, otherPosition.value); - if (distance < kReferenceDistanceMeters) { - proximityScore += (kReferenceDistanceMeters - distance) / kReferenceDistanceMeters; - ++nearbyCount; + } else { + const auto centerCell = spatialCellFor(position.value, kReferenceDistanceMeters); + for (int dy = -1; dy <= 1; ++dy) { + for (int dx = -1; dx <= 1; ++dx) { + const auto cellIt = agentCells.find(spatialKey({ + .x = centerCell.x + dx, + .y = centerCell.y + dy, + })); + if (cellIt == agentCells.end()) { + continue; + } + for (const auto otherEntity : cellIt->second) { + if (otherEntity == entity) { + continue; + } + + const auto& otherPosition = query.get(otherEntity); + const double distance = distanceBetween(position.value, otherPosition.value); + if (distance < kReferenceDistanceMeters) { + proximityScore += (kReferenceDistanceMeters - distance) / kReferenceDistanceMeters; + ++nearbyCount; + } + } + } } } diff --git a/src/domain/ScenarioRiskMetricsSystem.cpp b/src/domain/ScenarioRiskMetricsSystem.cpp index 679ed8b..e4f4df4 100644 --- a/src/domain/ScenarioRiskMetricsSystem.cpp +++ b/src/domain/ScenarioRiskMetricsSystem.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -256,6 +257,63 @@ double barrierCompression(const Barrier2D& barrier, const Point2D& position, dou return force; } +ScenarioAgentSpatialIndexResource buildDisplayFloorPressureIndex( + const std::vector& activeAgents, + const FacilityLayout2D& layout) { + ScenarioAgentSpatialIndexResource index; + index.cellSize = kPressureReferenceDistanceMeters; + index.displayCellsByFloor.reserve(4); + for (const auto& agent : activeAgents) { + const auto cellKey = spatialKey(spatialCellFor(agent.position, index.cellSize)); + index.displayCellsByFloor[agent.floorId][cellKey].push_back(agent.entity); + } + index.barrierIndicesByFloor.reserve(std::max(1, layout.floors.size())); + for (std::size_t barrierIndex = 0; barrierIndex < layout.barriers.size(); ++barrierIndex) { + const auto& barrier = layout.barriers[barrierIndex]; + if (!barrier.blocksMovement || barrier.geometry.vertices.size() < 2) { + continue; + } + + const auto& vertices = barrier.geometry.vertices; + auto insertCoverage = [&](const Point2D& minPoint, const Point2D& maxPoint) { + const auto minCell = spatialCellFor(minPoint, index.cellSize); + const auto maxCell = spatialCellFor(maxPoint, index.cellSize); + auto& floorCells = index.barrierIndicesByFloor[barrier.floorId]; + for (int y = minCell.y; y <= maxCell.y; ++y) { + for (int x = minCell.x; x <= maxCell.x; ++x) { + floorCells[spatialKey({.x = x, .y = y})].push_back(barrierIndex); + } + } + }; + + for (std::size_t vertexIndex = 0; vertexIndex + 1 < vertices.size(); ++vertexIndex) { + insertCoverage( + { + .x = std::min(vertices[vertexIndex].x, vertices[vertexIndex + 1].x), + .y = std::min(vertices[vertexIndex].y, vertices[vertexIndex + 1].y), + }, + { + .x = std::max(vertices[vertexIndex].x, vertices[vertexIndex + 1].x), + .y = std::max(vertices[vertexIndex].y, vertices[vertexIndex + 1].y), + }); + } + if (barrier.geometry.closed) { + auto minX = vertices.front().x; + auto minY = vertices.front().y; + auto maxX = vertices.front().x; + auto maxY = vertices.front().y; + for (const auto& vertex : vertices) { + minX = std::min(minX, vertex.x); + minY = std::min(minY, vertex.y); + maxX = std::max(maxX, vertex.x); + maxY = std::max(maxY, vertex.y); + } + insertCoverage({.x = minX, .y = minY}, {.x = maxX, .y = maxY}); + } + } + return index; +} + double localDensityRatio(std::size_t nearbyCount) { constexpr double kPi = 3.14159265358979323846; const auto areaSquareMeters = kPi * kPressureReferenceDistanceMeters * kPressureReferenceDistanceMeters; @@ -533,12 +591,17 @@ class ScenarioPressureFeedbackSystem final : public engine::EngineSystem { forces[index] += combinedRadius - distance; } - for (const auto& barrier : activeLayout.barriers) { - if (!barrierMatchesFloor(barrier, activeAgents[index].collisionFloorId)) { + for (const auto* barrier : scenarioNearbyBarriers( + activeLayout, + *spatialIndex, + activeAgents[index].position, + activeAgents[index].collisionFloorId, + activeAgents[index].radius)) { + if (barrier == nullptr) { continue; } forces[index] += barrierCompression( - barrier, + *barrier, activeAgents[index].position, activeAgents[index].radius); } @@ -678,6 +741,15 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { cell.entities.push_back(entity); } + std::optional fallbackPressureIndex; + const auto* pressureIndex = resources.contains() + ? &resources.get() + : nullptr; + if (pressureIndex == nullptr) { + fallbackPressureIndex = buildDisplayFloorPressureIndex(activeAgents, activeLayout); + pressureIndex = &*fallbackPressureIndex; + } + ScenarioSimulationClockResource clock; if (resources.contains()) { clock = resources.get(); @@ -687,6 +759,7 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { snapshot, activeLayout, activeAgents, + *pressureIndex, clock.elapsedSeconds, pressureTracking); @@ -773,6 +846,7 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { ScenarioRiskSnapshot& snapshot, const FacilityLayout2D& layout, const std::vector& activeAgents, + const ScenarioAgentSpatialIndexResource& spatialIndex, double elapsedSeconds, ScenarioPressureTrackingResource& tracking) const { const auto deltaSeconds = tracking.hasPreviousElapsedSeconds @@ -781,38 +855,77 @@ class ScenarioRiskMetricsSystem final : public engine::EngineSystem { tracking.previousElapsedSeconds = elapsedSeconds; tracking.hasPreviousElapsedSeconds = true; + std::unordered_map activeAgentIndices; + activeAgentIndices.reserve(activeAgents.size()); + for (std::size_t index = 0; index < activeAgents.size(); ++index) { + activeAgentIndices.emplace(activeAgents[index].agentId, index); + } + std::vector proximityForces(activeAgents.size(), 0.0); std::vector nearbyCounts(activeAgents.size(), 0); + const auto probeRange = std::max(1, static_cast(std::ceil( + kPressureReferenceDistanceMeters / std::max(0.1, spatialIndex.cellSize)))); for (std::size_t lhsIndex = 0; lhsIndex < activeAgents.size(); ++lhsIndex) { - for (std::size_t rhsIndex = lhsIndex + 1; rhsIndex < activeAgents.size(); ++rhsIndex) { - if (activeAgents[lhsIndex].floorId != activeAgents[rhsIndex].floorId) { - continue; - } - const auto distance = distanceBetween(activeAgents[lhsIndex].position, activeAgents[rhsIndex].position); - if (distance >= kPressureReferenceDistanceMeters) { - continue; - } + const auto& lhsAgent = activeAgents[lhsIndex]; + const auto floorIt = spatialIndex.displayCellsByFloor.find(lhsAgent.floorId); + if (floorIt == spatialIndex.displayCellsByFloor.end()) { + continue; + } - const auto proximityScore = - (kPressureReferenceDistanceMeters - distance) / kPressureReferenceDistanceMeters; - proximityForces[lhsIndex] += proximityScore; - proximityForces[rhsIndex] += proximityScore; - ++nearbyCounts[lhsIndex]; - ++nearbyCounts[rhsIndex]; + const auto centerCell = spatialCellFor(lhsAgent.position, spatialIndex.cellSize); + for (int dy = -probeRange; dy <= probeRange; ++dy) { + for (int dx = -probeRange; dx <= probeRange; ++dx) { + const auto cellIt = floorIt->second.find(spatialKey({ + .x = centerCell.x + dx, + .y = centerCell.y + dy, + })); + if (cellIt == floorIt->second.end()) { + continue; + } + + for (const auto rhsEntity : cellIt->second) { + const auto rhsIndexIt = activeAgentIndices.find(rhsEntity.index); + if (rhsIndexIt == activeAgentIndices.end()) { + continue; + } + const auto rhsIndex = rhsIndexIt->second; + if (rhsIndex <= lhsIndex) { + continue; + } + + const auto& rhsAgent = activeAgents[rhsIndex]; + const auto distance = distanceBetween(lhsAgent.position, rhsAgent.position); + if (distance >= kPressureReferenceDistanceMeters) { + continue; + } + + const auto proximityScore = + (kPressureReferenceDistanceMeters - distance) / kPressureReferenceDistanceMeters; + proximityForces[lhsIndex] += proximityScore; + proximityForces[rhsIndex] += proximityScore; + ++nearbyCounts[lhsIndex]; + ++nearbyCounts[rhsIndex]; + } + } } } std::vector forces(activeAgents.size(), 0.0); for (std::size_t index = 0; index < activeAgents.size(); ++index) { forces[index] = std::max(proximityForces[index], localDensityRatio(nearbyCounts[index])); - for (const auto& barrier : layout.barriers) { - if (!barrierMatchesFloor(barrier, activeAgents[index].floorId)) { + for (const auto* barrier : scenarioNearbyBarriers( + layout, + spatialIndex, + activeAgents[index].position, + activeAgents[index].floorId, + kPressureReferenceDistanceMeters)) { + if (barrier == nullptr) { continue; } forces[index] = std::max( forces[index], proximityForces[index] + barrierCompression( - barrier, + *barrier, activeAgents[index].position, kPressureReferenceDistanceMeters)); } diff --git a/src/domain/ScenarioSimulationInternal.cpp b/src/domain/ScenarioSimulationInternal.cpp index 707fc8f..063d343 100644 --- a/src/domain/ScenarioSimulationInternal.cpp +++ b/src/domain/ScenarioSimulationInternal.cpp @@ -1302,7 +1302,7 @@ Point2D forwardPreservingAgentAvoidanceVelocity( } Point2D barrierSeparationVelocity( - const FacilityLayout2D& layout, + const std::vector& barriers, const Position& position, const Agent& agent, double referenceSpeed) { @@ -1310,12 +1310,12 @@ Point2D barrierSeparationVelocity( const auto keepoutDistance = static_cast(agent.radius) + kBarrierAvoidanceBuffer; const auto separationSpeed = std::max(0.0, referenceSpeed); - for (const auto& barrier : layout.barriers) { - if (!barrier.blocksMovement || barrier.geometry.vertices.size() < 2) { + for (const auto* barrier : barriers) { + if (barrier == nullptr || !barrier->blocksMovement || barrier->geometry.vertices.size() < 2) { continue; } - const auto& vertices = barrier.geometry.vertices; + const auto& vertices = barrier->geometry.vertices; for (std::size_t index = 0; index + 1 < vertices.size(); ++index) { const auto closest = closestPointOnSegment(position.value, vertices[index], vertices[index + 1]); const auto delta = position.value - closest; @@ -1327,7 +1327,7 @@ Point2D barrierSeparationVelocity( } } - if (barrier.geometry.closed) { + if (barrier->geometry.closed) { const auto closest = closestPointOnSegment(position.value, vertices.back(), vertices.front()); const auto delta = position.value - closest; const auto distance = lengthOf(delta); @@ -1342,6 +1342,19 @@ Point2D barrierSeparationVelocity( return correction; } +Point2D barrierSeparationVelocity( + const FacilityLayout2D& layout, + const Position& position, + const Agent& agent, + double referenceSpeed) { + std::vector barriers; + barriers.reserve(layout.barriers.size()); + for (const auto& barrier : layout.barriers) { + barriers.push_back(&barrier); + } + return barrierSeparationVelocity(barriers, position, agent, referenceSpeed); +} + bool movementCrossesBarrier(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to) { if (distanceBetween(from, to) <= kGeometryEpsilon) { return false; @@ -1477,7 +1490,11 @@ bool nearlySamePoint(const Point2D& lhs, const Point2D& rhs) { return distanceBetween(lhs, rhs) <= 1e-6; } -std::vector buildVisibilityPath(const FacilityLayout2D& layout, const Point2D& start, const Point2D& goal, double clearance) { +std::vector buildVisibilityPath( + const FacilityLayout2D& layout, + const Point2D& start, + const Point2D& goal, + double clearance) { std::vector nodes; nodes.push_back({.point = start}); nodes.push_back({.point = goal}); diff --git a/src/domain/ScenarioSimulationInternal.h b/src/domain/ScenarioSimulationInternal.h index beda7cb..2f2d0e0 100644 --- a/src/domain/ScenarioSimulationInternal.h +++ b/src/domain/ScenarioSimulationInternal.h @@ -188,6 +188,11 @@ Point2D barrierSeparationVelocity( const Position& position, const Agent& agent, double referenceSpeed); +Point2D barrierSeparationVelocity( + const std::vector& barriers, + const Position& position, + const Agent& agent, + double referenceSpeed); bool movementCrossesBarrier(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to); bool lineOfSightClear(const FacilityLayout2D& layout, const Point2D& from, const Point2D& to, double clearance); std::vector buildPath(const FacilityLayout2D& layout, const Point2D& start, const Point2D& goal, double clearance); diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 60dd886..f219c23 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -71,6 +71,9 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto* activeHazards = resources.contains() ? &resources.get() : nullptr; + const auto* sharedSpatialIndex = resources.contains() + ? &resources.get() + : nullptr; applyRouteGuidance(query, entities, layoutCache, clock.elapsedSeconds, step.derivedSeed); advanceRoutesForCurrentZones(query, entities, layoutCache); @@ -79,7 +82,10 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { replanBlockedRouteSegments(query, entities, layoutCache, clock.elapsedSeconds, layoutRevision); replanHazardAwareExitRoutes(query, entities, layoutCache, clock.elapsedSeconds, reactions, activeHazards); updateAgentPhysicsFloorIds(query, layoutCache, entities); - const auto localNeighborIndex = buildAgentSpatialIndex(query, entities, 1.0); + std::optional localNeighborIndex; + if (sharedSpatialIndex == nullptr) { + localNeighborIndex = buildAgentSpatialIndex(query, entities, 1.0); + } const auto* pressureFeedback = resources.contains() ? &resources.get() : nullptr; @@ -234,8 +240,19 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { static_cast(agent.radius) + kDefaultAgentRadius + kPersonalSpaceBuffer, kHeadOnLookAheadDistance); const auto collisionFloorId = agentCollisionFloorId(route); - const auto neighborCandidates = - nearbyAgents(query, localNeighborIndex, position.value, collisionFloorId, neighborRadius); + const auto neighborCandidates = sharedSpatialIndex != nullptr + ? scenarioNearbyAgents( + query, + *sharedSpatialIndex, + position.value, + collisionFloorId, + neighborRadius) + : nearbyAgents( + query, + *localNeighborIndex, + position.value, + collisionFloorId, + neighborRadius); const auto avoidanceVelocity = forwardPreservingAgentAvoidanceVelocity( query, @@ -248,14 +265,25 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto barrierReferenceSpeed = std::max( adjustedHazardMaxSpeed, (static_cast(agent.maxSpeed) * 0.75) * pressureSpeedFactor); - const auto barrierVelocity = - barrierSeparationVelocity(floorLayout, position, agent, barrierReferenceSpeed) - * pressureBarrierScale; + const auto barrierVelocity = sharedSpatialIndex != nullptr + ? barrierSeparationVelocity( + scenarioNearbyBarriers( + layoutCache.layout, + *sharedSpatialIndex, + position.value, + route.currentFloorId, + static_cast(agent.radius) + kBarrierAvoidanceBuffer), + position, + agent, + barrierReferenceSpeed) + : barrierSeparationVelocity(floorLayout, position, agent, barrierReferenceSpeed); + const auto scaledBarrierVelocity = barrierVelocity * pressureBarrierScale; const auto hazardAvoidanceVelocity = hazardState == nullptr ? Point2D{} : hazardAvoidanceVelocityFor(*hazardState, position.value, routeDirection, adjustedHazardMaxSpeed); - auto finalVelocity = (desiredVelocity * speedScale) + avoidanceVelocity + barrierVelocity + hazardAvoidanceVelocity; + auto finalVelocity = + (desiredVelocity * speedScale) + avoidanceVelocity + scaledBarrierVelocity + hazardAvoidanceVelocity; const auto lateral = perpendicularLeft(routeDirection); if (dot(finalVelocity, routeDirection) < 0.0) { finalVelocity = (routeDirection * (adjustedHazardMaxSpeed * 0.15)) @@ -268,7 +296,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { finalVelocity = (routeDirection * std::max(0.0, forwardComponent)) + (lateral * std::clamp(lateralComponent, -maxLateralComponent, maxLateralComponent)); } - finalVelocity = velocityWithBarrierEscape(finalVelocity, barrierVelocity, adjustedHazardMaxSpeed); + finalVelocity = velocityWithBarrierEscape(finalVelocity, scaledBarrierVelocity, adjustedHazardMaxSpeed); plans.push_back({ .entity = entity, .velocity = clampedToLength(finalVelocity, adjustedHazardMaxSpeed), diff --git a/src/domain/ScenarioSimulationSystems.cpp b/src/domain/ScenarioSimulationSystems.cpp index 72d5adb..2e73b78 100644 --- a/src/domain/ScenarioSimulationSystems.cpp +++ b/src/domain/ScenarioSimulationSystems.cpp @@ -81,6 +81,127 @@ Point2D cellMax(const SpatialCell& cell, double cellSize) { }; } +void insertBarrierCoverageCells( + std::unordered_map>>& cellsByFloor, + const std::string& floorId, + const Point2D& minPoint, + const Point2D& maxPoint, + double cellSize, + std::size_t barrierIndex) { + const auto minCell = spatialCellFor(minPoint, cellSize); + const auto maxCell = spatialCellFor(maxPoint, cellSize); + auto& floorCells = cellsByFloor[floorId]; + for (int y = minCell.y; y <= maxCell.y; ++y) { + for (int x = minCell.x; x <= maxCell.x; ++x) { + floorCells[spatialKey({.x = x, .y = y})].push_back(barrierIndex); + } + } +} + +void appendBarrierIndexCoverage( + std::unordered_map>>& cellsByFloor, + const Barrier2D& barrier, + double cellSize, + std::size_t barrierIndex) { + const auto& vertices = barrier.geometry.vertices; + if (!barrier.blocksMovement || vertices.size() < 2) { + return; + } + + for (std::size_t index = 0; index + 1 < vertices.size(); ++index) { + insertBarrierCoverageCells( + cellsByFloor, + barrier.floorId, + { + .x = std::min(vertices[index].x, vertices[index + 1].x), + .y = std::min(vertices[index].y, vertices[index + 1].y), + }, + { + .x = std::max(vertices[index].x, vertices[index + 1].x), + .y = std::max(vertices[index].y, vertices[index + 1].y), + }, + cellSize, + barrierIndex); + } + + if (barrier.geometry.closed) { + insertBarrierCoverageCells( + cellsByFloor, + barrier.floorId, + { + .x = std::min(vertices.back().x, vertices.front().x), + .y = std::min(vertices.back().y, vertices.front().y), + }, + { + .x = std::max(vertices.back().x, vertices.front().x), + .y = std::max(vertices.back().y, vertices.front().y), + }, + cellSize, + barrierIndex); + + auto minX = vertices.front().x; + auto minY = vertices.front().y; + auto maxX = vertices.front().x; + auto maxY = vertices.front().y; + for (const auto& vertex : vertices) { + minX = std::min(minX, vertex.x); + minY = std::min(minY, vertex.y); + maxX = std::max(maxX, vertex.x); + maxY = std::max(maxY, vertex.y); + } + insertBarrierCoverageCells( + cellsByFloor, + barrier.floorId, + {.x = minX, .y = minY}, + {.x = maxX, .y = maxY}, + cellSize, + barrierIndex); + } +} + +bool barrierWithinRadius(const Barrier2D& barrier, const Point2D& point, double radius) { + if (!barrier.blocksMovement || barrier.geometry.vertices.size() < 2) { + return false; + } + + const auto& vertices = barrier.geometry.vertices; + for (std::size_t index = 0; index + 1 < vertices.size(); ++index) { + const auto closest = + simulation_internal::closestPointOnSegment(point, vertices[index], vertices[index + 1]); + if (distanceBetween(point, closest) <= radius) { + return true; + } + } + + if (barrier.geometry.closed) { + const auto closest = + simulation_internal::closestPointOnSegment(point, vertices.back(), vertices.front()); + if (distanceBetween(point, closest) <= radius + || simulation_internal::pointInRing(vertices, point)) { + return true; + } + } + + return false; +} + +template +void appendSpatialBucketEntries( + const std::unordered_map>& floorCells, + const SpatialCell& center, + int range, + std::vector& entries) { + for (int dy = -range; dy <= range; ++dy) { + for (int dx = -range; dx <= range; ++dx) { + const auto it = floorCells.find(spatialKey({.x = center.x + dx, .y = center.y + dy})); + if (it == floorCells.end()) { + continue; + } + entries.insert(entries.end(), it->second.begin(), it->second.end()); + } + } +} + void appendReplayFrame( ScenarioResultArtifactsResource& result, const SimulationFrame& frame) { @@ -594,18 +715,59 @@ std::vector scenarioNearbyAgents( const auto center = spatialCellFor(point, index.cellSize); const auto range = std::max(1, static_cast(std::ceil(radius / index.cellSize))); - for (int dy = -range; dy <= range; ++dy) { - for (int dx = -range; dx <= range; ++dx) { - const auto it = floorIt->second.find(spatialKey({.x = center.x + dx, .y = center.y + dy})); - if (it == floorIt->second.end()) { - continue; - } - for (const auto entity : it->second) { - const auto& otherPosition = query.get(entity); - if (distanceBetween(point, otherPosition.value) <= radius) { - candidates.push_back(entity); - } - } + std::vector bucketEntries; + appendSpatialBucketEntries(floorIt->second, center, range, bucketEntries); + for (const auto entity : bucketEntries) { + const auto& otherPosition = query.get(entity); + if (distanceBetween(point, otherPosition.value) <= radius) { + candidates.push_back(entity); + } + } + return candidates; +} + +std::vector scenarioNearbyBarriers( + const FacilityLayout2D& layout, + const ScenarioAgentSpatialIndexResource& index, + const Point2D& point, + const std::string& floorId, + double radius) { + std::vector candidates; + if (layout.barriers.empty()) { + return candidates; + } + + const auto center = spatialCellFor(point, index.cellSize); + const auto range = std::max(1, static_cast(std::ceil(radius / index.cellSize))); + std::unordered_set barrierIndices; + barrierIndices.reserve(16); + + auto appendFloorCandidates = [&](const std::string& candidateFloorId) { + const auto floorIt = index.barrierIndicesByFloor.find(candidateFloorId); + if (floorIt == index.barrierIndicesByFloor.end()) { + return; + } + std::vector bucketEntries; + appendSpatialBucketEntries(floorIt->second, center, range, bucketEntries); + barrierIndices.insert(bucketEntries.begin(), bucketEntries.end()); + }; + + if (floorId.empty()) { + for (const auto& [candidateFloorId, _] : index.barrierIndicesByFloor) { + appendFloorCandidates(candidateFloorId); + } + } else { + appendFloorCandidates(floorId); + appendFloorCandidates(std::string{}); + } + + for (const auto barrierIndex : barrierIndices) { + if (barrierIndex >= layout.barriers.size()) { + continue; + } + const auto& barrier = layout.barriers[barrierIndex]; + if (barrierWithinRadius(barrier, point, radius)) { + candidates.push_back(&barrier); } } return candidates; @@ -633,17 +795,34 @@ void ScenarioSpatialIndexSystem::update(engine::EngineWorld& world, const engine const auto entities = query.view(); index.cellsByFloor.reserve(4); + index.displayCellsByFloor.reserve(4); for (const auto entity : entities) { const auto& status = query.get(entity); if (status.evacuated) { continue; } const auto& position = query.get(entity); - const auto floorId = query.contains(entity) + const auto collisionFloorId = query.contains(entity) ? simulation_internal::agentCollisionFloorId(query.get(entity)) : std::string{}; - auto& floorCells = index.cellsByFloor[floorId]; - floorCells[spatialKey(spatialCellFor(position.value, index.cellSize))].push_back(entity); + const auto displayFloorId = query.contains(entity) + ? simulation_internal::agentDisplayFloorId(query.get(entity)) + : collisionFloorId; + const auto cellKey = spatialKey(spatialCellFor(position.value, index.cellSize)); + index.cellsByFloor[collisionFloorId][cellKey].push_back(entity); + index.displayCellsByFloor[displayFloorId][cellKey].push_back(entity); + } + + if (resources.contains()) { + const auto& layout = resources.get().layout; + index.barrierIndicesByFloor.reserve(std::max(1, layout.floors.size())); + for (std::size_t barrierIndex = 0; barrierIndex < layout.barriers.size(); ++barrierIndex) { + appendBarrierIndexCoverage( + index.barrierIndicesByFloor, + layout.barriers[barrierIndex], + index.cellSize, + barrierIndex); + } } resources.set(std::move(index)); diff --git a/src/domain/ScenarioSimulationSystems.h b/src/domain/ScenarioSimulationSystems.h index 9a5c43c..f4a2915 100644 --- a/src/domain/ScenarioSimulationSystems.h +++ b/src/domain/ScenarioSimulationSystems.h @@ -35,6 +35,8 @@ struct ScenarioSimulationStepResource { struct ScenarioAgentSpatialIndexResource { double cellSize{1.0}; std::unordered_map>> cellsByFloor{}; + std::unordered_map>> displayCellsByFloor{}; + std::unordered_map>> barrierIndicesByFloor{}; }; struct ScenarioConnectionTraversal { @@ -161,6 +163,12 @@ std::vector scenarioNearbyAgents( const Point2D& point, const std::string& floorId, double radius); +std::vector scenarioNearbyBarriers( + const FacilityLayout2D& layout, + const ScenarioAgentSpatialIndexResource& index, + const Point2D& point, + const std::string& floorId, + double radius); std::unique_ptr makeScenarioSimulationMotionSystem(); std::unique_ptr makeScenarioControlSystem( diff --git a/tests/ScenarioSimulationSystemsTests.cpp b/tests/ScenarioSimulationSystemsTests.cpp index dafe364..3a8e814 100644 --- a/tests/ScenarioSimulationSystemsTests.cpp +++ b/tests/ScenarioSimulationSystemsTests.cpp @@ -107,6 +107,28 @@ class ConfigureConnectedStairEndpointAgentsSystem final : public safecrowd::engi safecrowd::domain::Point2D position_{}; }; +class ConfigureBarrierIndexedLayoutSystem final : public safecrowd::engine::EngineSystem { +public: + explicit ConfigureBarrierIndexedLayoutSystem(safecrowd::domain::FacilityLayout2D layout) + : layout_(std::move(layout)) { + } + + void configure(safecrowd::engine::EngineWorld& world) override { + world.resources().set(safecrowd::domain::ScenarioSimulationClockResource{ + .elapsedSeconds = 0.0, + .timeLimitSeconds = 10.0, + .complete = false, + }); + world.resources().set(safecrowd::domain::simulation_internal::buildScenarioLayoutCache(layout_)); + } + + void update(safecrowd::engine::EngineWorld&, const safecrowd::engine::EngineStepContext&) override { + } + +private: + safecrowd::domain::FacilityLayout2D layout_{}; +}; + class ConfigureEvacuatedAgentsSystem final : public safecrowd::engine::EngineSystem { public: void configure(safecrowd::engine::EngineWorld& world) override { @@ -938,6 +960,61 @@ SC_TEST(ScenarioSpatialIndexSystem_SeparatesConnectedStairEndpointsAwayFromVerti SC_EXPECT_EQ(l2Nearby.size(), std::size_t{1}); } +SC_TEST(ScenarioSpatialIndexSystem_BuildsNearbyBarrierResourceByFloor) { + safecrowd::domain::FacilityLayout2D layout; + layout.floors = { + {.id = "L1"}, + {.id = "L2"}, + }; + layout.barriers.push_back({ + .id = "wall-l1", + .floorId = "L1", + .geometry = {.vertices = {{.x = -1.0, .y = 0.0}, {.x = 1.0, .y = 0.0}}}, + .blocksMovement = true, + }); + layout.barriers.push_back({ + .id = "wall-l2", + .floorId = "L2", + .geometry = {.vertices = {{.x = -1.0, .y = 0.0}, {.x = 1.0, .y = 0.0}}}, + .blocksMovement = true, + }); + + safecrowd::engine::EngineRuntime runtime({ + .fixedDeltaTime = 1.0 / 30.0, + .maxCatchUpSteps = 1, + .baseSeed = 34, + }); + runtime.addSystem(std::make_unique(layout)); + runtime.addSystem( + std::make_unique(1.0), + {.phase = safecrowd::engine::UpdatePhase::PreSimulation, + .triggerPolicy = safecrowd::engine::TriggerPolicy::EveryFrame}); + + runtime.play(); + runtime.stepFrame(1.0 / 30.0); + + const auto& resources = runtime.world().resources(); + const auto& index = resources.get(); + const auto& activeLayout = resources.get().layout; + const auto l1Barriers = safecrowd::domain::scenarioNearbyBarriers( + activeLayout, + index, + {.x = 0.0, .y = 0.15}, + "L1", + 0.4); + const auto l2Barriers = safecrowd::domain::scenarioNearbyBarriers( + activeLayout, + index, + {.x = 0.0, .y = 0.15}, + "L2", + 0.4); + + SC_EXPECT_EQ(l1Barriers.size(), std::size_t{1}); + SC_EXPECT_EQ(l2Barriers.size(), std::size_t{1}); + SC_EXPECT_EQ(l1Barriers.front()->id, std::string{"wall-l1"}); + SC_EXPECT_EQ(l2Barriers.front()->id, std::string{"wall-l2"}); +} + SC_TEST(ScenarioClockSystem_AdvancesClockResourceOnFixedSteps) { safecrowd::engine::EngineRuntime runtime({ .fixedDeltaTime = 0.25,