diff --git a/src/domain/AgentComponents.h b/src/domain/AgentComponents.h index fa02739..d6f6711 100644 --- a/src/domain/AgentComponents.h +++ b/src/domain/AgentComponents.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -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 { diff --git a/src/domain/ScenarioSimulationInternal.cpp b/src/domain/ScenarioSimulationInternal.cpp index bf1f9dc..ca5e4df 100644 --- a/src/domain/ScenarioSimulationInternal.cpp +++ b/src/domain/ScenarioSimulationInternal.cpp @@ -2,9 +2,12 @@ #include #include +#include #include #include #include +#include +#include #include 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> 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{startZoneId}; + } + + std::unordered_set 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 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>> 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 dist; + dist.reserve(layout.zones.size()); + std::unordered_map prev; + prev.reserve(layout.zones.size()); + std::priority_queue, std::greater> 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 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; diff --git a/src/domain/ScenarioSimulationInternal.h b/src/domain/ScenarioSimulationInternal.h index 7e8bd68..b2c231d 100644 --- a/src/domain/ScenarioSimulationInternal.h +++ b/src/domain/ScenarioSimulationInternal.h @@ -103,6 +103,7 @@ bool pointInRing(const std::vector& 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> 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); diff --git a/src/domain/ScenarioSimulationMotionSystem.cpp b/src/domain/ScenarioSimulationMotionSystem.cpp index 93a0ff9..e1644e8 100644 --- a/src/domain/ScenarioSimulationMotionSystem.cpp +++ b/src/domain/ScenarioSimulationMotionSystem.cpp @@ -3,6 +3,7 @@ #include "domain/ScenarioSimulationInternal.h" #include +#include #include #include #include @@ -36,6 +37,10 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { return; } + const std::uint64_t layoutRevision = resources.contains() + ? resources.get().revision + : 0U; + const auto clampedDelta = std::max(0.0, resources.get().deltaSeconds); if (clampedDelta <= 0.0) { activeLayout_ = nullptr; @@ -52,7 +57,8 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { advanceRoutesForCurrentZones(query, entities); advanceRoutesForWaypointProgress(query, 0.0, entities); - replanBlockedRouteSegments(query, entities); + replanBlockedExitRoutes(query, entities, clock.elapsedSeconds, layoutRevision); + replanBlockedRouteSegments(query, entities, clock.elapsedSeconds, layoutRevision); for (const auto entity : entities) { auto& position = query.get(entity); @@ -143,10 +149,124 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } private: + static constexpr double kExitReplanCooldownSeconds = 0.75; + static constexpr double kNoExitReplanCooldownSeconds = 7.0; + static constexpr double kSegmentReplanCooldownSeconds = 0.25; + static constexpr double kFailedSegmentReplanCooldownSeconds = 1.25; + + struct RoutePlan { + std::vector waypoints{}; + std::vector waypointPassages{}; + std::vector waypointFromZoneIds{}; + std::vector waypointZoneIds{}; + std::vector waypointFloorIds{}; + std::vector waypointConnectionIds{}; + std::vector waypointVerticalTransitions{}; + std::string destinationZoneId{}; + }; + const FacilityLayout2D& layout() const { return activeLayout_ == nullptr ? layout_ : *activeLayout_; } + const Connection2D* findConnectionById(const std::string& connectionId) const { + if (connectionId.empty()) { + return nullptr; + } + const auto it = std::find_if(layout().connections.begin(), layout().connections.end(), [&](const auto& connection) { + return connection.id == connectionId; + }); + return it == layout().connections.end() ? nullptr : &(*it); + } + + bool nextConnectionBlocked(const EvacuationRoute& route) const { + if (route.nextWaypointIndex >= route.waypoints.size() || route.nextWaypointIndex >= route.waypointConnectionIds.size()) { + return false; + } + for (std::size_t index = route.nextWaypointIndex; index < route.waypointConnectionIds.size(); ++index) { + const auto& connectionId = route.waypointConnectionIds[index]; + if (connectionId.empty()) { + continue; + } + const auto* connection = findConnectionById(connectionId); + return connection != nullptr && connection->directionality == TravelDirection::Closed; + } + return false; + } + + RoutePlan routePlanToNearestExit(const Point2D& start, const std::string& startZoneId) const { + RoutePlan plan; + auto zoneRoute = zoneRouteToNearestExit(layout(), startZoneId); + if (!zoneRoute.has_value() || zoneRoute->empty()) { + return plan; + } + + plan.destinationZoneId = zoneRoute->back(); + + Point2D segmentStart = start; + auto appendSegment = [&](const std::vector& segment, + const LineSegment2D& finalPassage, + const std::string& finalFromZoneId, + const std::string& finalZoneId, + const std::string& finalFloorId, + const std::string& finalConnectionId, + bool finalVerticalTransition) { + for (std::size_t waypointIndex = 0; waypointIndex < segment.size(); ++waypointIndex) { + const bool isFinalWaypoint = waypointIndex + 1 == segment.size(); + plan.waypoints.push_back(segment[waypointIndex]); + plan.waypointPassages.push_back(isFinalWaypoint ? finalPassage : pointPassage(segment[waypointIndex])); + plan.waypointFromZoneIds.push_back(isFinalWaypoint ? finalFromZoneId : std::string{}); + plan.waypointZoneIds.push_back(isFinalWaypoint ? finalZoneId : std::string{}); + plan.waypointFloorIds.push_back(isFinalWaypoint ? finalFloorId : std::string{}); + plan.waypointConnectionIds.push_back(isFinalWaypoint ? finalConnectionId : std::string{}); + plan.waypointVerticalTransitions.push_back(isFinalWaypoint && finalVerticalTransition); + } + }; + + for (std::size_t index = 1; index < zoneRoute->size(); ++index) { + const auto& fromZoneId = (*zoneRoute)[index - 1]; + const auto& toZoneId = (*zoneRoute)[index]; + if (const auto* connection = findConnectionBetween(layout(), fromZoneId, toZoneId)) { + const auto passage = passageWithClearance(*connection, kCandidateClearance); + const auto fromFloorId = floorIdForZone(layout(), fromZoneId); + const auto toFloorId = floorIdForZone(layout(), toZoneId); + const auto segmentLayout = layoutForFloor(layout(), fromFloorId); + const auto target = closestPointOnSegment(segmentStart, passage.start, passage.end); + const auto segment = buildPath(segmentLayout, segmentStart, target, kCandidateClearance); + appendSegment( + segment, + passage, + fromZoneId, + toZoneId, + toFloorId.empty() ? fromFloorId : toFloorId, + connection->id, + isVerticalConnection(*connection)); + segmentStart = target; + } + } + + if (const auto* exitZone = findZone(layout(), zoneRoute->back())) { + const auto exitCenter = polygonCenter(exitZone->area); + if (distanceBetween(segmentStart, exitCenter) > kArrivalEpsilon) { + const auto exitFloorId = exitZone->floorId; + const auto segmentLayout = layoutForFloor(layout(), exitFloorId); + const auto segment = buildPath(segmentLayout, segmentStart, exitCenter, kCandidateClearance); + appendSegment(segment, pointPassage(exitCenter), std::string{}, exitZone->id, exitFloorId, std::string{}, false); + } + } + + if (!plan.waypoints.empty() && distanceBetween(start, plan.waypoints.front()) <= kArrivalEpsilon) { + plan.waypoints.erase(plan.waypoints.begin()); + plan.waypointPassages.erase(plan.waypointPassages.begin()); + plan.waypointFromZoneIds.erase(plan.waypointFromZoneIds.begin()); + plan.waypointZoneIds.erase(plan.waypointZoneIds.begin()); + plan.waypointFloorIds.erase(plan.waypointFloorIds.begin()); + plan.waypointConnectionIds.erase(plan.waypointConnectionIds.begin()); + plan.waypointVerticalTransitions.erase(plan.waypointVerticalTransitions.begin()); + } + return plan; + } + void advanceRouteWaypoint(EvacuationRoute& route, const Point2D& reachedPoint) const { if (route.nextWaypointIndex >= route.waypoints.size()) { return; @@ -166,6 +286,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { route.previousDistanceToWaypoint = 0.0; } route.stalledSeconds = 0.0; + route.nextSegmentReplanSeconds = 0.0; } void advanceRoutesForWaypointProgress( @@ -262,7 +383,86 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { } } - void replanBlockedRouteSegments(engine::WorldQuery& query, const std::vector& entities) const { + void replanBlockedExitRoutes( + engine::WorldQuery& query, + const std::vector& entities, + double elapsedSeconds, + std::uint64_t layoutRevision) const { + for (const auto entity : entities) { + const auto& status = query.get(entity); + if (status.evacuated) { + continue; + } + + auto& route = query.get(entity); + if (layoutRevision != route.observedLayoutRevision) { + route.observedLayoutRevision = layoutRevision; + route.nextExitReplanSeconds = 0.0; + route.nextSegmentReplanSeconds = 0.0; + } + + const bool blockedAhead = nextConnectionBlocked(route); + if (!blockedAhead && !route.noExitAvailable) { + continue; + } + + if (elapsedSeconds + 1e-9 < route.nextExitReplanSeconds) { + continue; + } + + const auto& position = query.get(entity); + const auto startZoneId = zoneAt(position.value, route.currentFloorId); + if (startZoneId.empty()) { + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + continue; + } + + const auto plan = routePlanToNearestExit(position.value, startZoneId); + if (plan.destinationZoneId.empty()) { + route.noExitAvailable = true; + route.destinationZoneId.clear(); + route.waypoints.clear(); + route.waypointPassages.clear(); + route.waypointFromZoneIds.clear(); + route.waypointZoneIds.clear(); + route.waypointFloorIds.clear(); + route.waypointConnectionIds.clear(); + route.waypointVerticalTransitions.clear(); + route.nextWaypointIndex = 0; + route.currentSegmentStart = position.value; + route.displayFloorId = route.currentFloorId; + route.previousDistanceToWaypoint = 0.0; + route.stalledSeconds = 0.0; + route.nextExitReplanSeconds = elapsedSeconds + kNoExitReplanCooldownSeconds; + continue; + } + + route.destinationZoneId = plan.destinationZoneId; + route.waypoints = plan.waypoints; + route.waypointPassages = plan.waypointPassages; + route.waypointFromZoneIds = plan.waypointFromZoneIds; + route.waypointZoneIds = plan.waypointZoneIds; + route.waypointFloorIds = plan.waypointFloorIds; + route.waypointConnectionIds = plan.waypointConnectionIds; + route.waypointVerticalTransitions = plan.waypointVerticalTransitions; + route.nextWaypointIndex = 0; + route.currentSegmentStart = position.value; + route.displayFloorId = route.currentFloorId; + route.previousDistanceToWaypoint = route.waypoints.empty() + ? 0.0 + : distanceToRouteWaypoint(route, position.value); + route.stalledSeconds = 0.0; + route.noExitAvailable = false; + route.nextSegmentReplanSeconds = 0.0; + route.nextExitReplanSeconds = elapsedSeconds + kExitReplanCooldownSeconds; + } + } + + void replanBlockedRouteSegments( + engine::WorldQuery& query, + const std::vector& entities, + double elapsedSeconds, + std::uint64_t layoutRevision) const { for (const auto entity : entities) { const auto& status = query.get(entity); if (status.evacuated) { @@ -272,9 +472,23 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto& position = query.get(entity); const auto& agent = query.get(entity); auto& route = query.get(entity); + if (route.noExitAvailable) { + continue; + } + if (layoutRevision != route.observedLayoutRevision) { + route.observedLayoutRevision = layoutRevision; + route.nextExitReplanSeconds = 0.0; + route.nextSegmentReplanSeconds = 0.0; + } if (route.nextWaypointIndex >= route.waypoints.size()) { continue; } + if (nextConnectionBlocked(route)) { + continue; + } + if (elapsedSeconds + 1e-9 < route.nextSegmentReplanSeconds) { + continue; + } const auto target = routeWaypointTarget(route, position.value); const auto clearance = static_cast(agent.radius) + kPathClearance; @@ -285,6 +499,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { const auto replacement = buildPath(floorLayout, position.value, target, clearance); if (replacement.size() <= 1) { + route.nextSegmentReplanSeconds = elapsedSeconds + kFailedSegmentReplanCooldownSeconds; continue; } @@ -366,6 +581,7 @@ class ScenarioSimulationMotionSystem final : public engine::EngineSystem { route.currentSegmentStart = position.value; route.previousDistanceToWaypoint = distanceToRouteWaypoint(route, position.value); route.stalledSeconds = 0.0; + route.nextSegmentReplanSeconds = elapsedSeconds + kSegmentReplanCooldownSeconds; } } @@ -514,4 +730,3 @@ std::unique_ptr makeScenarioSimulationMotionSystem(Facilit } } // namespace safecrowd::domain - diff --git a/src/domain/ScenarioSimulationRunner.cpp b/src/domain/ScenarioSimulationRunner.cpp index 1210518..4ae9623 100644 --- a/src/domain/ScenarioSimulationRunner.cpp +++ b/src/domain/ScenarioSimulationRunner.cpp @@ -264,55 +264,7 @@ ScenarioSimulationRunner::RoutePlan ScenarioSimulationRunner::routePlan(const Po } std::optional> ScenarioSimulationRunner::zoneRouteToExit(const std::string& startZoneId) const { - if (startZoneId.empty()) { - return std::nullopt; - } - if (const auto* startZone = findZone(layout_, startZoneId); startZone != nullptr && startZone->kind == ZoneKind::Exit) { - return std::vector{startZoneId}; - } - - std::unordered_map previous; - std::unordered_set visited; - std::deque queue; - visited.insert(startZoneId); - queue.push_back(startZoneId); - - while (!queue.empty()) { - const auto current = queue.front(); - queue.pop_front(); - if (const auto* zone = findZone(layout_, current); zone != nullptr && zone->kind == ZoneKind::Exit) { - std::vector route; - for (auto zoneId = current; !zoneId.empty();) { - route.push_back(zoneId); - const auto prev = previous.find(zoneId); - zoneId = prev == previous.end() ? std::string{} : prev->second; - } - std::reverse(route.begin(), route.end()); - return route; - } - - for (const auto& connection : layout_.connections) { - if (connection.directionality == TravelDirection::Closed) { - continue; - } - if (!canTraverseConnection(layout_, connection)) { - continue; - } - std::string next; - if (connection.fromZoneId == current && connection.directionality != TravelDirection::ReverseOnly) { - next = connection.toZoneId; - } else if (connection.toZoneId == current && connection.directionality != TravelDirection::ForwardOnly) { - next = connection.fromZoneId; - } - if (!next.empty() && !visited.contains(next)) { - visited.insert(next); - previous[next] = current; - queue.push_back(next); - } - } - } - - return std::nullopt; + return zoneRouteToNearestExit(layout_, startZoneId); } std::string ScenarioSimulationRunner::zoneAt(const Point2D& point, const std::string& floorId) const {