[Domain] Reroute agents when exits are blocked

src/domain/AgentComponents.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstddef>
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -36,6 +37,11 @@ struct EvacuationRoute {
     std::string destinationZoneId{};
     std::string currentFloorId{};
     std::string displayFloorId{};
+
+    double nextExitReplanSeconds{0.0};
+    double nextSegmentReplanSeconds{0.0};
+    std::uint64_t observedLayoutRevision{0};
+    bool noExitAvailable{false};
 };
 
 struct EvacuationStatus {

src/domain/ScenarioSimulationInternal.cpp

@@ -2,9 +2,12 @@
 
 #include <algorithm>
 #include <cmath>
+#include <functional>
 #include <limits>
 #include <optional>
 #include <queue>
+#include <unordered_map>
+#include <unordered_set>
 #include <utility>
 
 namespace safecrowd::domain::simulation_internal {
@@ -505,6 +508,122 @@ const Connection2D* findConnectionBetween(const FacilityLayout2D& layout, const
     return it == layout.connections.end() ? nullptr : &(*it);
 }
 
+std::optional<std::vector<std::string>> zoneRouteToNearestExit(const FacilityLayout2D& layout, const std::string& startZoneId) {
+    if (startZoneId.empty()) {
+        return std::nullopt;
+    }
+
+    if (const auto* startZone = findZone(layout, startZoneId); startZone != nullptr && startZone->kind == ZoneKind::Exit) {
+        return std::vector<std::string>{startZoneId};
+    }
+
+    std::unordered_set<std::string> exitZoneIds;
+    exitZoneIds.reserve(layout.zones.size());
+    for (const auto& zone : layout.zones) {
+        if (zone.kind == ZoneKind::Exit) {
+            exitZoneIds.insert(zone.id);
+        }
+    }
+    if (exitZoneIds.empty()) {
+        return std::nullopt;
+    }
+
+    std::unordered_map<std::string, Point2D> centers;
+    centers.reserve(layout.zones.size());
+    for (const auto& zone : layout.zones) {
+        centers.emplace(zone.id, polygonCenter(zone.area));
+    }
+    const auto zoneCenter = [&](const std::string& zoneId) -> Point2D {
+        const auto it = centers.find(zoneId);
+        return it == centers.end() ? Point2D{} : it->second;
+    };
+
+    std::unordered_map<std::string, std::vector<std::pair<std::string, double>>> adjacency;
+    adjacency.reserve(layout.zones.size() * 2);
+    for (const auto& connection : layout.connections) {
+        if (connection.directionality == TravelDirection::Closed) {
+            continue;
+        }
+        if (!canTraverseConnection(layout, connection)) {
+            continue;
+        }
+
+        const auto portal = midpoint(connection.centerSpan);
+        const auto fromCenter = zoneCenter(connection.fromZoneId);
+        const auto toCenter = zoneCenter(connection.toZoneId);
+        const auto forwardWeight =
+            distanceBetween(fromCenter, portal) + distanceBetween(portal, toCenter);
+        const auto reverseWeight =
+            distanceBetween(toCenter, portal) + distanceBetween(portal, fromCenter);
+
+        if (connection.directionality != TravelDirection::ReverseOnly) {
+            adjacency[connection.fromZoneId].push_back({connection.toZoneId, forwardWeight});
+        }
+        if (connection.directionality != TravelDirection::ForwardOnly) {
+            adjacency[connection.toZoneId].push_back({connection.fromZoneId, reverseWeight});
+        }
+    }
+
+    struct QueueItem {
+        double distance{0.0};
+        std::string zoneId{};
+
+        bool operator>(const QueueItem& other) const noexcept {
+            return distance > other.distance;
+        }
+    };
+
+    std::unordered_map<std::string, double> dist;
+    dist.reserve(layout.zones.size());
+    std::unordered_map<std::string, std::string> prev;
+    prev.reserve(layout.zones.size());
+    std::priority_queue<QueueItem, std::vector<QueueItem>, std::greater<QueueItem>> pq;
+
+    dist[startZoneId] = 0.0;
+    pq.push({.distance = 0.0, .zoneId = startZoneId});
+
+    while (!pq.empty()) {
+        const auto current = pq.top();
+        pq.pop();
+
+        const auto bestIt = dist.find(current.zoneId);
+        if (bestIt == dist.end() || current.distance > bestIt->second + 1e-12) {
+            continue;
+        }
+
+        if (exitZoneIds.contains(current.zoneId)) {
+            std::vector<std::string> route;
+            for (auto zoneId = current.zoneId; !zoneId.empty();) {
+                route.push_back(zoneId);
+                const auto it = prev.find(zoneId);
+                zoneId = it == prev.end() ? std::string{} : it->second;
+            }
+            std::reverse(route.begin(), route.end());
+            return route;
+        }
+
+        const auto adjIt = adjacency.find(current.zoneId);
+        if (adjIt == adjacency.end()) {
+            continue;
+        }
+
+        for (const auto& [next, cost] : adjIt->second) {
+            if (next.empty()) {
+                continue;
+            }
+            const auto nextDistance = current.distance + std::max(0.0, cost);
+            const auto distIt = dist.find(next);
+            if (distIt == dist.end() || nextDistance + 1e-12 < distIt->second) {
+                dist[next] = nextDistance;
+                prev[next] = current.zoneId;
+                pq.push({.distance = nextDistance, .zoneId = next});
+            }
+        }
+    }
+
+    return std::nullopt;
+}
+
 double speedOf(const Point2D& velocity) {
     const auto speed = std::hypot(velocity.x, velocity.y);
     return speed > 0.0 ? speed : kDefaultAgentSpeed;

src/domain/ScenarioSimulationInternal.h

@@ -103,6 +103,7 @@ bool pointInRing(const std::vector<Point2D>& ring, const Point2D& point);
 Point2D polygonCenter(const Polygon2D& polygon);
 const Zone2D* findZone(const FacilityLayout2D& layout, const std::string& zoneId);
 const Connection2D* findConnectionBetween(const FacilityLayout2D& layout, const std::string& from, const std::string& to);
+std::optional<std::vector<std::string>> zoneRouteToNearestExit(const FacilityLayout2D& layout, const std::string& startZoneId);
 std::string floorIdForZone(const FacilityLayout2D& layout, const std::string& zoneId);
 bool isVerticalConnection(const Connection2D& connection);
 bool canTraverseConnection(const FacilityLayout2D& layout, const Connection2D& connection);