From d550bf09c8b6b7fdd7bb6dbc182c203fcc1a590a Mon Sep 17 00:00:00 2001 From: Grufoony Date: Wed, 11 Mar 2026 16:57:46 +0100 Subject: [PATCH 1/5] Refactor Dynamics classes --- src/dsf/bindings.cpp | 58 +- src/dsf/mobility/FirstOrderDynamics.cpp | 2181 +++++++++++++++++- src/dsf/mobility/FirstOrderDynamics.hpp | 516 ++++- src/dsf/mobility/RoadDynamics.hpp | 2735 ----------------------- src/dsf/utility/Typedef.hpp | 1 + test/mobility/Test_dynamics.cpp | 132 +- 6 files changed, 2767 insertions(+), 2856 deletions(-) delete mode 100644 src/dsf/mobility/RoadDynamics.hpp diff --git a/src/dsf/bindings.cpp b/src/dsf/bindings.cpp index 7dbd458b..942d4deb 100644 --- a/src/dsf/bindings.cpp +++ b/src/dsf/bindings.cpp @@ -47,6 +47,12 @@ PYBIND11_MODULE(dsf_cpp, m) { .value("DOUBLE_TAIL", dsf::TrafficLightOptimization::DOUBLE_TAIL) .export_values(); + // Bind SpeedFunction enum + pybind11::enum_(mobility, "SpeedFunction") + .value("CUSTOM", dsf::SpeedFunction::CUSTOM) + .value("LINEAR", dsf::SpeedFunction::LINEAR) + .export_values(); + // Bind RoadStatus enum pybind11::enum_(mobility, "RoadStatus") .value("OPEN", dsf::mobility::RoadStatus::OPEN) @@ -441,22 +447,42 @@ PYBIND11_MODULE(dsf_cpp, m) { pybind11::class_(mobility, "Dynamics") // // Constructors are not directly exposed due to the template nature and complexity. // // Users should use derived classes like FirstOrderDynamics. - .def(pybind11::init, - double, - dsf::PathWeight, - std::optional>(), - pybind11::arg("graph"), - pybind11::arg("useCache") = false, - pybind11::arg("seed") = std::nullopt, - pybind11::arg("alpha") = 0., - pybind11::arg("weightFunction") = dsf::PathWeight::TRAVELTIME, - pybind11::arg("weightThreshold") = std::nullopt, - pybind11::keep_alive<1, 2>(), - dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::FirstOrderDynamics") - .c_str()) - // Note: Constructors with std::function parameters are not exposed to avoid stub generation issues + .def( + pybind11::init>(), + pybind11::arg("graph"), + pybind11::arg("useCache") = false, + pybind11::arg("seed") = std::nullopt, + pybind11::keep_alive<1, 2>(), + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::FirstOrderDynamics") + .c_str()) + .def( + "setSpeedFunction", + [](dsf::mobility::FirstOrderDynamics& self, + dsf::SpeedFunction speedFunction, + pybind11::object arg) { + if (speedFunction == dsf::SpeedFunction::LINEAR) { + double alpha = arg.cast(); + self.setSpeedFunction(dsf::SpeedFunction::LINEAR, alpha); + } else if (speedFunction == dsf::SpeedFunction::CUSTOM) { + auto pyFunc = arg.cast>(); + self.setSpeedFunction( + dsf::SpeedFunction::CUSTOM, + [pyFunc]( + std::unique_ptr const& pStreet) -> double { + return pyFunc( + pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); + }); + } + }, + pybind11::arg("speedFunction"), + pybind11::arg("arg"), + "Set the speed function for agents.\n\n" + "Args:\n" + " speedFunction (SpeedFunction): The speed function type (LINEAR or " + "CUSTOM)\n" + " arg: For LINEAR, a float alpha in [0., 1.]. " + "For CUSTOM, a callable(max_speed: float, density: float, length: float) -> " + "float") .def("setName", &dsf::mobility::FirstOrderDynamics::setName, pybind11::arg("name"), diff --git a/src/dsf/mobility/FirstOrderDynamics.cpp b/src/dsf/mobility/FirstOrderDynamics.cpp index 7e6b5a7d..342c0114 100644 --- a/src/dsf/mobility/FirstOrderDynamics.cpp +++ b/src/dsf/mobility/FirstOrderDynamics.cpp @@ -1,13 +1,890 @@ #include "FirstOrderDynamics.hpp" +#include +#include + namespace dsf::mobility { - double FirstOrderDynamics::m_speedFactor(double const& density) const { - return (1. - m_alpha * density); + FirstOrderDynamics::FirstOrderDynamics(RoadNetwork& graph, + bool useCache, + std::optional seed) + : Dynamics(graph, seed), m_bCacheEnabled{useCache} { + if (m_bCacheEnabled) { + if (!std::filesystem::exists(CACHE_FOLDER)) { + std::filesystem::create_directory(CACHE_FOLDER); + } + spdlog::info("Cache enabled (default folder is {})", CACHE_FOLDER); + } + for (auto const& [nodeId, pNode] : this->graph().nodes()) { + m_nodeIndices.push_back(nodeId); + } + for (auto const& [nodeId, weight] : this->m_destinationNodes) { + m_itineraries.emplace(nodeId, std::make_shared(nodeId, nodeId)); + } + std::for_each( + this->graph().edges().cbegin(), + this->graph().edges().cend(), + [this](auto const& pair) { + auto const& pEdge{pair.second}; + auto const edgeId{pair.first}; + // fill turn mapping as [pair.first, [left street Id, straight street Id, right street Id, U self street Id]] + m_turnMapping.emplace(edgeId, std::array{-1, -1, -1, -1}); + // Turn mappings + const auto& srcNodeId = pEdge->target(); + for (auto const& outEdgeId : this->graph().node(srcNodeId)->outgoingEdges()) { + auto const& pStreet{this->graph().edge(outEdgeId)}; + auto const previousStreetId = pStreet->id(); + auto const& delta{pEdge->deltaAngle(pStreet->angle())}; + if (std::abs(delta) < std::numbers::pi) { + if (delta < 0.) { + m_turnMapping[edgeId][dsf::Direction::RIGHT] = previousStreetId; // right + } else if (delta > 0.) { + m_turnMapping[edgeId][dsf::Direction::LEFT] = previousStreetId; // left + } else { + m_turnMapping[edgeId][dsf::Direction::STRAIGHT] = + previousStreetId; // straight + } + } else { + m_turnMapping[edgeId][dsf::Direction::UTURN] = previousStreetId; // U + } + } + }); + } + + std::unique_ptr FirstOrderDynamics::m_killAgent(std::unique_ptr pAgent) { + spdlog::trace("Killing agent {}", *pAgent); + m_travelDTs.push_back({pAgent->distance(), + static_cast(this->time_step() - pAgent->spawnTime())}); + --m_nAgents; + ++m_nKilledAgents; + auto const& streetId = pAgent->streetId(); + if (streetId.has_value()) { + auto const& pStreet{this->graph().edge(streetId.value())}; + auto const& pNode{this->graph().node(pStreet->target())}; + auto [it, bInserted] = m_destinationCounts.insert({pNode->id(), 1}); + if (!bInserted) { + ++it->second; + } + } + return pAgent; + } + + void FirstOrderDynamics::m_updatePath(std::shared_ptr const& pItinerary) { + if (m_bCacheEnabled) { + auto const& file = std::format("{}{}.ity", CACHE_FOLDER, pItinerary->id()); + if (std::filesystem::exists(file)) { + pItinerary->load(file); + spdlog::debug("Loaded cached path for itinerary {}", pItinerary->id()); + return; + } + } + auto const oldSize{pItinerary->path().size()}; + + auto const& path{this->graph().allPathsTo( + pItinerary->destination(), m_weightFunction, m_weightTreshold)}; + pItinerary->setPath(path); + auto const newSize{pItinerary->path().size()}; + if (oldSize > 0 && newSize != oldSize) { + spdlog::debug("Path for itinerary {} changed size from {} to {}", + pItinerary->id(), + oldSize, + newSize); + } + if (m_bCacheEnabled) { + pItinerary->save(std::format("{}{}.ity", CACHE_FOLDER, pItinerary->id())); + spdlog::debug("Saved path in cache for itinerary {}", pItinerary->id()); + } + } + void FirstOrderDynamics::m_addAgentsRandom(std::size_t nAgents) { + m_nAddedAgents += nAgents; + std::uniform_real_distribution uniformDist{0., 1.}; + std::exponential_distribution distDist{1. / + m_meanTravelDistance.value_or(1.)}; + std::exponential_distribution timeDist{1. / m_meanTravelTime.value_or(1.)}; + auto const bUniformSpawn{m_originNodes.empty()}; + if (m_originNodes.size() == 1) { + auto [originId, weight] = m_originNodes.at(0); + this->addAgents(nAgents, nullptr, originId); + return; + } + while (nAgents--) { + if (bUniformSpawn) { + this->addAgent(); + } else { + auto randValue{uniformDist(this->m_generator)}; + for (auto const& [origin, weight] : m_originNodes) { + if (randValue < weight) { + this->addAgent(nullptr, origin); + break; + } + randValue -= weight; + } + } + if (m_meanTravelDistance.has_value()) { + auto const& pAgent{this->m_agents.back()}; + pAgent->setMaxDistance(distDist(this->m_generator)); + } + if (m_meanTravelTime.has_value()) { + auto const& pAgent{this->m_agents.back()}; + pAgent->setMaxTime(timeDist(this->m_generator)); + } + } + } + void FirstOrderDynamics::m_addAgentsODs(std::size_t nAgents) { + if (m_ODs.empty()) { + throw std::runtime_error( + "FirstOrderDynamics::m_addAgentsODs: No origin-destination pairs provided"); + } + m_nAddedAgents += nAgents; + if (m_ODs.size() == 1) { + auto [originId, destinationId, weight] = m_ODs.at(0); + this->addAgents(nAgents, this->itineraries().at(destinationId), originId); + return; + } + std::uniform_real_distribution uniformDist{ + 0., 1.}; // Weight distribution should be normalized to 1 + while (nAgents--) { + Id originId{0}, destinationId{0}; + auto randValue = uniformDist(this->m_generator); + for (auto const& [origin, destination, weight] : m_ODs) { + if (randValue < weight) { + originId = origin; + destinationId = destination; + break; + } + randValue -= weight; + } + this->addAgent(this->itineraries().at(destinationId), originId); + } + } + void FirstOrderDynamics::m_addAgentsRandomODs(std::size_t nAgents) { + m_nAddedAgents += nAgents; + if (m_timeToleranceFactor.has_value() && !m_agents.empty()) { + auto const nStagnantAgents{m_agents.size()}; + spdlog::debug( + "Removing {} stagnant agents that were not inserted since the previous call to " + "addAgentsRandomly().", + nStagnantAgents); + m_agents.clear(); + m_nAgents -= nStagnantAgents; + } + auto const& nSources{m_originNodes.size()}; + auto const& nDestinations{m_destinationNodes.size()}; + spdlog::debug("Init addAgentsRandomly for {} agents from {} nodes to {} nodes.", + nAgents, + nSources, + nDestinations); + if (nSources == 1 && nDestinations == 1 && + std::get(m_originNodes.at(0)) == std::get(m_destinationNodes.at(0))) { + throw std::invalid_argument( + std::format("The only source node {} is also the only destination node.", + std::get(m_originNodes.at(0)))); + } + std::uniform_int_distribution nodeDist{ + 0, static_cast(this->graph().nNodes() - 1)}; + std::uniform_real_distribution uniformDist{0., 1.}; + spdlog::debug("Adding {} agents at time {}.", nAgents, this->time_step()); + while (nAgents--) { + std::optional srcId{std::nullopt}, dstId{std::nullopt}; + + // Select source using weighted random selection + if (nSources == 1) { + srcId = std::get(m_originNodes.at(0)); + } else { + auto randValue = uniformDist(this->m_generator); + for (const auto& [id, weight] : m_originNodes) { + if (randValue < weight) { + srcId = id; + break; + } + randValue -= weight; + } + } + + // Select destination using weighted random selection + if (nDestinations == 1) { + dstId = std::get(m_destinationNodes.at(0)); + } else { + auto randValue = uniformDist(this->m_generator); + for (const auto& [id, weight] : m_destinationNodes) { + if (randValue < weight) { + dstId = id; + break; + } + randValue -= weight; + } + } + + // Fallback to random nodes if selection failed + if (!srcId.has_value()) { + auto nodeIt{this->graph().nodes().begin()}; + std::advance(nodeIt, nodeDist(this->m_generator)); + srcId = nodeIt->first; + } + if (!dstId.has_value()) { + auto nodeIt{this->graph().nodes().begin()}; + std::advance(nodeIt, nodeDist(this->m_generator)); + dstId = nodeIt->first; + } + + // Find the itinerary with the given destination + auto itineraryIt{std::find_if(this->itineraries().cbegin(), + this->itineraries().cend(), + [dstId](const auto& itinerary) { + return itinerary.second->destination() == *dstId; + })}; + if (itineraryIt == this->itineraries().cend()) { + spdlog::error("Itinerary with destination {} not found. Skipping agent.", *dstId); + continue; + } + + // Check if destination is reachable from source + auto const& itinerary = itineraryIt->second; + if (!itinerary->path().contains(*srcId)) { + spdlog::debug("Destination {} not reachable from source {}. Skipping agent.", + *dstId, + *srcId); + continue; + } + + this->addAgent(itineraryIt->second, *srcId); + } + } + + std::optional FirstOrderDynamics::m_nextStreetId( + const std::unique_ptr& pAgent, const std::unique_ptr& pNode) { + spdlog::trace("Computing m_nextStreetId for {}", *pAgent); + auto const& outgoingEdges = pNode->outgoingEdges(); + + // Get current street information + std::optional previousNodeId = std::nullopt; + std::set forbiddenTurns; + double speedCurrent{1.0}; + double lengthCurrent{1.0}; + double stationaryWeightCurrent = 1.0; + double bcCurrent{1.0}; + if (pAgent->streetId().has_value()) { + auto const& pStreetCurrent{this->graph().edge(pAgent->streetId().value())}; + previousNodeId = pStreetCurrent->source(); + forbiddenTurns = pStreetCurrent->forbiddenTurns(); + speedCurrent = pStreetCurrent->maxSpeed(); + lengthCurrent = pStreetCurrent->length(); + stationaryWeightCurrent = pStreetCurrent->stationaryWeight(); + bcCurrent = pStreetCurrent->betweennessCentrality().value_or(1.0); + } + + // Get path targets for non-random agents + std::vector pathTargets; + if (!pAgent->isRandom()) { + pathTargets = pAgent->itinerary()->path().at(pNode->id()); + } + + // Calculate transition probabilities for all valid outgoing edges + std::unordered_map transitionProbabilities; + double cumulativeProbability = 0.0; + + for (const auto outEdgeId : outgoingEdges) { + auto const& pStreetOut{this->graph().edge(outEdgeId)}; + + // Check if this is a valid path target for non-random agents + bool bIsPathTarget = false; + if (!pathTargets.empty()) { + bIsPathTarget = + std::find(pathTargets.cbegin(), pathTargets.cend(), pStreetOut->target()) != + pathTargets.cend(); + } + + if (forbiddenTurns.contains(outEdgeId) && !bIsPathTarget) { + continue; + } + + if (!pathTargets.empty()) { + if (!this->m_errorProbability.has_value() && !bIsPathTarget) { + continue; + } + } + + // Calculate base probability + auto const speedNext{pStreetOut->maxSpeed()}; + auto const lengthNext{pStreetOut->length()}; + auto const bcNext{pStreetOut->betweennessCentrality().value_or(1.0)}; + double const stationaryWeightNext = pStreetOut->stationaryWeight(); + auto const weightRatio{stationaryWeightNext / + stationaryWeightCurrent}; // SQRT (p_i / p_j) + double probability = + std::sqrt((bcCurrent * bcNext) * (speedCurrent / lengthCurrent) * + (speedNext / lengthNext) * weightRatio); + + // Apply error probability for non-random agents + if (this->m_errorProbability.has_value() && !pathTargets.empty()) { + probability *= + (bIsPathTarget + ? (1. - this->m_errorProbability.value()) + : this->m_errorProbability.value() / + static_cast(outgoingEdges.size() - pathTargets.size())); + } + + // Handle U-turns + if (previousNodeId.has_value() && pStreetOut->target() == previousNodeId.value()) { + if (pNode->isRoundabout()) { + probability *= U_TURN_PENALTY_FACTOR; + } else if (!bIsPathTarget) { + continue; // No U-turns allowed + } + } + + transitionProbabilities[pStreetOut->id()] = probability; + cumulativeProbability += probability; + } + + // Select street based on weighted probabilities + if (transitionProbabilities.empty()) { + spdlog::debug("No valid transitions found for {} at {}", *pAgent, *pNode); + return std::nullopt; + } + if (transitionProbabilities.size() == 1) { + auto const& onlyStreetId = transitionProbabilities.cbegin()->first; + spdlog::debug("Only one valid transition for {} at {}: street {}", + *pAgent, + *pNode, + onlyStreetId); + return onlyStreetId; + } + + std::uniform_real_distribution uniformDist{0., cumulativeProbability}; + auto const randValue = uniformDist(this->m_generator); + Id fallbackStreetId; + double accumulated = 0.0; + for (const auto& [targetStreetId, probability] : transitionProbabilities) { + accumulated += probability; + fallbackStreetId = targetStreetId; + if (randValue < accumulated) { + return targetStreetId; + } + } + // Return last one as fallback + spdlog::debug( + "Fallback selection for {} at {}: street {}", *pAgent, *pNode, fallbackStreetId); + return fallbackStreetId; + } + + void FirstOrderDynamics::m_evolveStreet(const std::unique_ptr& pStreet, + bool reinsert_agents) { + auto const nLanes = pStreet->nLanes(); + // Enqueue moving agents if their free time is up + while (!pStreet->movingAgents().empty()) { + auto const& pAgent{pStreet->movingAgents().top()}; + if (pAgent->freeTime() < this->time_step()) { + break; + } + pAgent->setSpeed(0.); + bool bArrived{false}; + if (!pAgent->isRandom()) { + if (pAgent->itinerary()->destination() == pStreet->target()) { + pAgent->updateItinerary(); + } + if (pAgent->itinerary()->destination() == pStreet->target()) { + bArrived = true; + } + } + if (bArrived) { + std::uniform_int_distribution laneDist{0, + static_cast(nLanes - 1)}; + pStreet->enqueue(laneDist(this->m_generator)); + continue; + } + auto const nextStreetId = + this->m_nextStreetId(pAgent, this->graph().node(pStreet->target())); + if (!nextStreetId.has_value()) { + spdlog::debug( + "No next street found for agent {} at node {}", *pAgent, pStreet->target()); + if (pAgent->isRandom()) { + std::uniform_int_distribution laneDist{0, + static_cast(nLanes - 1)}; + pStreet->enqueue(laneDist(this->m_generator)); + continue; + } + this->m_killAgent(pStreet->dequeueMovingAgent()); + continue; + // Grufoony - 09/03/2026 + // The agent is now killed. The old behavior (throw exception) is kept here: + // + // throw std::runtime_error(std::format( + // "No next street found for agent {} at node {}", *pAgent, pStreet->target())); + } + auto const& pNextStreet{this->graph().edge(nextStreetId.value())}; + pAgent->setNextStreetId(pNextStreet->id()); + if (nLanes == 1) { + pStreet->enqueue(0); + continue; + } + auto const direction{pNextStreet->turnDirection(pStreet->angle())}; + switch (direction) { + case Direction::UTURN: + case Direction::LEFT: + pStreet->enqueue(nLanes - 1); + break; + case Direction::RIGHT: + pStreet->enqueue(0); + break; + default: + std::vector weights; + for (auto const& queue : pStreet->exitQueues()) { + weights.push_back(1. / (queue.size() + 1)); + } + // If all weights are the same, make the last 0 + if (std::all_of(weights.begin(), weights.end(), [&](double w) { + return std::abs(w - weights.front()) < + std::numeric_limits::epsilon(); + })) { + weights.back() = 0.; + if (nLanes > 2) { + weights.front() = 0.; + } + } + // Normalize the weights + auto const sum = std::accumulate(weights.begin(), weights.end(), 0.); + for (auto& w : weights) { + w /= sum; + } + std::discrete_distribution laneDist{weights.begin(), weights.end()}; + pStreet->enqueue(laneDist(this->m_generator)); + } + } + auto const& transportCapacity{pStreet->transportCapacity()}; + std::uniform_real_distribution uniformDist{0., 1.}; + for (auto i = 0; i < std::ceil(transportCapacity); ++i) { + if (i == std::ceil(transportCapacity) - 1) { + double integral; + double fractional = std::modf(transportCapacity, &integral); + if (fractional != 0. && uniformDist(this->m_generator) > fractional) { + spdlog::trace("Skipping due to fractional capacity {:.2f} < random value", + fractional); + continue; + } + } + for (auto queueIndex = 0; queueIndex < nLanes; ++queueIndex) { + if (pStreet->queue(queueIndex).empty()) { + continue; + } + // Logger::debug("Taking temp agent"); + auto const& pAgentTemp{pStreet->queue(queueIndex).front()}; + if (pAgentTemp->freeTime() > this->time_step()) { + spdlog::trace("Skipping due to time {} < free time {}", + this->time_step(), + pAgentTemp->freeTime()); + continue; + } + + if (m_timeToleranceFactor.has_value()) { + auto const timeDiff{this->time_step() - pAgentTemp->freeTime()}; + auto const timeTolerance{m_timeToleranceFactor.value() * + std::ceil(pStreet->length() / pStreet->maxSpeed())}; + if (timeDiff > timeTolerance) { + spdlog::debug( + "Time-step {} - {} currently on {} ({} turn - Traffic Light? {}), " + "has been still for more than {} seconds ({} seconds). Killing it.", + this->time_step(), + *pAgentTemp, + *pStreet, + directionToString.at(pStreet->laneMapping().at(queueIndex)), + this->graph().node(pStreet->target())->isTrafficLight(), + timeTolerance, + timeDiff); + // Kill the agent + this->m_killAgent(pStreet->dequeue(queueIndex, this->time_step())); + continue; + } + } + pAgentTemp->setSpeed(0.); + const auto& destinationNode{this->graph().node(pStreet->target())}; + if (destinationNode->isFull()) { + spdlog::trace("Skipping due to full destination node {}", *destinationNode); + continue; + } + if (destinationNode->isTrafficLight()) { + auto& tl = dynamic_cast(*destinationNode); + auto const direction{pStreet->laneMapping().at(queueIndex)}; + if (!tl.isGreen(pStreet->id(), direction)) { + spdlog::trace("Skipping due to red light on street {} and direction {}", + pStreet->id(), + directionToString.at(direction)); + continue; + } + spdlog::debug("Green light on street {} and direction {}", + pStreet->id(), + directionToString.at(direction)); + } else if (destinationNode->isIntersection() && + pAgentTemp->nextStreetId().has_value()) { + auto& intersection = static_cast(*destinationNode); + bool bCanPass{true}; + if (!intersection.streetPriorities().empty()) { + spdlog::debug("Checking priorities for street {} -> {}", + pStreet->source(), + pStreet->target()); + auto const& thisDirection{this->graph() + .edge(pAgentTemp->nextStreetId().value()) + ->turnDirection(pStreet->angle())}; + if (!intersection.streetPriorities().contains(pStreet->id())) { + // I have to check if the agent has right of way + auto const& inNeighbours{destinationNode->ingoingEdges()}; + for (auto const& inEdgeId : inNeighbours) { + auto const& pStreetTemp{this->graph().edge(inEdgeId)}; + if (pStreetTemp->id() == pStreet->id()) { + continue; + } + if (pStreetTemp->nExitingAgents() == 0) { + continue; + } + if (intersection.streetPriorities().contains(pStreetTemp->id())) { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to right of way.", + pStreet->source(), + pStreet->target()); + bCanPass = false; + break; + } else if (thisDirection >= Direction::LEFT) { + // Check if the agent has right of way using direction + // The problem arises only when you have to turn left + for (auto i{0}; i < pStreetTemp->nLanes(); ++i) { + // check queue is not empty and take the top agent + if (pStreetTemp->queue(i).empty()) { + continue; + } + auto const& pAgentTemp2{pStreetTemp->queue(i).front()}; + if (!pAgentTemp2->nextStreetId().has_value()) { + continue; + } + auto const& otherDirection{ + this->graph() + .edge(pAgentTemp2->nextStreetId().value()) + ->turnDirection(this->graph() + .edge(pAgentTemp2->streetId().value()) + ->angle())}; + if (otherDirection < Direction::LEFT) { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to right of " + "way with other agents.", + pStreet->source(), + pStreet->target()); + bCanPass = false; + break; + } + } + } + } + } else if (thisDirection >= Direction::LEFT) { + for (auto const& streetId : intersection.streetPriorities()) { + if (streetId == pStreet->id()) { + continue; + } + auto const& pStreetTemp{this->graph().edge(streetId)}; + for (auto i{0}; i < pStreetTemp->nLanes(); ++i) { + // check queue is not empty and take the top agent + if (pStreetTemp->queue(i).empty()) { + continue; + } + auto const& pAgentTemp2{pStreetTemp->queue(i).front()}; + if (!pAgentTemp2->streetId().has_value()) { + continue; + } + auto const& otherDirection{ + this->graph() + .edge(pAgentTemp2->nextStreetId().value()) + ->turnDirection(this->graph() + .edge(pAgentTemp2->streetId().value()) + ->angle())}; + if (otherDirection < thisDirection) { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to right of " + "way with other agents.", + pStreet->source(), + pStreet->target()); + bCanPass = false; + break; + } + } + } + } + } + if (!bCanPass) { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to right of way", + pStreet->source(), + pStreet->target()); + continue; + } + } + bool bArrived{false}; + if (!(uniformDist(this->m_generator) < + m_passageProbability.value_or(std::numeric_limits::max()))) { + if (pAgentTemp->isRandom()) { + bArrived = true; + } else { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to passage " + "probability", + pStreet->source(), + pStreet->target()); + continue; + } + } + if (!pAgentTemp->isRandom()) { + if (destinationNode->id() == pAgentTemp->itinerary()->destination()) { + bArrived = true; + spdlog::debug("Agent {} has arrived at destination node {}", + pAgentTemp->id(), + destinationNode->id()); + } + } else { + if (!pAgentTemp->nextStreetId().has_value()) { + bArrived = true; + spdlog::debug("Random agent {} has arrived at destination node {}", + pAgentTemp->id(), + destinationNode->id()); + } else if (pAgentTemp->hasArrived(this->time_step())) { + bArrived = true; + } + } + if (bArrived) { + auto pAgent = + this->m_killAgent(pStreet->dequeue(queueIndex, this->time_step())); + ++m_nArrivedAgents; + if (reinsert_agents) { + // reset Agent's values + pAgent->reset(this->time_step()); + this->addAgent(std::move(pAgent)); + } + continue; + } + if (!pAgentTemp->streetId().has_value()) { + spdlog::error("{} has no street id", *pAgentTemp); + } + auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())}; + if (nextStreet->isFull()) { + spdlog::debug( + "Skipping agent emission from street {} -> {} due to full " + "next street: {}", + pStreet->source(), + pStreet->target(), + *nextStreet); + continue; + } + auto pAgent{pStreet->dequeue(queueIndex, this->time_step())}; + spdlog::debug( + "{} at time {} has been dequeued from street {} and enqueued on street {} " + "with free time {}.", + *pAgent, + this->time_step(), + pStreet->id(), + nextStreet->id(), + pAgent->freeTime()); + assert(destinationNode->id() == nextStreet->source()); + if (destinationNode->isIntersection()) { + auto& intersection = dynamic_cast(*destinationNode); + auto const delta{nextStreet->deltaAngle(pStreet->angle())}; + intersection.addAgent(delta, std::move(pAgent)); + } else if (destinationNode->isRoundabout()) { + auto& roundabout = dynamic_cast(*destinationNode); + roundabout.enqueue(std::move(pAgent)); + } + } + } + } + + void FirstOrderDynamics::m_evolveNode(const std::unique_ptr& pNode) { + auto const transportCapacity{pNode->transportCapacity()}; + for (auto i{0}; i < std::ceil(transportCapacity); ++i) { + if (i == std::ceil(transportCapacity) - 1) { + std::uniform_real_distribution uniformDist{0., 1.}; + double integral; + double fractional = std::modf(transportCapacity, &integral); + if (fractional != 0. && uniformDist(this->m_generator) > fractional) { + spdlog::debug("Skipping dequeue from node {} due to transport capacity {}", + pNode->id(), + transportCapacity); + return; + } + } + if (pNode->isIntersection()) { + auto& intersection = dynamic_cast(*pNode); + if (intersection.agents().empty()) { + return; + } + for (auto it{intersection.agents().begin()}; it != intersection.agents().end();) { + auto& pAgent{it->second}; + auto const& nextStreet{this->graph().edge(pAgent->nextStreetId().value())}; + if (nextStreet->isFull()) { + spdlog::debug("Next street is full: {}", *nextStreet); + if (m_forcePriorities) { + spdlog::debug("Forcing priority from {} on {}", *pNode, *nextStreet); + return; + } + ++it; + continue; + } + if (!m_turnCounts.empty() && pAgent->streetId().has_value()) { + ++m_turnCounts[*(pAgent->streetId())][nextStreet->id()]; + } + pAgent->setStreetId(); + pAgent->setSpeed(this->m_speedFunction(nextStreet)); + pAgent->setFreeTime(this->time_step() + + std::ceil(nextStreet->length() / pAgent->speed())); + spdlog::debug( + "{} at time {} has been dequeued from intersection {} and " + "enqueued on street {} with free time {}.", + *pAgent, + this->time_step(), + pNode->id(), + nextStreet->id(), + pAgent->freeTime()); + nextStreet->addAgent(std::move(pAgent), this->time_step()); + it = intersection.agents().erase(it); + break; + } + } else if (pNode->isRoundabout()) { + auto& roundabout = dynamic_cast(*pNode); + if (roundabout.agents().empty()) { + return; + } + auto const& pAgentTemp{roundabout.agents().front()}; + auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())}; + if (!(nextStreet->isFull())) { + if (!m_turnCounts.empty() && pAgentTemp->streetId().has_value()) { + ++m_turnCounts[*(pAgentTemp->streetId())][nextStreet->id()]; + } + auto pAgent{roundabout.dequeue()}; + pAgent->setStreetId(); + pAgent->setSpeed(this->m_speedFunction(nextStreet)); + pAgent->setFreeTime(this->time_step() + + std::ceil(nextStreet->length() / pAgent->speed())); + spdlog::debug( + "An agent at time {} has been dequeued from roundabout {} and " + "enqueued on street {} with free time {}: {}", + this->time_step(), + pNode->id(), + nextStreet->id(), + pAgent->freeTime(), + *pAgent); + nextStreet->addAgent(std::move(pAgent), this->time_step()); + } else { + return; + } + } + } + } + void FirstOrderDynamics::m_evolveAgents() { + if (m_agents.empty()) { + spdlog::trace("No agents to process."); + return; + } + std::uniform_int_distribution nodeDist{ + 0, static_cast(this->graph().nNodes() - 1)}; + spdlog::debug("Processing {} agents", m_agents.size()); + for (auto itAgent{m_agents.begin()}; itAgent != m_agents.end();) { + auto& pAgent{*itAgent}; + if (!pAgent->srcNodeId().has_value()) { + auto nodeIt{this->graph().nodes().begin()}; + std::advance(nodeIt, nodeDist(this->m_generator)); + pAgent->setSrcNodeId(nodeIt->second->id()); + } + auto const& pSourceNode{this->graph().node(*(pAgent->srcNodeId()))}; + if (pSourceNode->isFull()) { + spdlog::debug("Skipping {} due to full source {}", *pAgent, *pSourceNode); + ++itAgent; + continue; + } + if (!pAgent->nextStreetId().has_value()) { + spdlog::debug("No next street id, generating a random one"); + auto const nextStreetId{this->m_nextStreetId(pAgent, pSourceNode)}; + if (!nextStreetId.has_value()) { + spdlog::debug( + "No next street found for agent {} at node {}", *pAgent, pSourceNode->id()); + itAgent = m_agents.erase(itAgent); + continue; + } + pAgent->setNextStreetId(nextStreetId.value()); + } + // spdlog::debug("Checking next street {}", pAgent->nextStreetId().value()); + auto const& nextStreet{ + this->graph().edge(pAgent->nextStreetId().value())}; // next street + if (nextStreet->isFull()) { + ++itAgent; + spdlog::debug("Skipping {} due to full input {}", *pAgent, *nextStreet); + continue; + } + // spdlog::debug("Adding agent on the source node"); + if (pSourceNode->isIntersection()) { + auto& intersection = dynamic_cast(*pSourceNode); + intersection.addAgent(0., std::move(pAgent)); + } else if (pSourceNode->isRoundabout()) { + auto& roundabout = dynamic_cast(*pSourceNode); + roundabout.enqueue(std::move(pAgent)); + } + itAgent = m_agents.erase(itAgent); + } + spdlog::debug("There are {} agents left in the list.", m_agents.size()); + } + + void FirstOrderDynamics::m_initStreetTable() const { + if (!this->database()) { + throw std::runtime_error( + "No database connected. Call connectDataBase() before saving data."); + } + // Create table if it doesn't exist + this->database()->exec( + "CREATE TABLE IF NOT EXISTS road_data (" + "id INTEGER PRIMARY KEY AUTOINCREMENT, " + "simulation_id INTEGER NOT NULL, " + "datetime TEXT NOT NULL, " + "time_step INTEGER NOT NULL, " + "street_id INTEGER NOT NULL, " + "coil TEXT, " + "density_vpk REAL, " + "avg_speed_kph REAL, " + "std_speed_kph REAL, " + "n_observations INTEGER, " + "counts INTEGER, " + "queue_length INTEGER)"); + + spdlog::info("Initialized road_data table in the database."); + } + void FirstOrderDynamics::m_initAvgStatsTable() const { + if (!this->database()) { + throw std::runtime_error( + "No database connected. Call connectDataBase() before saving data."); + } + // Create table if it doesn't exist + this->database()->exec( + "CREATE TABLE IF NOT EXISTS avg_stats (" + "id INTEGER PRIMARY KEY AUTOINCREMENT, " + "simulation_id INTEGER NOT NULL, " + "datetime TEXT NOT NULL, " + "time_step INTEGER NOT NULL, " + "n_ghost_agents INTEGER NOT NULL, " + "n_agents INTEGER NOT NULL, " + "mean_speed_kph REAL, " + "std_speed_kph REAL, " + "mean_density_vpk REAL NOT NULL, " + "std_density_vpk REAL NOT NULL)"); + + spdlog::info("Initialized avg_stats table in the database."); } - double FirstOrderDynamics::m_streetEstimatedTravelTime( - std::unique_ptr const& pStreet) const { - return pStreet->length() / - (pStreet->maxSpeed() * m_speedFactor(pStreet->density(true))); + void FirstOrderDynamics::m_initTravelDataTable() const { + if (!this->database()) { + throw std::runtime_error( + "No database connected. Call connectDataBase() before saving data."); + } + // Create table if it doesn't exist + this->database()->exec( + "CREATE TABLE IF NOT EXISTS travel_data (" + "id INTEGER PRIMARY KEY AUTOINCREMENT, " + "simulation_id INTEGER NOT NULL, " + "datetime TEXT NOT NULL, " + "time_step INTEGER NOT NULL, " + "distance_m REAL NOT NULL, " + "travel_time_s REAL NOT NULL)"); + + spdlog::info("Initialized travel_data table in the database."); } void FirstOrderDynamics::m_dumpSimInfo() const { // Dump simulation info (parameters) to the database, if connected @@ -43,8 +920,8 @@ namespace dsf::mobility { "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"); insertSimStmt.bind(1, static_cast(this->id())); insertSimStmt.bind(2, this->name()); - insertSimStmt.bind(3, m_alpha); - insertSimStmt.bind(4, m_speedFluctuationSTD); + insertSimStmt.bind(3); // alpha (removed) + insertSimStmt.bind(4); // speed_fluctuation_std (removed) switch (this->m_pathWeight) { case PathWeight::LENGTH: insertSimStmt.bind(5, "LENGTH"); @@ -88,51 +965,1267 @@ namespace dsf::mobility { insertSimStmt.bind(15, this->m_bSaveTravelData); insertSimStmt.exec(); } - FirstOrderDynamics::FirstOrderDynamics(RoadNetwork& graph, - bool useCache, - std::optional seed, - double alpha, - PathWeight const weightFunction, - std::optional weightTreshold) - : RoadDynamics(graph, useCache, seed, weightFunction, weightTreshold), - m_alpha{alpha}, - m_speedFluctuationSTD{0.} { - if (alpha < 0. || alpha > 1.) { + void FirstOrderDynamics::m_dumpNetwork() const { + if (!this->database()) { + throw std::runtime_error( + "No database connected. Call connectDataBase() before saving data."); + } + // Check if edges and nodes tables already exists + SQLite::Statement edgesQuery( + *this->database(), + "SELECT name FROM sqlite_master WHERE type='table' AND name='edges';"); + SQLite::Statement nodesQuery( + *this->database(), + "SELECT name FROM sqlite_master WHERE type='table' AND name='nodes';"); + bool edgesTableExists = edgesQuery.executeStep(); + bool nodesTableExists = nodesQuery.executeStep(); + if (edgesTableExists && nodesTableExists) { + spdlog::info( + "Edges and nodes tables already exist in the database. Skipping network dump."); + return; + } + + // Create edges table + this->database()->exec( + "CREATE TABLE IF NOT EXISTS edges (" + "id INTEGER PRIMARY KEY, " + "source INTEGER NOT NULL, " + "target INTEGER NOT NULL, " + "length REAL NOT NULL, " + "maxspeed REAL NOT NULL, " + "name TEXT, " + "nlanes INTEGER NOT NULL, " + "geometry TEXT NOT NULL)"); + // Create nodes table + this->database()->exec( + "CREATE TABLE IF NOT EXISTS nodes (" + "id INTEGER PRIMARY KEY, " + "type TEXT, " + "geometry TEXT)"); + + // Insert edges + SQLite::Statement insertEdgeStmt(*this->database(), + "INSERT INTO edges (id, source, target, length, " + "maxspeed, name, nlanes, geometry) " + "VALUES (?, ?, ?, ?, ?, ?, ?, ?);"); + for (const auto& [edgeId, pEdge] : this->graph().edges()) { + insertEdgeStmt.bind(1, static_cast(edgeId)); + insertEdgeStmt.bind(2, static_cast(pEdge->source())); + insertEdgeStmt.bind(3, static_cast(pEdge->target())); + insertEdgeStmt.bind(4, pEdge->length()); + insertEdgeStmt.bind(5, pEdge->maxSpeed()); + insertEdgeStmt.bind(6, pEdge->name()); + insertEdgeStmt.bind(7, pEdge->nLanes()); + insertEdgeStmt.bind(8, std::format("{}", pEdge->geometry())); + insertEdgeStmt.exec(); + insertEdgeStmt.reset(); + } + // Insert nodes + SQLite::Statement insertNodeStmt( + *this->database(), "INSERT INTO nodes (id, type, geometry) VALUES (?, ?, ?);"); + for (const auto& [nodeId, pNode] : this->graph().nodes()) { + insertNodeStmt.bind(1, static_cast(nodeId)); + if (pNode->isTrafficLight()) { + insertNodeStmt.bind(2, "traffic_light"); + } else if (pNode->isRoundabout()) { + insertNodeStmt.bind(2, "roundabout"); + } else { + insertNodeStmt.bind(2); + } + if (pNode->geometry().has_value()) { + insertNodeStmt.bind(3, std::format("{}", *pNode->geometry())); + } else { + insertNodeStmt.bind(3); + } + insertNodeStmt.exec(); + insertNodeStmt.reset(); + } + } + + void FirstOrderDynamics::setErrorProbability(double errorProbability) { + if (errorProbability < 0. || errorProbability > 1.) { throw std::invalid_argument( - std::format("The minimum speed ratio ({}) must be in [0, 1[", alpha)); + std::format("The error probability ({}) must be in [0, 1]", errorProbability)); } - double globMaxTimePenalty{0.}; - for (const auto& [streetId, pStreet] : this->graph().edges()) { - globMaxTimePenalty = - std::max(globMaxTimePenalty, - std::ceil(pStreet->length() / ((1. - m_alpha) * pStreet->maxSpeed()))); + m_errorProbability = errorProbability; + } + void FirstOrderDynamics::setPassageProbability(double passageProbability) { + if (passageProbability < 0. || passageProbability > 1.) { + throw std::invalid_argument(std::format( + "The passage probability ({}) must be in [0, 1]", passageProbability)); + } + m_passageProbability = passageProbability; + } + void FirstOrderDynamics::killStagnantAgents(double timeToleranceFactor) { + if (timeToleranceFactor <= 0.) { + throw std::invalid_argument(std::format( + "The time tolerance factor ({}) must be positive", timeToleranceFactor)); + } + m_timeToleranceFactor = timeToleranceFactor; + } + void FirstOrderDynamics::setWeightFunction(PathWeight const pathWeight, + std::optional weightTreshold) { + m_pathWeight = pathWeight; + switch (pathWeight) { + case PathWeight::LENGTH: + m_weightFunction = [](std::unique_ptr const& pStreet) { + return pStreet->length(); + }; + m_weightTreshold = weightTreshold.value_or(1.); + break; + case PathWeight::TRAVELTIME: + m_weightFunction = [this](std::unique_ptr const& pStreet) { + return this->m_streetEstimatedTravelTime(pStreet); + }; + m_weightTreshold = weightTreshold.value_or(0.0069); + break; + case PathWeight::WEIGHT: + m_weightFunction = [](std::unique_ptr const& pStreet) { + return pStreet->weight(); + }; + m_weightTreshold = weightTreshold.value_or(1.); + break; + default: + spdlog::error("Invalid weight function. Defaulting to traveltime"); + m_weightFunction = [this](std::unique_ptr const& pStreet) { + return this->m_streetEstimatedTravelTime(pStreet); + }; + m_weightTreshold = weightTreshold.value_or(0.0069); + break; + } + } + void FirstOrderDynamics::setOriginNodes( + std::unordered_map const& originNodes) { + m_originNodes.clear(); + m_originNodes.reserve(originNodes.size()); + if (originNodes.empty()) { + // If no origin nodes are provided, try to set origin nodes basing on streets' stationary weights + double totalStationaryWeight = 0.0; + for (auto const& [edgeId, pEdge] : this->graph().edges()) { + auto const& weight = pEdge->stationaryWeight(); + if (weight <= 0.) { + continue; + } + m_originNodes.push_back({pEdge->source(), weight}); + totalStationaryWeight += weight; + } + for (auto& [nodeId, weight] : m_originNodes) { + weight /= totalStationaryWeight; + } + return; } - if (globMaxTimePenalty > static_cast(std::numeric_limits::max())) { - throw std::overflow_error( - std::format("The maximum time penalty ({}) is greater than the " - "maximum value of delay_t ({})", - globMaxTimePenalty, - std::numeric_limits::max())); + auto const sumWeights = std::accumulate( + originNodes.begin(), originNodes.end(), 0., [](double sum, auto const& pair) { + return sum + pair.second; + }); + if (sumWeights <= 0.) { + throw std::invalid_argument( + std::format("The sum of the weights ({}) must be positive", sumWeights)); + } + if (sumWeights == 1.) { + std::copy( + originNodes.begin(), originNodes.end(), std::back_inserter(m_originNodes)); + return; + } + std::transform(originNodes.begin(), + originNodes.end(), + std::back_inserter(m_originNodes), + [sumWeights](auto const& pair) -> std::pair { + return {pair.first, pair.second / sumWeights}; + }); + } + void FirstOrderDynamics::setDestinationNodes( + std::unordered_map const& destinationNodes) { + m_itineraries.clear(); + m_destinationNodes.clear(); + m_destinationNodes.reserve(destinationNodes.size()); + auto sumWeights{0.}; + std::for_each(destinationNodes.begin(), + destinationNodes.end(), + [this, &sumWeights](auto const& pair) -> void { + sumWeights += pair.second; + this->addItinerary(pair.first, pair.first); + }); + if (sumWeights <= 0.) { + throw std::invalid_argument( + std::format("The sum of the weights ({}) must be positive", sumWeights)); + } + if (sumWeights == 1.) { + std::copy(destinationNodes.begin(), + destinationNodes.end(), + std::back_inserter(m_destinationNodes)); + return; + } + std::transform(destinationNodes.begin(), + destinationNodes.end(), + std::back_inserter(m_destinationNodes), + [sumWeights](auto const& pair) -> std::pair { + return {pair.first, pair.second / sumWeights}; + }); + } + void FirstOrderDynamics::initTurnCounts() { + if (!m_turnCounts.empty()) { + throw std::runtime_error("Turn counts have already been initialized."); + } + for (auto const& [edgeId, pEdge] : this->graph().edges()) { + auto const& pTargetNode{this->graph().node(pEdge->target())}; + for (auto const& nextEdgeId : pTargetNode->outgoingEdges()) { + spdlog::debug("Initializing turn count for edge {} -> {}", edgeId, nextEdgeId); + m_turnCounts[edgeId][nextEdgeId] = 0; + } + } + } + // You may wonder why not just use one function... + // Never trust the user! + // Jokes aside, the init is necessary because it allocates the memory for the first time and + // turn counts are not incremented if the map is empty for performance reasons. + void FirstOrderDynamics::resetTurnCounts() { + if (m_turnCounts.empty()) { + throw std::runtime_error("Turn counts have not been initialized."); + } + for (auto const& [edgeId, pEdge] : this->graph().edges()) { + auto const& pTargetNode{this->graph().node(pEdge->target())}; + for (auto const& nextEdgeId : pTargetNode->outgoingEdges()) { + m_turnCounts[edgeId][nextEdgeId] = 0; + } + } + } + + void FirstOrderDynamics::saveData(std::time_t const savingInterval, + bool const saveAverageStats, + bool const saveStreetData, + bool const saveTravelData) { + m_savingInterval = savingInterval; + m_bSaveAverageStats = saveAverageStats; + m_bSaveStreetData = saveStreetData; + m_bSaveTravelData = saveTravelData; + + // Initialize the required tables + if (saveStreetData) { + m_initStreetTable(); + } + if (saveAverageStats) { + m_initAvgStatsTable(); + } + if (saveTravelData) { + m_initTravelDataTable(); + } + + this->m_dumpSimInfo(); + this->m_dumpNetwork(); + + spdlog::info( + "Data saving configured: interval={}s, avg_stats={}, street_data={}, " + "travel_data={}", + savingInterval, + saveAverageStats, + saveStreetData, + saveTravelData); + } + + void FirstOrderDynamics::setDestinationNodes( + std::initializer_list destinationNodes) { + m_itineraries.clear(); + auto const numNodes{destinationNodes.size()}; + m_destinationNodes.clear(); + m_destinationNodes.reserve(numNodes); + std::for_each(destinationNodes.begin(), + destinationNodes.end(), + [this, &numNodes](auto const& nodeId) -> void { + this->m_destinationNodes.push_back({nodeId, 1. / numNodes}); + this->addItinerary(nodeId, nodeId); + }); + } + void FirstOrderDynamics::setODs(std::vector> const& ODs) { + m_ODs.clear(); + auto const sumWeights = std::accumulate( + ODs.begin(), ODs.end(), 0., [this](double sum, auto const& tuple) { + // Add itineraries while summing weights + if (!this->itineraries().contains(std::get<1>(tuple))) { + this->addItinerary(std::get<1>(tuple), std::get<1>(tuple)); + } + return sum + std::get<2>(tuple); + }); + if (sumWeights <= 0.) { + throw std::invalid_argument( + std::format("The sum of the weights ({}) must be positive", sumWeights)); + } + if (sumWeights == 1.) { + std::copy(ODs.begin(), ODs.end(), std::back_inserter(m_ODs)); + return; + } + // Copy but divide by weights sum + std::transform(ODs.begin(), + ODs.end(), + std::back_inserter(m_ODs), + [sumWeights](auto const& tuple) { + return std::make_tuple(std::get<0>(tuple), + std::get<1>(tuple), + std::get<2>(tuple) / sumWeights); + }); + } + + void FirstOrderDynamics::updatePaths(bool const throw_on_empty) { + spdlog::debug("Init updating paths..."); + tbb::concurrent_vector emptyItineraries; + tbb::parallel_for_each( + this->itineraries().cbegin(), + this->itineraries().cend(), + [this, throw_on_empty, &emptyItineraries](auto const& pair) -> void { + auto const& pItinerary{pair.second}; + this->m_updatePath(pItinerary); + if (pItinerary->empty()) { + if (!throw_on_empty) { + spdlog::warn("No path found for itinerary {} with destination node {}", + pItinerary->id(), + pItinerary->destination()); + emptyItineraries.push_back(pItinerary->id()); + return; + } + throw std::runtime_error( + std::format("No path found for itinerary {} with destination node {}", + pItinerary->id(), + pItinerary->destination())); + } + }); + if (!emptyItineraries.empty()) { + spdlog::warn("Removing {} itineraries with no valid path from the dynamics.", + emptyItineraries.size()); + for (auto const& id : emptyItineraries) { + auto const destination = m_itineraries.at(id)->destination(); + std::erase_if(m_ODs, [destination](auto const& tuple) { + return std::get<1>(tuple) == destination; + }); + std::erase_if(m_destinationNodes, [destination](auto const& tuple) { + return std::get<0>(tuple) == destination; + }); + std::erase_if(m_originNodes, [destination](auto const& tuple) { + return std::get<0>(tuple) == destination; + }); + m_itineraries.erase(id); + } + } + spdlog::debug("End updating paths."); + } + + void FirstOrderDynamics::addAgentsUniformly(std::size_t nAgents, + std::optional optItineraryId) { + m_nAddedAgents += nAgents; + if (m_timeToleranceFactor.has_value() && !m_agents.empty()) { + auto const nStagnantAgents{m_agents.size()}; + spdlog::debug( + "Removing {} stagnant agents that were not inserted since the previous call to " + "addAgentsUniformly().", + nStagnantAgents); + m_agents.clear(); + m_nAgents -= nStagnantAgents; + } + if (optItineraryId.has_value() && !this->itineraries().contains(*optItineraryId)) { + throw std::invalid_argument( + std::format("No itineraries available. Cannot add agents with itinerary id {}", + optItineraryId.value())); + } + bool const bRandomItinerary{!optItineraryId.has_value() && + !this->itineraries().empty()}; + std::shared_ptr pItinerary; + std::uniform_int_distribution itineraryDist{ + 0, this->itineraries().size() - 1}; + std::uniform_int_distribution streetDist{0, this->graph().nEdges() - 1}; + if (this->nAgents() + nAgents > this->graph().capacity()) { + throw std::overflow_error(std::format( + "Cannot add {} agents. The graph has currently {} with a maximum capacity of " + "{}.", + nAgents, + this->nAgents(), + this->graph().capacity())); + } + for (std::size_t i{0}; i < nAgents; ++i) { + if (bRandomItinerary) { + auto itineraryIt{this->itineraries().cbegin()}; + std::advance(itineraryIt, itineraryDist(this->m_generator)); + pItinerary = itineraryIt->second; + } + auto streetIt = this->graph().edges().begin(); + while (true) { + auto step = streetDist(this->m_generator); + std::advance(streetIt, step); + if (!(streetIt->second->isFull())) { + break; + } + streetIt = this->graph().edges().begin(); + } + auto const& street{streetIt->second}; + this->addAgent(pItinerary, street->source()); + auto& pAgent{this->m_agents.back()}; + pAgent->setStreetId(street->id()); + pAgent->setSpeed(this->m_speedFunction(streetIt->second)); + pAgent->setFreeTime(this->time_step() + + std::ceil(street->length() / pAgent->speed())); + street->addAgent(std::move(pAgent), this->time_step()); + this->m_agents.pop_back(); } } - void FirstOrderDynamics::setAgentSpeed(std::unique_ptr const& pAgent) { - const auto& street{this->graph().edge(pAgent->streetId().value())}; - double speed{street->maxSpeed() * this->m_speedFactor(street->density(true))}; - if (m_speedFluctuationSTD > 0.) { - std::normal_distribution speedDist{speed, speed * m_speedFluctuationSTD}; - speed = speedDist(this->m_generator); + void FirstOrderDynamics::addAgent(std::unique_ptr pAgent) { + m_agents.push_back(std::move(pAgent)); + ++m_nAgents; + ++m_nInsertedAgents; + spdlog::trace("Added {}", *m_agents.back()); + auto const& optNodeId{m_agents.back()->srcNodeId()}; + if (optNodeId.has_value()) { + auto [it, bInserted] = m_originCounts.insert({*optNodeId, 1}); + if (!bInserted) { + ++it->second; + } } - speed < 0. ? pAgent->setSpeed(street->maxSpeed() * (1. - m_alpha)) - : pAgent->setSpeed(speed); } - void FirstOrderDynamics::setSpeedFluctuationSTD(double speedFluctuationSTD) { - if (speedFluctuationSTD < 0.) { + void FirstOrderDynamics::addAgents(std::size_t const nAgents, + AgentInsertionMethod const mode) { + switch (mode) { + case AgentInsertionMethod::RANDOM: + this->m_addAgentsRandom(nAgents); + break; + case AgentInsertionMethod::ODS: + this->m_addAgentsODs(nAgents); + break; + case AgentInsertionMethod::RANDOM_ODS: + this->m_addAgentsRandomODs(nAgents); + break; + default: + throw std::runtime_error( + "Cannot add agents without a valid insertion methods. Possible values are " + "\"RANDOM\", \"ODS\" and \"RANDOM_ODS\""); + } + } + + void FirstOrderDynamics::addItinerary(std::shared_ptr itinerary) { + if (m_itineraries.contains(itinerary->id())) { throw std::invalid_argument( - std::format("The speed fluctuation standard deviation ({}) must be positive", - speedFluctuationSTD)); + std::format("Itinerary with id {} already exists.", itinerary->id())); + } + m_itineraries.emplace(itinerary->id(), std::move(itinerary)); + } + + void FirstOrderDynamics::evolve(bool const reinsert_agents) { + auto const n_threads{std::max(1, this->concurrency())}; + std::atomic mean_speed{0.}, mean_density{0.}; + std::atomic std_speed{0.}, std_density{0.}; + std::atomic nValidEdges{0}; + bool const bComputeStats = this->database() != nullptr && + m_savingInterval.has_value() && + (m_savingInterval.value() == 0 || + this->time_step() % m_savingInterval.value() == 0); + + // Struct to collect street data for batch insert after parallel section + struct StreetDataRecord { + Id streetId; + std::optional coilName; + double density; + std::optional avgSpeed; + std::optional stdSpeed; + std::optional nObservations; + std::optional counts; + std::size_t queueLength; + }; + tbb::concurrent_vector streetDataRecords; + + spdlog::debug("Init evolve at time {}", this->time_step()); + // move the first agent of each street queue, if possible, putting it in the next node + bool const bUpdateData = m_dataUpdatePeriod.has_value() && + this->time_step() % m_dataUpdatePeriod.value() == 0; + auto const numNodes{this->graph().nNodes()}; + auto const numEdges{this->graph().nEdges()}; + + const auto grainSize = std::max(1, numNodes / n_threads); + this->m_taskArena.execute([&] { + tbb::parallel_for( + tbb::blocked_range(0, numNodes, grainSize), + [&](const tbb::blocked_range& range) { + for (std::size_t i = range.begin(); i != range.end(); ++i) { + auto const& pNode = this->graph().node(m_nodeIndices[i]); + for (auto const& inEdgeId : pNode->ingoingEdges()) { + auto const& pStreet{this->graph().edge(inEdgeId)}; + if (bUpdateData && pNode->isTrafficLight()) { + if (!m_queuesAtTrafficLights.contains(inEdgeId)) { + auto& tl = dynamic_cast(*pNode); + assert(!tl.cycles().empty()); + for (auto const& [id, pair] : tl.cycles()) { + for (auto const& [direction, cycle] : pair) { + m_queuesAtTrafficLights[id].emplace(direction, 0.); + } + } + } + for (auto& [direction, value] : m_queuesAtTrafficLights.at(inEdgeId)) { + value += pStreet->nExitingAgents(direction, true); + } + } + m_evolveStreet(pStreet, reinsert_agents); + if (bComputeStats) { + auto const& density{pStreet->density() * 1e3}; + + auto const speedMeasure = pStreet->meanSpeed(true); + if (speedMeasure.is_valid) { + auto const speed = speedMeasure.mean * 3.6; // to kph + auto const speed_std = speedMeasure.std * 3.6; + if (m_bSaveAverageStats) { + mean_speed.fetch_add(speed, std::memory_order_relaxed); + std_speed.fetch_add(speed * speed + speed_std * speed_std, + std::memory_order_relaxed); + + ++nValidEdges; + } + } + if (m_bSaveAverageStats) { + mean_density.fetch_add(density, std::memory_order_relaxed); + std_density.fetch_add(density * density, std::memory_order_relaxed); + } + + if (m_bSaveStreetData) { + // Collect data for batch insert after parallel section + StreetDataRecord record; + record.streetId = pStreet->id(); + record.density = density; + if (pStreet->hasCoil()) { + record.coilName = pStreet->counterName(); + record.counts = pStreet->counts(); + pStreet->resetCounter(); + } + if (speedMeasure.is_valid) { + record.avgSpeed = speedMeasure.mean * 3.6; // to kph + record.stdSpeed = speedMeasure.std * 3.6; + record.nObservations = speedMeasure.n; + } + record.queueLength = pStreet->nExitingAgents(); + streetDataRecords.push_back(record); + } + } + } + } + }); + }); + spdlog::debug("Pre-nodes"); + // Move transport capacity agents from each node + this->m_taskArena.execute([&] { + tbb::parallel_for(tbb::blocked_range(0, numNodes, grainSize), + [&](const tbb::blocked_range& range) { + for (size_t i = range.begin(); i != range.end(); ++i) { + const auto& pNode = this->graph().node(m_nodeIndices[i]); + m_evolveNode(pNode); + if (pNode->isTrafficLight()) { + auto& tl = dynamic_cast(*pNode); + ++tl; + } + } + }); + }); + this->m_evolveAgents(); + + if (bComputeStats) { + // Batch insert street data collected during parallel section + if (m_bSaveStreetData) { + SQLite::Transaction transaction(*this->database()); + SQLite::Statement insertStmt( + *this->database(), + "INSERT INTO road_data (datetime, time_step, simulation_id, street_id, " + "coil, density_vpk, avg_speed_kph, std_speed_kph, n_observations, counts, " + "queue_length) " + "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"); + + for (auto const& record : streetDataRecords) { + insertStmt.bind(1, this->strDateTime()); + insertStmt.bind(2, static_cast(this->time_step())); + insertStmt.bind(3, static_cast(this->id())); + insertStmt.bind(4, static_cast(record.streetId)); + if (record.coilName.has_value()) { + insertStmt.bind(5, record.coilName.value()); + } else { + insertStmt.bind(5); + } + insertStmt.bind(6, record.density); + if (record.avgSpeed.has_value()) { + insertStmt.bind(7, record.avgSpeed.value()); + insertStmt.bind(8, record.stdSpeed.value()); + } else { + insertStmt.bind(7); + insertStmt.bind(8); + } + insertStmt.bind(9, static_cast(record.nObservations.value_or(0))); + if (record.counts.has_value()) { + insertStmt.bind(10, static_cast(record.counts.value())); + } else { + insertStmt.bind(10); + } + insertStmt.bind(11, static_cast(record.queueLength)); + insertStmt.exec(); + insertStmt.reset(); + } + transaction.commit(); + } + + if (m_bSaveTravelData) { // Begin transaction for better performance + SQLite::Transaction transaction(*this->database()); + SQLite::Statement insertStmt(*this->database(), + "INSERT INTO travel_data (datetime, time_step, " + "simulation_id, distance_m, travel_time_s) " + "VALUES (?, ?, ?, ?, ?)"); + + for (auto const& [distance, time] : m_travelDTs) { + insertStmt.bind(1, this->strDateTime()); + insertStmt.bind(2, static_cast(this->time_step())); + insertStmt.bind(3, static_cast(this->id())); + insertStmt.bind(4, distance); + insertStmt.bind(5, time); + insertStmt.exec(); + insertStmt.reset(); + } + transaction.commit(); + m_travelDTs.clear(); + } + + if (m_bSaveAverageStats) { // Average Stats Table + mean_speed.store(mean_speed.load() / nValidEdges.load()); + mean_density.store(mean_density.load() / numEdges); + { + double std_speed_val = std_speed.load(); + double mean_speed_val = mean_speed.load(); + std_speed.store(std::sqrt(std_speed_val / nValidEdges.load() - + mean_speed_val * mean_speed_val)); + } + { + double std_density_val = std_density.load(); + double mean_density_val = mean_density.load(); + std_density.store(std::sqrt(std_density_val / numEdges - + mean_density_val * mean_density_val)); + } + SQLite::Statement insertStmt( + *this->database(), + "INSERT INTO avg_stats (" + "simulation_id, datetime, time_step, n_ghost_agents, n_agents, " + "mean_speed_kph, std_speed_kph, mean_density_vpk, std_density_vpk) " + "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?)"); + insertStmt.bind(1, static_cast(this->id())); + insertStmt.bind(2, this->strDateTime()); + insertStmt.bind(3, static_cast(this->time_step())); + insertStmt.bind(4, static_cast(m_agents.size())); + insertStmt.bind(5, static_cast(this->nAgents())); + if (nValidEdges.load() > 0) { + insertStmt.bind(6, mean_speed); + insertStmt.bind(7, std_speed); + } else { + insertStmt.bind(6); + insertStmt.bind(7); + } + insertStmt.bind(8, mean_density); + insertStmt.bind(9, std_density); + insertStmt.exec(); + } + // Special case: if m_savingInterval == 0, it was a triggered saveData() call, so we need to reset all flags + if (m_savingInterval.value() == 0) { + m_savingInterval.reset(); + m_bSaveStreetData = false; + m_bSaveTravelData = false; + m_bSaveAverageStats = false; + } } - m_speedFluctuationSTD = speedFluctuationSTD; + + Dynamics::m_evolve(); } -} // namespace dsf::mobility \ No newline at end of file + + void FirstOrderDynamics::m_trafficlightSingleTailOptimizer( + double const& beta, std::optional& logStream) { + assert(beta >= 0. && beta <= 1.); + if (logStream.has_value()) { + *logStream << std::format( + "Init Traffic Lights optimization (SINGLE TAIL) - Time {} - Alpha {}\n", + this->time_step(), + beta); + } + for (auto const& [nodeId, pNode] : this->graph().nodes()) { + if (!pNode->isTrafficLight()) { + continue; + } + auto& tl = dynamic_cast(*pNode); + + auto const& inNeighbours{pNode->ingoingEdges()}; + + // Default is RIGHTANDSTRAIGHT - LEFT phases for both priority and non-priority + std::array inputPrioritySum{0., 0.}, inputNonPrioritySum{0., 0.}; + bool isPrioritySinglePhase{false}, isNonPrioritySinglePhase{false}; + + for (const auto& streetId : inNeighbours) { + if (tl.cycles().at(streetId).contains(Direction::ANY)) { + tl.streetPriorities().contains(streetId) ? isPrioritySinglePhase = true + : isNonPrioritySinglePhase = true; + } + } + if (isPrioritySinglePhase && logStream.has_value()) { + *logStream << "\tFound a single phase for priority streets.\n"; + } + if (isNonPrioritySinglePhase && logStream.has_value()) { + *logStream << "\tFound a single phase for non-priority streets.\n"; + } + + for (const auto& streetId : inNeighbours) { + for (auto const& [direction, tail] : m_queuesAtTrafficLights.at(streetId)) { + if (tl.streetPriorities().contains(streetId)) { + if (isPrioritySinglePhase) { + inputPrioritySum[0] += tail; + } else { + if (direction == Direction::LEFT || + direction == Direction::LEFTANDSTRAIGHT) { + inputPrioritySum[1] += tail; + } else { + inputPrioritySum[0] += tail; + } + } + } else { + if (isNonPrioritySinglePhase) { + inputNonPrioritySum[0] += tail; + } else { + if (direction == Direction::LEFT || + direction == Direction::LEFTANDSTRAIGHT) { + inputNonPrioritySum[1] += tail; + } else { + inputNonPrioritySum[0] += tail; + } + } + } + } + } + { + // Sum normalization + auto const sum{inputPrioritySum[0] + inputPrioritySum[1] + + inputNonPrioritySum[0] + inputNonPrioritySum[1]}; + if (sum == 0.) { + continue; + } + inputPrioritySum[0] /= sum; + inputPrioritySum[1] /= sum; + inputNonPrioritySum[0] /= sum; + inputNonPrioritySum[1] /= sum; + + // int const cycleTime{(1. - alpha) * tl.cycleTime()}; + + inputPrioritySum[0] *= beta; + inputPrioritySum[1] *= beta; + inputNonPrioritySum[0] *= beta; + inputNonPrioritySum[1] *= beta; + } + + if (logStream.has_value()) { + *logStream << std::format( + "\tInput cycle queue ratios are {:.2f} {:.2f} - {:.2f} {:.2f}\n", + inputPrioritySum[0], + inputPrioritySum[1], + inputNonPrioritySum[0], + inputNonPrioritySum[1]); + } + + tl.resetCycles(); + auto cycles{tl.cycles()}; + std::array n{0, 0, 0, 0}; + std::array greenTimes{0., 0., 0., 0.}; + + for (auto const& [streetId, pair] : cycles) { + for (auto const& [direction, cycle] : pair) { + if (tl.streetPriorities().contains(streetId)) { + if (isPrioritySinglePhase) { + greenTimes[0] += cycle.greenTime(); + ++n[0]; + } else { + if (direction == Direction::LEFT || + direction == Direction::LEFTANDSTRAIGHT) { + greenTimes[1] += cycle.greenTime(); + ++n[1]; + } else { + greenTimes[0] += cycle.greenTime(); + ++n[0]; + } + } + } else { + if (isNonPrioritySinglePhase) { + greenTimes[2] += cycle.greenTime(); + ++n[2]; + } else { + if (direction == Direction::LEFT || + direction == Direction::LEFTANDSTRAIGHT) { + greenTimes[3] += cycle.greenTime(); + ++n[3]; + } else { + greenTimes[2] += cycle.greenTime(); + ++n[2]; + } + } + } + } + } + + if (logStream.has_value()) { + *logStream << std::format("\tGreen times are {} {} - {} {}\n", + greenTimes[0], + greenTimes[1], + greenTimes[2], + greenTimes[3]); + } + + for (auto i{0}; i < 4; ++i) { + if (n[i] > 1) { + greenTimes[i] /= n[i]; + } + } + + { + auto sum{0.}; + for (auto const& greenTime : greenTimes) { + sum += greenTime; + } + if (sum == 0.) { + continue; + } + for (auto& greenTime : greenTimes) { + greenTime /= sum; + } + } + for (auto& el : greenTimes) { + el *= (1. - beta); + } + + int inputPriorityR{static_cast( + std::floor((inputPrioritySum[0] + greenTimes[0]) * tl.cycleTime()))}; + int inputPriorityS{inputPriorityR}; + int inputPriorityL{static_cast( + std::floor((inputPrioritySum[1] + greenTimes[1]) * tl.cycleTime()))}; + + int inputNonPriorityR{static_cast( + std::floor((inputNonPrioritySum[0] + greenTimes[2]) * tl.cycleTime()))}; + int inputNonPriorityS{inputNonPriorityR}; + int inputNonPriorityL{static_cast( + std::floor((inputNonPrioritySum[1] + greenTimes[3]) * tl.cycleTime()))}; + + { + // Adjust phases to have the sum equal to the cycle time + // To do this, first add seconds to the priority streets, then to the + // non-priority streets + auto total{static_cast(inputPriorityR + inputPriorityL + + inputNonPriorityR + inputNonPriorityL)}; + size_t idx{0}; + while (total < tl.cycleTime()) { + switch (idx % 4) { + case 0: + ++inputPriorityR; + ++inputPriorityS; + break; + case 1: + ++inputPriorityL; + break; + case 2: + ++inputNonPriorityR; + ++inputNonPriorityS; + break; + case 3: + ++inputNonPriorityL; + break; + } + ++idx; + ++total; + } + } + + if (isPrioritySinglePhase) { + inputPriorityR = 0; + inputPriorityL = 0; + } + if (isNonPrioritySinglePhase) { + inputNonPriorityR = 0; + inputNonPriorityL = 0; + } + + // Logger::info(std::format( + // "Cycle time: {} - Current sum: {}", + // tl.cycleTime(), + // inputPriorityRS + inputPriorityL + inputNonPriorityRS + inputNonPriorityL)); + assert(inputPriorityS + inputPriorityL + inputNonPriorityS + inputNonPriorityL == + tl.cycleTime()); + + std::unordered_map priorityCycles; + priorityCycles.emplace(Direction::RIGHT, + TrafficLightCycle{static_cast(inputPriorityR), 0}); + priorityCycles.emplace(Direction::STRAIGHT, + TrafficLightCycle{static_cast(inputPriorityS), 0}); + priorityCycles.emplace(Direction::RIGHTANDSTRAIGHT, + TrafficLightCycle{static_cast(inputPriorityS), 0}); + priorityCycles.emplace( + Direction::ANY, + TrafficLightCycle{static_cast(inputPriorityS + inputPriorityL), 0}); + priorityCycles.emplace(Direction::LEFT, + TrafficLightCycle{static_cast(inputPriorityL), + static_cast(inputPriorityS)}); + + std::unordered_map nonPriorityCycles; + nonPriorityCycles.emplace( + Direction::RIGHT, + TrafficLightCycle{static_cast(inputNonPriorityR), + static_cast(inputPriorityS + inputPriorityL)}); + nonPriorityCycles.emplace( + Direction::STRAIGHT, + TrafficLightCycle{static_cast(inputNonPriorityS), + static_cast(inputPriorityS + inputPriorityL)}); + nonPriorityCycles.emplace( + Direction::RIGHTANDSTRAIGHT, + TrafficLightCycle{static_cast(inputNonPriorityS), + static_cast(inputPriorityS + inputPriorityL)}); + nonPriorityCycles.emplace( + Direction::ANY, + TrafficLightCycle{static_cast(inputNonPriorityS + inputNonPriorityL), + static_cast(inputPriorityS + inputPriorityL)}); + nonPriorityCycles.emplace( + Direction::LEFT, + TrafficLightCycle{ + static_cast(inputNonPriorityL), + static_cast(inputPriorityS + inputPriorityL + inputNonPriorityS)}); + nonPriorityCycles.emplace( + Direction::LEFTANDSTRAIGHT, + TrafficLightCycle{ + static_cast(inputNonPriorityL + inputNonPriorityS), + static_cast(inputPriorityS + inputPriorityL + inputNonPriorityR)}); + + std::vector streetIds; + std::set forbiddenLeft; + + for (auto const& pair : cycles) { + streetIds.push_back(pair.first); + } + for (auto const streetId : streetIds) { + auto const& pStreet{this->graph().edge(streetId)}; + if (tl.streetPriorities().contains(streetId)) { + for (auto& [dir, cycle] : cycles.at(streetId)) { + if (isPrioritySinglePhase) { + cycle = priorityCycles.at(Direction::STRAIGHT); + } else { + cycle = priorityCycles.at(dir); + } + } + if (cycles.at(streetId).contains(Direction::RIGHT) && + cycles.at(streetId).contains(Direction::STRAIGHT)) { + TrafficLightCycle freecycle{ + static_cast(inputPriorityS + inputPriorityL), 0}; + // Logger::info(std::format("Free cycle (RIGHT) for {} -> {}: {} {}", + // pStreet->source(), + // pStreet->target(), + // freecycle.greenTime(), + // freecycle.phase())); + cycles.at(streetId).at(Direction::RIGHT) = freecycle; + } + } else { + for (auto& [dir, cycle] : cycles.at(streetId)) { + if (isNonPrioritySinglePhase) { + cycle = nonPriorityCycles.at(Direction::STRAIGHT); + } else { + cycle = nonPriorityCycles.at(dir); + } + } + if (cycles.at(streetId).contains(Direction::RIGHT) && + cycles.at(streetId).contains(Direction::STRAIGHT)) { + TrafficLightCycle freecycle{ + static_cast(inputNonPriorityS + inputNonPriorityL), + static_cast(inputPriorityS + inputPriorityL)}; + // Logger::info(std::format("Free cycle (RIGHT) for {} -> {}: {} {}", + // pStreet->source(), + // pStreet->target(), + // freecycle.greenTime(), + // freecycle.phase())); + cycles.at(streetId).at(Direction::RIGHT) = freecycle; + } + } + bool found{false}; + for (auto const dir : pStreet->laneMapping()) { + if (dir == Direction::LEFT || dir == Direction::LEFTANDSTRAIGHT || + dir == Direction::ANY) { + found = true; + break; + } + } + if (!found) { + forbiddenLeft.insert(streetId); + // Logger::info(std::format("Street {} -> {} has forbidden left turn.", + // pStreet->source(), + // pStreet->target())); + } + } + for (auto const forbiddenLeftStreetId : forbiddenLeft) { + for (auto const streetId : streetIds) { + if (streetId == forbiddenLeftStreetId) { + continue; + } + if (tl.streetPriorities().contains(streetId) && + tl.streetPriorities().contains(forbiddenLeftStreetId)) { + TrafficLightCycle freecycle{ + static_cast(inputPriorityS + inputPriorityL), 0}; + for (auto& [direction, cycle] : cycles.at(streetId)) { + if (direction == Direction::RIGHT || direction == Direction::STRAIGHT || + direction == Direction::RIGHTANDSTRAIGHT) { + auto const& pStreet{this->graph().edge(streetId)}; + if (logStream.has_value()) { + *logStream << std::format("\tFree cycle for {} -> {}: dir {} - {}\n", + pStreet->source(), + pStreet->target(), + directionToString[direction], + freecycle); + } + cycle = freecycle; + } + } + } else if (!tl.streetPriorities().contains(streetId) && + !tl.streetPriorities().contains(forbiddenLeftStreetId)) { + TrafficLightCycle freecycle{ + static_cast(inputNonPriorityS + inputNonPriorityL), + static_cast(inputPriorityS + inputPriorityL)}; + for (auto& [direction, cycle] : cycles.at(streetId)) { + if (direction == Direction::RIGHT || direction == Direction::STRAIGHT || + direction == Direction::RIGHTANDSTRAIGHT) { + auto const& pStreet{this->graph().edge(streetId)}; + if (logStream.has_value()) { + *logStream << std::format("Free cycle ({}) for {} -> {}: {} {}\n", + directionToString[direction], + pStreet->source(), + pStreet->target(), + freecycle.greenTime(), + freecycle.phase()); + } + cycle = freecycle; + } + } + } + } + } + + tl.setCycles(cycles); + if (logStream.has_value()) { + *logStream << std::format("\nNew cycles for {}", tl); + } + } + if (logStream.has_value()) { + *logStream << std::format("End Traffic Lights optimization - Time {}\n", + this->time_step()); + } + } + + void FirstOrderDynamics::optimizeTrafficLights( + TrafficLightOptimization const optimizationType, + const std::string& logFile, + double const percentage, + double const threshold) { + std::optional logStream; + if (!logFile.empty()) { + logStream.emplace(logFile, std::ios::app); + if (!logStream->is_open()) { + spdlog::error("Could not open log file: {}", logFile); + } + } + this->m_trafficlightSingleTailOptimizer(percentage, logStream); + if (optimizationType == TrafficLightOptimization::DOUBLE_TAIL) { + // Try to synchronize congested traffic lights + std::unordered_map densities; + for (auto const& [nodeId, pNode] : this->graph().nodes()) { + if (!pNode->isTrafficLight()) { + continue; + } + double density{0.}, n{0.}; + auto const& inNeighbours{pNode->ingoingEdges()}; + for (auto const& inEdgeId : inNeighbours) { + auto const& pStreet{this->graph().edge(inEdgeId)}; + auto const& pSourceNode{this->graph().node(pStreet->source())}; + if (!pSourceNode->isTrafficLight()) { + continue; + } + density += pStreet->density(true); + ++n; + } + density /= n; + densities[nodeId] = density; + } + // Sort densities map from big to small values + std::vector> sortedDensities(densities.begin(), + densities.end()); + + // Sort by density descending + std::sort(sortedDensities.begin(), + sortedDensities.end(), + [](auto const& a, auto const& b) { return a.second > b.second; }); + std::unordered_set optimizedNodes; + + for (auto const& [nodeId, density] : sortedDensities) { + auto const& inNeighbours{this->graph().node(nodeId)->ingoingEdges()}; + for (auto const& inEdgeId : inNeighbours) { + auto const& pStreet{this->graph().edge(inEdgeId)}; + auto const& sourceId{pStreet->source()}; + if (!densities.contains(sourceId) || optimizedNodes.contains(sourceId)) { + continue; + } + auto const& neighbourDensity{densities.at(sourceId)}; + if (neighbourDensity < threshold * density) { + continue; + } + // Try to green-wave the situation + auto& tl{dynamic_cast(*this->graph().node(sourceId))}; + tl.increasePhases(pStreet->length() / + (pStreet->maxSpeed() * (1. - 0.6 * pStreet->density(true)))); + optimizedNodes.insert(sourceId); + if (logStream.has_value()) { + *logStream << std::format("\nNew cycles for {}", tl); + } + } + } + } + // Cleaning variables + for (auto& [streetId, pair] : m_queuesAtTrafficLights) { + for (auto& [direction, value] : pair) { + value = 0.; + } + } + m_previousOptimizationTime = this->time_step(); + if (logStream.has_value()) { + logStream->close(); + } + } + + Measurement FirstOrderDynamics::meanTravelTime(bool clearData) { + std::vector travelTimes; + if (!m_travelDTs.empty()) { + travelTimes.reserve(m_travelDTs.size()); + for (auto const& [distance, time] : m_travelDTs) { + travelTimes.push_back(time); + } + if (clearData) { + m_travelDTs.clear(); + } + } + return Measurement(travelTimes); + } + Measurement FirstOrderDynamics::meanTravelDistance(bool clearData) { + if (m_travelDTs.empty()) { + return Measurement(); + } + std::vector travelDistances; + travelDistances.reserve(m_travelDTs.size()); + for (auto const& [distance, time] : m_travelDTs) { + travelDistances.push_back(distance); + } + if (clearData) { + m_travelDTs.clear(); + } + return Measurement(travelDistances); + } + Measurement FirstOrderDynamics::meanTravelSpeed(bool clearData) { + std::vector travelSpeeds; + travelSpeeds.reserve(m_travelDTs.size()); + for (auto const& [distance, time] : m_travelDTs) { + travelSpeeds.push_back(distance / time); + } + if (clearData) { + m_travelDTs.clear(); + } + return Measurement(travelSpeeds); + } + std::unordered_map> const + FirstOrderDynamics::normalizedTurnCounts() const noexcept { + std::unordered_map> normalizedTurnCounts; + for (auto const& [fromId, map] : m_turnCounts) { + auto const sum{ + std::accumulate(map.begin(), map.end(), 0., [](auto const sum, auto const& p) { + return sum + static_cast(p.second); + })}; + for (auto const& [toId, count] : map) { + normalizedTurnCounts[fromId][toId] = + sum == 0. ? 0. : static_cast(count) / sum; + } + } + return normalizedTurnCounts; + } + + tbb::concurrent_unordered_map FirstOrderDynamics::originCounts( + bool const bReset) noexcept { + if (!bReset) { + return m_originCounts; + } + auto const tempCounts{std::move(m_originCounts)}; + m_originCounts.clear(); + return tempCounts; + } + tbb::concurrent_unordered_map FirstOrderDynamics::destinationCounts( + bool const bReset) noexcept { + if (!bReset) { + return m_destinationCounts; + } + auto const tempCounts{std::move(m_destinationCounts)}; + m_destinationCounts.clear(); + return tempCounts; + } + + Measurement FirstOrderDynamics::streetMeanDensity(bool normalized) const { + if (this->graph().edges().empty()) { + return Measurement(); + } + std::vector densities; + densities.reserve(this->graph().nEdges()); + if (normalized) { + for (const auto& [streetId, pStreet] : this->graph().edges()) { + densities.push_back(pStreet->density(true)); + } + } else { + double sum{0.}; + for (const auto& [streetId, pStreet] : this->graph().edges()) { + densities.push_back(pStreet->density(false) * pStreet->length()); + sum += pStreet->length(); + } + if (sum == 0) { + return Measurement(); + } + auto meanDensity{std::accumulate(densities.begin(), densities.end(), 0.) / sum}; + return Measurement(meanDensity, 0., densities.size()); + } + return Measurement(densities); + } + + Measurement FirstOrderDynamics::streetMeanFlow() const { + std::vector flows; + flows.reserve(this->graph().nEdges()); + for (const auto& [streetId, pStreet] : this->graph().edges()) { + auto const speedMeasure = pStreet->meanSpeed(); + if (speedMeasure.is_valid) { + flows.push_back(pStreet->density() * speedMeasure.mean); + } + } + return Measurement(flows); + } + + Measurement FirstOrderDynamics::streetMeanFlow(double threshold, + bool above) const { + std::vector flows; + flows.reserve(this->graph().nEdges()); + for (const auto& [streetId, pStreet] : this->graph().edges()) { + auto const speedMeasure = pStreet->meanSpeed(); + if (!speedMeasure.is_valid) { + continue; + } + if (above && (pStreet->density(true) > threshold)) { + flows.push_back(pStreet->density() * speedMeasure.mean); + } else if (!above && (pStreet->density(true) < threshold)) { + flows.push_back(pStreet->density() * speedMeasure.mean); + } + } + return Measurement(flows); + } + + void FirstOrderDynamics::summary(std::ostream& os) const { + os << "FirstOrderDynamics Summary:\n"; + this->graph().describe(os); + os << "\nNumber of added agents: " << m_nAddedAgents << '\n' + << "Number of inserted agents: " << m_nInsertedAgents << '\n' + << "Number of arrived agents: " << m_nArrivedAgents << '\n' + << "Number of killed agents: " << m_nKilledAgents << '\n' + << "Current number of agents: " << this->nAgents() << '\n'; + } + +} // namespace dsf::mobility diff --git a/src/dsf/mobility/FirstOrderDynamics.hpp b/src/dsf/mobility/FirstOrderDynamics.hpp index c24c10bc..30b2d5cf 100644 --- a/src/dsf/mobility/FirstOrderDynamics.hpp +++ b/src/dsf/mobility/FirstOrderDynamics.hpp @@ -1,39 +1,507 @@ +/// @file /src/dsf/headers/FirstOrderDynamics.hpp +/// @brief Defines the FirstOrderDynamics class. +/// +/// @details This file contains the definition of the FirstOrderDynamics class. +/// The FirstOrderDynamics class represents the dynamics of the network. It is templated by the type +/// of the graph's id and the type of the graph's capacity. +/// The graph's id and capacity must be unsigned integral types. + #pragma once -#include "RoadDynamics.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "../base/Dynamics.hpp" +#include "Agent.hpp" +#include "Itinerary.hpp" +#include "RoadNetwork.hpp" +#include "../utility/Typedef.hpp" + +static constexpr auto CACHE_FOLDER = "./.dsfcache/"; +static constexpr auto U_TURN_PENALTY_FACTOR = 0.1; namespace dsf::mobility { - class FirstOrderDynamics : public RoadDynamics { - double m_alpha; - double m_speedFluctuationSTD; + /// @brief The method for inserting agents into the network + enum class AgentInsertionMethod : uint8_t { + RANDOM = 0, // Agents spawn randomly in the network and travel with no destination + ODS = 1, // Agents are generated according to the provided OD pairs + RANDOM_ODS = + 2, // Agents are generated by randomly selecting their origin and destination according to the weights + }; + /// @brief The FirstOrderDynamics class represents the dynamics of the network. + class FirstOrderDynamics : public Dynamics { + std::vector m_nodeIndices; + std::vector> m_agents; + std::unordered_map> m_itineraries; + std::vector> m_originNodes; + std::vector> m_destinationNodes; + std::vector> m_ODs; + tbb::concurrent_unordered_map m_originCounts; + tbb::concurrent_unordered_map m_destinationCounts; + std::atomic m_nAgents{0}, m_nAddedAgents{0}, m_nInsertedAgents{0}, + m_nKilledAgents{0}, m_nArrivedAgents{0}; + std::function const&)> m_speedFunction; + + protected: + std::unordered_map> m_turnCounts; + std::unordered_map> m_turnMapping; + tbb::concurrent_unordered_map> + m_queuesAtTrafficLights; + tbb::concurrent_vector> m_travelDTs; + std::time_t m_previousOptimizationTime{0}; + + protected: + std::function const&)> m_weightFunction; + std::optional m_errorProbability{std::nullopt}; + std::optional m_passageProbability{std::nullopt}; + std::optional m_meanTravelDistance{std::nullopt}; + std::optional m_meanTravelTime{std::nullopt}; + std::optional m_dataUpdatePeriod; + bool m_bCacheEnabled; + PathWeight m_pathWeight = PathWeight::TRAVELTIME; + double m_weightTreshold; + std::optional m_timeToleranceFactor{std::nullopt}; + bool m_forcePriorities{false}; + // Saving variables + std::optional m_savingInterval{std::nullopt}; + bool m_bSaveStreetData{false}; + bool m_bSaveTravelData{false}; + bool m_bSaveAverageStats{false}; + + private: + /// @brief Kill an agent + /// @param pAgent A std::unique_ptr to the agent to kill + std::unique_ptr m_killAgent(std::unique_ptr pAgent); + /// @brief Update the path of a single itinerary using Dijsktra's algorithm + /// @param pItinerary An std::shared_ptr to the itinerary + void m_updatePath(std::shared_ptr const& pItinerary); - double m_speedFactor(double const& density) const final; + void m_addAgentsRandom(std::size_t nAgents); + void m_addAgentsODs(std::size_t nAgents); + void m_addAgentsRandomODs(std::size_t nAgents); - double m_streetEstimatedTravelTime(std::unique_ptr const& pStreet) const final; + /// @brief Get the next street id + /// @param pAgent A std::unique_ptr to the agent + /// @param pNode A std::unique_ptr to the current node + /// @return Id The id of the randomly selected next street + std::optional m_nextStreetId(const std::unique_ptr& pAgent, + const std::unique_ptr& pNode); + /// @brief Evolve a street + /// @param pStreet A std::unique_ptr to the street + /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination + /// @details If possible, removes the first agent of the street's queue, putting it in the destination node. + /// If the agent is going into the destination node, it is removed from the simulation (and then reinserted if reinsert_agents is true) + void m_evolveStreet(std::unique_ptr const& pStreet, bool reinsert_agents); + /// @brief If possible, removes one agent from the node, putting it on the next street. + /// @param pNode A std::unique_ptr to the node + void m_evolveNode(const std::unique_ptr& pNode); + /// @brief Evolve the agents. + /// @details Puts all new agents on a street, if possible, decrements all delays + /// and increments all travel times. + void m_evolveAgents(); - void m_dumpSimInfo() const final; + void m_trafficlightSingleTailOptimizer(double const& beta, + std::optional& logStream); + + inline double m_streetEstimatedTravelTime( + std::unique_ptr const& pStreet) const { + return pStreet->length() / m_speedFunction(pStreet); + }; + + /// @brief Initialize the street data table. + /// This table contains the data of each street. Columns are: + /// - id: The entry id (auto-incremented) + /// - simulation_id: The simulation id + /// - datetime: The datetime of the data entry + /// - time_step: The time step of the data entry + /// - street_id: The id of the street + /// - coil: The name of the coil on the street (can be null) + /// - density_vpk: The density in vehicles per kilometer + /// - avg_speed_kph: The average speed in kilometers per hour + /// - std_speed_kph: The standard deviation of the speed in kilometers per hour + /// - n_observations: The number of speed observations used to compute avg/std (0 if none) + /// - counts: The counts of the coil sensor (can be null) + /// - queue_length: The length of the queue on the street + void m_initStreetTable() const; + /// @brief Initialize the average stats table. + /// This table contains the average stats of the simulation at each time step. Columns are: + /// - id: The entry id (auto-incremented) + /// - simulation_id: The simulation id + /// - datetime: The datetime of the data entry + /// - time_step: The time step of the data entry + /// - n_ghost_agents: The number of ghost agents + /// - n_agents: The number of agents + /// - mean_speed_kph: The mean speed of the agents in kilometers per hour + /// - std_speed_kph: The standard deviation of the speed of the agents in kilometers per hour + /// - mean_density_vpk: The mean density of the streets in vehicles per kilometer + /// - std_density_vpk: The standard deviation of the density of the streets in vehicles per kilometer + void m_initAvgStatsTable() const; + /// @brief Initialize the travel data table. + /// This table contains the travel data of the agents. Columns are: + /// - id: The entry id (auto-incremented) + /// - simulation_id: The simulation id + /// - datetime: The datetime of the data entry + /// - time_step: The time step of the data entry + /// - distance_m: The distance travelled by the agent in meters + /// - travel_time_s: The travel time of the agent in seconds + void m_initTravelDataTable() const; + + void m_dumpSimInfo() const; + + void m_dumpNetwork() const; public: - /// @brief Construct a new First Order Dynamics object + /// @brief Construct a new FirstOrderDynamics object /// @param graph The graph representing the network /// @param useCache If true, the cache is used (default is false) /// @param seed The seed for the random number generator (default is std::nullopt) - /// @param alpha The minimum speed rate (default is 0) - /// @param weightFunction The dsf::PathWeight function to use for the pathfinding (default is dsf::PathWeight::TRAVELTIME) - /// @param weightTreshold The weight threshold for the pathfinding (default is std::nullopt) FirstOrderDynamics(RoadNetwork& graph, bool useCache = false, - std::optional seed = std::nullopt, - double alpha = 0., - PathWeight const weightFunction = PathWeight::TRAVELTIME, - std::optional weightTreshold = std::nullopt); - /// @brief Set the speed of an agent - /// @param agentId The id of the agent - /// @throw std::invalid_argument, If the agent is not found - void setAgentSpeed(std::unique_ptr const& pAgent) override; - /// @brief Set the standard deviation of the speed fluctuation - /// @param speedFluctuationSTD The standard deviation of the speed fluctuation - /// @throw std::invalid_argument, If the standard deviation is negative - void setSpeedFluctuationSTD(double speedFluctuationSTD); + std::optional seed = std::nullopt); + + /// @brief Set the error probability + /// @param errorProbability The error probability + /// @throw std::invalid_argument If the error probability is not between 0 and 1 + void setErrorProbability(double errorProbability); + /// @brief Set the passage probability + /// @param passageProbability The passage probability + /// @details The passage probability is the probability of passing through a node + /// It is useful in the case of random agents + void setPassageProbability(double passageProbability); + /// @brief Set the time tolerance factor for killing stagnant agents. + /// An agent will be considered stagnant if it has not moved for timeToleranceFactor * std::ceil(street_length / street_maxSpeed) time units. + /// @param timeToleranceFactor The time tolerance factor + /// @throw std::invalid_argument If the time tolerance factor is not positive + void killStagnantAgents(double timeToleranceFactor = 3.); + /// @brief Set the weight function + /// @param pathWeight The dsf::PathWeight function to use for the pathfinding + /// @param weightThreshold The weight threshold for updating the paths (default is std::nullopt) + void setWeightFunction(PathWeight const pathWeight, + std::optional weightThreshold = std::nullopt); + template + void setSpeedFunction(SpeedFunction const speedFunction, TArgs&&... args); + /// @brief Set the force priorities flag + /// @param forcePriorities The flag + /// @details If true, if an agent cannot move to the next street, the whole node is skipped + inline void setForcePriorities(bool forcePriorities) noexcept { + m_forcePriorities = forcePriorities; + } + /// @brief Set the data update period. + /// @param dataUpdatePeriod Delay, The period + /// @details Some data, i.e. the street queue lengths, are stored only after a fixed amount of time which is represented by this variable. + inline void setDataUpdatePeriod(Delay const dataUpdatePeriod) noexcept { + m_dataUpdatePeriod = dataUpdatePeriod; + } + /// @brief Set the mean distance travelled by a random agent. The distance will be sampled from an exponential distribution with this mean. + /// @param meanTravelDistance The mean distance + /// @throw std::invalid_argument If the mean distance is negative + inline void setMeanTravelDistance(double const meanTravelDistance) { + meanTravelDistance > 0. ? m_meanTravelDistance = meanTravelDistance + : throw std::invalid_argument( + "FirstOrderDynamics::setMeanTravelDistance: " + "meanTravelDistance must be positive"); + }; + /// @brief Set the mean travel time for random agents. The travel time will be sampled from an exponential distribution with this mean. + /// @param meanTravelTime The mean travel time + inline void setMeanTravelTime(std::time_t const meanTravelTime) noexcept { + m_meanTravelTime = meanTravelTime; + }; + /// @brief Set the origin nodes. If the provided map is empty, the origin nodes are set using the streets' stationary weights. + /// NOTE: the default stationary weights are 1.0 so, if not set, this is equivalent to setting uniform weights. + /// @param originNodes The origin nodes + void setOriginNodes(std::unordered_map const& originNodes = {}); + /// @brief Set the destination nodes + /// @param destinationNodes The destination nodes + void setDestinationNodes(std::unordered_map const& destinationNodes); + /// @brief Set the destination nodes + /// @param destinationNodes The destination nodes (as an initializer list) + void setDestinationNodes(std::initializer_list destinationNodes); + /// @brief Set the origin-destination pairs with their associated weights + /// @param ODs A vector of tuples (origin node id, destination node id, weight) + void setODs(std::vector> const& ODs); + /// @brief Set the destination nodes + /// @param destinationNodes A container of destination nodes ids + /// @details The container must have a value_type convertible to Id and begin() and end() methods + template + requires(std::is_convertible_v) + void setDestinationNodes(TContainer const& destinationNodes); + + /// @brief Initialize the turn counts map + /// @throws std::runtime_error if the turn counts map is already initialized + void initTurnCounts(); + /// @brief Reset the turn counts map values to zero + /// @throws std::runtime_error if the turn counts map is not initialized + void resetTurnCounts(); + /// @brief Enable data saving to the database + /// @param savingInterval The interval at which save the data (in time steps). If zero, saves data at the next time step and then disables saving (working like a manual trigger). + /// @param saveAverageStats If true, saves the average stats of the simulation (default is false) + /// @param saveStreetData If true, saves the street data (default is false) + /// @param saveTravelData If true, saves the travel data of the agents (default is false) + void saveData(std::time_t const savingInterval, + bool const saveAverageStats = false, + bool const saveStreetData = false, + bool const saveTravelData = false); + + /// @brief Update the paths of the itineraries based on the given weight function + /// @param throw_on_empty If true, throws an exception if an itinerary has an empty path (default is true) + /// If false, removes the itinerary with empty paths and the associated node from the origin/destination nodes + /// @throws std::runtime_error if throw_on_empty is true and an itinerary has an empty path + void updatePaths(bool const throw_on_empty = true); + /// @brief Add agents uniformly on the road network + /// @param nAgents The number of agents to add + /// @param itineraryId The id of the itinerary to use (default is std::nullopt) + /// @throw std::runtime_error If there are no itineraries + void addAgentsUniformly(std::size_t nAgents, + std::optional itineraryId = std::nullopt); + + /// @brief Add an agent to the simulation + /// @param agent std::unique_ptr to the agent + void addAgent(std::unique_ptr agent); + + template + requires(std::is_constructible_v) + void addAgent(TArgs&&... args); + + template + requires(std::is_constructible_v) + void addAgents(std::size_t const nAgents, TArgs&&... args); + /// @brief Add agents to the simulation + /// @param nAgents The number of agents to add + /// @param mode The method to use for adding the agents.Possible values are: + /// - ODS: Agents are generated according to the provided OD pairs + /// - RANDOM: Agents spwan randomly in the network and travel with no destination + /// - RANDOM_ODS: Agents are generated by randomly selecting their origin and destination according to the weights + void addAgents(std::size_t const nAgents, AgentInsertionMethod const mode); + + /// @brief Add an itinerary + /// @param ...args The arguments to construct the itinerary + /// @details The arguments must be compatible with any constructor of the Itinerary class + template + requires(std::is_constructible_v) + void addItinerary(TArgs&&... args); + /// @brief Add an itinerary + /// @param itinerary std::unique_ptr to the itinerary + /// @throws std::invalid_argument If the itinerary already exists + /// @throws std::invalid_argument If the itinerary's destination is not a node of the graph + void addItinerary(std::shared_ptr itinerary); + + /// @brief Evolve the simulation + /// @details Evolve the simulation by moving the agents and updating the travel times. + /// In particular: + /// - Move the first agent of each street queue, if possible, putting it in the next node + /// - Move the agents from each node, if possible, putting them in the next street and giving them a speed. + /// If the error probability is not zero, the agents can move to a random street. + /// If the agent is in the destination node, it is removed from the simulation (and then reinserted if reinsert_agents is true) + /// - Cycle over agents and update their times + /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination + void evolve(bool const reinsert_agents = false); + /// @brief Optimize the traffic lights by changing the green and red times + /// @param optimizationType TrafficLightOptimization, The type of optimization. Default is DOUBLE_TAIL + /// @param logFile The file into which write the logs (default is empty, meaning no logging) + /// @param percentage double, the maximum amount (percentage) of the green time to change (default is 0.3) + /// @param threshold double, The ratio between the self-density and neighbour density to trigger the non-local optimization (default is 1.3) + /// @details The local optimization is done by changing the green time of each traffic light, trying to make it proportional to the + /// queue lengths at each phase. The non-local optimization is done by synchronizing the traffic lights which are congested over threshold. + void optimizeTrafficLights( + TrafficLightOptimization optimizationType = TrafficLightOptimization::DOUBLE_TAIL, + const std::string& logFile = std::string(), + double const percentage = 0.3, + double const threshold = 1.3); + + /// @brief Get the itineraries + /// @return const std::unordered_map&, The itineraries + inline auto const& itineraries() const noexcept { return m_itineraries; } + /// @brief Get the origin nodes of the graph + /// @return std::vector> const& The origin nodes of the graph + inline std::vector> const& originNodes() const noexcept { + return m_originNodes; + } + /// @brief Get the origin nodes of the graph + /// @return std::vector>& The origin nodes of the graph + inline std::vector>& originNodes() noexcept { + return m_originNodes; + } + /// @brief Get the destination nodes of the graph + /// @return std::vector> const& The destination nodes of the graph + inline std::vector> const& destinationNodes() const noexcept { + return m_destinationNodes; + } + /// @brief Get the destination nodes of the graph + /// @return std::vector>>& The destination nodes of the graph + inline std::vector>& destinationNodes() noexcept { + return m_destinationNodes; + } + /// @brief Get the agents + /// @return const std::unordered_map>&, The agents + inline const std::vector>& agents() const noexcept { + return m_agents; + } + /// @brief Get the number of agents currently in the simulation + /// @return std::size_t The number of agents + inline auto nAgents() const { return m_nAgents.load(); }; + + /// @brief Get the mean travel time of the agents in \f$s\f$ + /// @param clearData If true, the travel times are cleared after the computation + /// @return Measurement The mean travel time of the agents and the standard deviation + Measurement meanTravelTime(bool clearData = false); + /// @brief Get the mean travel distance of the agents in \f$m\f$ + /// @param clearData If true, the travel distances are cleared after the computation + /// @return Measurement The mean travel distance of the agents and the standard deviation + Measurement meanTravelDistance(bool clearData = false); + /// @brief Get the mean travel speed of the agents in \f$m/s\f$ + /// @param clearData If true, the travel times and distances are cleared after the computation + /// @return Measurement The mean travel speed of the agents and the standard deviation + Measurement meanTravelSpeed(bool clearData = false); + /// @brief Get the turn counts of the agents + /// @return const std::unordered_map>& The turn counts. The outer map's key is the street id, the inner map's key is the next street id and the value is the number of counts + inline std::unordered_map> const& turnCounts() + const noexcept { + return m_turnCounts; + }; + /// @brief Get the normalized turn counts of the agents + /// @return const std::unordered_map>& The normalized turn counts. The outer map's key is the street id, the inner map's key is the next street id and the value is the normalized number of counts + std::unordered_map> const normalizedTurnCounts() + const noexcept; + + std::unordered_map> turnMapping() const { + return m_turnMapping; + } + + /// @brief Get the origin counts of the agents + /// @param bReset If true, the origin counts are cleared (default is true) + tbb::concurrent_unordered_map originCounts( + bool const bReset = true) noexcept; + /// @brief Get the destination counts of the agents + /// @param bReset If true, the destination counts are cleared (default is true) + tbb::concurrent_unordered_map destinationCounts( + bool const bReset = true) noexcept; + + /// @brief Get the mean density of the streets in \f$m^{-1}\f$ + /// @return Measurement The mean density of the streets and the standard deviation + Measurement streetMeanDensity(bool normalized = false) const; + /// @brief Get the mean flow of the streets in \f$s^{-1}\f$ + /// @return Measurement The mean flow of the streets and the standard deviation + Measurement streetMeanFlow() const; + /// @brief Get the mean flow of the streets in \f$s^{-1}\f$ + /// @param threshold The density threshold to consider + /// @param above If true, the function returns the mean flow of the streets with a density above the threshold, otherwise below + /// @return Measurement The mean flow of the streets and the standard deviation + Measurement streetMeanFlow(double threshold, bool above) const; + + /// @brief Print a summary of the dynamics to an output stream + /// @param os The output stream to write to (default is std::cout) + /// @details The summary includes: + /// - The RoadNetwork description (nodes, edges, capacity, intersections, traffic lights, roundabouts, coil sensors) + /// - Number of inserted agents + /// - Number of added agents + /// - Number of arrived agents + /// - Number of killed agents + /// - Current number of agents in the simulation + void summary(std::ostream& os = std::cout) const; }; -} // namespace dsf::mobility \ No newline at end of file + + template + void FirstOrderDynamics::setSpeedFunction(SpeedFunction const speedFunction, + TArgs&&... args) { + switch (speedFunction) { + case SpeedFunction::CUSTOM: + // Assert that the only argument is a function with the correct signature + if constexpr (sizeof...(args) != 1) { + throw std::invalid_argument(std::format( + "Custom speed function requires exactly one argument, but {} were provided", + sizeof...(args))); + } else if constexpr (!std::is_invocable_r_v< + double, + std::tuple_element_t<0, std::tuple>, + std::unique_ptr const&>) { + throw std::invalid_argument( + "Custom speed function requires a callable argument with signature " + "double(std::unique_ptr const&)"); + } else { + m_speedFunction = std::get<0>(std::forward_as_tuple(args...)); + } + break; + case SpeedFunction::LINEAR: + // There should be only one argument, named "alpha", so check it and extract it + if constexpr (sizeof...(args) != 1) { + throw std::invalid_argument(std::format( + "Linear speed function requires exactly one argument, but {} were provided", + sizeof...(args))); + } else if constexpr (!std::is_same_v>, + double>) { + throw std::invalid_argument(std::format( + "Linear speed function requires a double argument, but {} was provided", + typeid(std::tuple_element_t<0, std::tuple>).name())); + } else { + double alpha = std::get<0>(std::forward_as_tuple(args...)); + if (alpha < 0. || alpha > 1.) { + throw std::invalid_argument( + std::format("The alpha parameter ({}) must be in [0., 1]", alpha)); + } + m_speedFunction = [alpha](std::unique_ptr const& pStreet) { + return pStreet->maxSpeed() * (1. - alpha * pStreet->density(true)); + }; + } + break; + } + } + + template + requires(std::is_convertible_v) + void FirstOrderDynamics::setDestinationNodes(TContainer const& destinationNodes) { + m_itineraries.clear(); + auto const numNodes{destinationNodes.size()}; + m_destinationNodes.clear(); + m_destinationNodes.reserve(numNodes); + std::for_each(destinationNodes.begin(), + destinationNodes.end(), + [this, &numNodes](auto const& nodeId) -> void { + this->m_destinationNodes.push_back({nodeId, 1. / numNodes}); + this->addItinerary(nodeId, nodeId); + }); + } + + template + requires(std::is_constructible_v) + void FirstOrderDynamics::addAgent(TArgs&&... args) { + addAgent(std::make_unique( + this->m_nInsertedAgents, this->time_step(), std::forward(args)...)); + } + + template + requires(std::is_constructible_v) + void FirstOrderDynamics::addAgents(std::size_t const nAgents, TArgs&&... args) { + for (size_t i{0}; i < nAgents; ++i) { + addAgent(std::make_unique( + this->m_nInsertedAgents, this->time_step(), std::forward(args)...)); + } + } + + template + requires(std::is_constructible_v) + void FirstOrderDynamics::addItinerary(TArgs&&... args) { + addItinerary(std::make_shared(std::forward(args)...)); + } + +} // namespace dsf::mobility diff --git a/src/dsf/mobility/RoadDynamics.hpp b/src/dsf/mobility/RoadDynamics.hpp deleted file mode 100644 index 30abe784..00000000 --- a/src/dsf/mobility/RoadDynamics.hpp +++ /dev/null @@ -1,2735 +0,0 @@ -/// @file /src/dsf/headers/RoadDynamics.hpp -/// @brief Defines the RoadDynamics class. -/// -/// @details This file contains the definition of the RoadDynamics class. -/// The RoadDynamics class represents the dynamics of the network. It is templated by the type -/// of the graph's id and the type of the graph's capacity. -/// The graph's id and capacity must be unsigned integral types. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include "../base/Dynamics.hpp" -#include "Agent.hpp" -#include "Itinerary.hpp" -#include "RoadNetwork.hpp" -#include "../utility/Typedef.hpp" - -static constexpr auto CACHE_FOLDER = "./.dsfcache/"; -static constexpr auto U_TURN_PENALTY_FACTOR = 0.1; - -namespace dsf::mobility { - /// @brief The method for inserting agents into the network - enum class AgentInsertionMethod : uint8_t { - RANDOM = 0, // Agents spawn randomly in the network and travel with no destination - ODS = 1, // Agents are generated according to the provided OD pairs - RANDOM_ODS = - 2, // Agents are generated by randomly selecting their origin and destination according to the weights - }; - /// @brief The RoadDynamics class represents the dynamics of the network. - /// @tparam delay_t The type of the agent's delay - template - requires(is_numeric_v) - class RoadDynamics : public Dynamics { - std::vector m_nodeIndices; - std::vector> m_agents; - std::unordered_map> m_itineraries; - std::vector> m_originNodes; - std::vector> m_destinationNodes; - std::vector> m_ODs; - tbb::concurrent_unordered_map m_originCounts; - tbb::concurrent_unordered_map m_destinationCounts; - std::atomic m_nAgents{0}, m_nAddedAgents{0}, m_nInsertedAgents{0}, - m_nKilledAgents{0}, m_nArrivedAgents{0}; - - protected: - std::unordered_map> m_turnCounts; - std::unordered_map> m_turnMapping; - tbb::concurrent_unordered_map> - m_queuesAtTrafficLights; - tbb::concurrent_vector> m_travelDTs; - std::time_t m_previousOptimizationTime{0}; - - protected: - std::function const&)> m_weightFunction; - std::optional m_errorProbability{std::nullopt}; - std::optional m_passageProbability{std::nullopt}; - std::optional m_meanTravelDistance{std::nullopt}; - std::optional m_meanTravelTime{std::nullopt}; - std::optional m_dataUpdatePeriod; - bool m_bCacheEnabled; - PathWeight m_pathWeight = PathWeight::TRAVELTIME; - double m_weightTreshold; - std::optional m_timeToleranceFactor{std::nullopt}; - bool m_forcePriorities{false}; - // Saving variables - std::optional m_savingInterval{std::nullopt}; - bool m_bSaveStreetData{false}; - bool m_bSaveTravelData{false}; - bool m_bSaveAverageStats{false}; - - private: - /// @brief Kill an agent - /// @param pAgent A std::unique_ptr to the agent to kill - std::unique_ptr m_killAgent(std::unique_ptr pAgent); - /// @brief Update the path of a single itinerary using Dijsktra's algorithm - /// @param pItinerary An std::shared_ptr to the itinerary - void m_updatePath(std::shared_ptr const& pItinerary); - - void m_addAgentsRandom(std::size_t nAgents); - void m_addAgentsODs(std::size_t nAgents); - void m_addAgentsRandomODs(std::size_t nAgents); - - /// @brief Get the next street id - /// @param pAgent A std::unique_ptr to the agent - /// @param pNode A std::unique_ptr to the current node - /// @return Id The id of the randomly selected next street - std::optional m_nextStreetId(const std::unique_ptr& pAgent, - const std::unique_ptr& pNode); - /// @brief Evolve a street - /// @param pStreet A std::unique_ptr to the street - /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination - /// @details If possible, removes the first agent of the street's queue, putting it in the destination node. - /// If the agent is going into the destination node, it is removed from the simulation (and then reinserted if reinsert_agents is true) - void m_evolveStreet(std::unique_ptr const& pStreet, bool reinsert_agents); - /// @brief If possible, removes one agent from the node, putting it on the next street. - /// @param pNode A std::unique_ptr to the node - void m_evolveNode(const std::unique_ptr& pNode); - /// @brief Evolve the agents. - /// @details Puts all new agents on a street, if possible, decrements all delays - /// and increments all travel times. - void m_evolveAgents(); - - void m_trafficlightSingleTailOptimizer(double const& beta, - std::optional& logStream); - - virtual double m_speedFactor(double const& density) const = 0; - virtual double m_streetEstimatedTravelTime( - std::unique_ptr const& pStreet) const = 0; - - /// @brief Initialize the street data table. - /// This table contains the data of each street. Columns are: - /// - /// - id: The entry id (auto-incremented) - /// - /// - simulation_id: The simulation id - /// - /// - datetime: The datetime of the data entry - /// - /// - time_step: The time step of the data entry - /// - /// - street_id: The id of the street - /// - /// - coil: The name of the coil on the street (can be null) - /// - /// - density_vpk: The density in vehicles per kilometer - /// - /// - avg_speed_kph: The average speed in kilometers per hour - /// - /// - std_speed_kph: The standard deviation of the speed in kilometers per hour - /// - /// - n_observations: The number of speed observations used to compute avg/std (0 if none) - /// - /// - counts: The counts of the coil sensor (can be null) - /// - /// - queue_length: The length of the queue on the street - void m_initStreetTable() const; - /// @brief Initialize the average stats table. - /// This table contains the average stats of the simulation at each time step. Columns are: - /// - /// - id: The entry id (auto-incremented) - /// - /// - simulation_id: The simulation id - /// - /// - datetime: The datetime of the data entry - /// - /// - time_step: The time step of the data entry - /// - /// - n_ghost_agents: The number of ghost agents - /// - /// - n_agents: The number of agents - /// - /// - mean_speed_kph: The mean speed of the agents in kilometers per hour - /// - /// - std_speed_kph: The standard deviation of the speed of the agents in kilometers per hour - /// - /// - mean_density_vpk: The mean density of the streets in vehicles per kilometer - /// - /// - std_density_vpk: The standard deviation of the density of the streets in vehicles per kilometer - void m_initAvgStatsTable() const; - /// @brief Initialize the travel data table. - /// This table contains the travel data of the agents. Columns are: - /// - /// - id: The entry id (auto-incremented) - /// - /// - simulation_id: The simulation id - /// - /// - datetime: The datetime of the data entry - /// - /// - time_step: The time step of the data entry - /// - /// - distance_m: The distance travelled by the agent in meters - /// - /// - travel_time_s: The travel time of the agent in seconds - void m_initTravelDataTable() const; - - virtual void m_dumpSimInfo() const = 0; - - void m_dumpNetwork() const; - - public: - /// @brief Construct a new RoadDynamics object - /// @param graph The graph representing the network - /// @param useCache If true, the cache is used (default is false) - /// @param seed The seed for the random number generator (default is std::nullopt) - /// @param weightFunction The dsf::PathWeight function to use for the pathfinding (default is dsf::PathWeight::TRAVELTIME) - /// @param weightTreshold The weight treshold for updating the paths (default is std::nullopt) - RoadDynamics(RoadNetwork& graph, - bool useCache = false, - std::optional seed = std::nullopt, - PathWeight const weightFunction = PathWeight::TRAVELTIME, - std::optional weightTreshold = - std::nullopt); // 60 seconds thresholds for paths - - /// @brief Set the error probability - /// @param errorProbability The error probability - /// @throw std::invalid_argument If the error probability is not between 0 and 1 - void setErrorProbability(double errorProbability); - /// @brief Set the passage probability - /// @param passageProbability The passage probability - /// @details The passage probability is the probability of passing through a node - /// It is useful in the case of random agents - void setPassageProbability(double passageProbability); - /// @brief Set the time tolerance factor for killing stagnant agents. - /// An agent will be considered stagnant if it has not moved for timeToleranceFactor * std::ceil(street_length / street_maxSpeed) time units. - /// @param timeToleranceFactor The time tolerance factor - /// @throw std::invalid_argument If the time tolerance factor is not positive - void killStagnantAgents(double timeToleranceFactor = 3.); - /// @brief Set the weight function - /// @param pathWeight The dsf::PathWeight function to use for the pathfinding - /// @param weightThreshold The weight threshold for updating the paths (default is std::nullopt) - void setWeightFunction(PathWeight const pathWeight, - std::optional weightThreshold = std::nullopt); - /// @brief Set the force priorities flag - /// @param forcePriorities The flag - /// @details If true, if an agent cannot move to the next street, the whole node is skipped - inline void setForcePriorities(bool forcePriorities) noexcept { - m_forcePriorities = forcePriorities; - } - /// @brief Set the data update period. - /// @param dataUpdatePeriod delay_t, The period - /// @details Some data, i.e. the street queue lengths, are stored only after a fixed amount of time which is represented by this variable. - inline void setDataUpdatePeriod(delay_t dataUpdatePeriod) noexcept { - m_dataUpdatePeriod = dataUpdatePeriod; - } - /// @brief Set the mean distance travelled by a random agent. The distance will be sampled from an exponential distribution with this mean. - /// @param meanTravelDistance The mean distance - /// @throw std::invalid_argument If the mean distance is negative - inline void setMeanTravelDistance(double const meanTravelDistance) { - meanTravelDistance > 0. ? m_meanTravelDistance = meanTravelDistance - : throw std::invalid_argument( - "RoadDynamics::setMeanTravelDistance: " - "meanTravelDistance must be positive"); - }; - /// @brief Set the mean travel time for random agents. The travel time will be sampled from an exponential distribution with this mean. - /// @param meanTravelTime The mean travel time - inline void setMeanTravelTime(std::time_t const meanTravelTime) noexcept { - m_meanTravelTime = meanTravelTime; - }; - /// @brief Set the origin nodes. If the provided map is empty, the origin nodes are set using the streets' stationary weights. - /// NOTE: the default stationary weights are 1.0 so, if not set, this is equivalent to setting uniform weights. - /// @param originNodes The origin nodes - void setOriginNodes(std::unordered_map const& originNodes = {}); - /// @brief Set the destination nodes - /// @param destinationNodes The destination nodes - void setDestinationNodes(std::unordered_map const& destinationNodes); - /// @brief Set the destination nodes - /// @param destinationNodes The destination nodes (as an initializer list) - void setDestinationNodes(std::initializer_list destinationNodes); - /// @brief Set the origin-destination pairs with their associated weights - /// @param ODs A vector of tuples (origin node id, destination node id, weight) - void setODs(std::vector> const& ODs); - /// @brief Set the destination nodes - /// @param destinationNodes A container of destination nodes ids - /// @details The container must have a value_type convertible to Id and begin() and end() methods - template - requires(std::is_convertible_v) - void setDestinationNodes(TContainer const& destinationNodes); - - virtual void setAgentSpeed(std::unique_ptr const& pAgent) = 0; - /// @brief Initialize the turn counts map - /// @throws std::runtime_error if the turn counts map is already initialized - void initTurnCounts(); - /// @brief Reset the turn counts map values to zero - /// @throws std::runtime_error if the turn counts map is not initialized - void resetTurnCounts(); - /// @brief Enable data saving to the database - /// @param savingInterval The interval at which save the data (in time steps). If zero, saves data at the next time step and then disables saving (working like a manual trigger). - /// @param saveAverageStats If true, saves the average stats of the simulation (default is false) - /// @param saveStreetData If true, saves the street data (default is false) - /// @param saveTravelData If true, saves the travel data of the agents (default is false) - void saveData(std::time_t const savingInterval, - bool const saveAverageStats = false, - bool const saveStreetData = false, - bool const saveTravelData = false); - - /// @brief Update the paths of the itineraries based on the given weight function - /// @param throw_on_empty If true, throws an exception if an itinerary has an empty path (default is true) - /// If false, removes the itinerary with empty paths and the associated node from the origin/destination nodes - /// @throws std::runtime_error if throw_on_empty is true and an itinerary has an empty path - void updatePaths(bool const throw_on_empty = true); - /// @brief Add agents uniformly on the road network - /// @param nAgents The number of agents to add - /// @param itineraryId The id of the itinerary to use (default is std::nullopt) - /// @throw std::runtime_error If there are no itineraries - void addAgentsUniformly(std::size_t nAgents, - std::optional itineraryId = std::nullopt); - - /// @brief Add an agent to the simulation - /// @param agent std::unique_ptr to the agent - void addAgent(std::unique_ptr agent); - - template - requires(std::is_constructible_v) - void addAgent(TArgs&&... args); - - template - requires(std::is_constructible_v) - void addAgents(std::size_t const nAgents, TArgs&&... args); - /// @brief Add agents to the simulation - /// @param nAgents The number of agents to add - /// @param mode The method to use for adding the agents.Possible values are: - /// - ODS: Agents are generated according to the provided OD pairs - /// - RANDOM: Agents spwan randomly in the network and travel with no destination - /// - RANDOM_ODS: Agents are generated by randomly selecting their origin and destination according to the weights - void addAgents(std::size_t const nAgents, AgentInsertionMethod const mode); - - /// @brief Add an itinerary - /// @param ...args The arguments to construct the itinerary - /// @details The arguments must be compatible with any constructor of the Itinerary class - template - requires(std::is_constructible_v) - void addItinerary(TArgs&&... args); - /// @brief Add an itinerary - /// @param itinerary std::unique_ptr to the itinerary - /// @throws std::invalid_argument If the itinerary already exists - /// @throws std::invalid_argument If the itinerary's destination is not a node of the graph - void addItinerary(std::shared_ptr itinerary); - - /// @brief Evolve the simulation - /// @details Evolve the simulation by moving the agents and updating the travel times. - /// In particular: - /// - Move the first agent of each street queue, if possible, putting it in the next node - /// - Move the agents from each node, if possible, putting them in the next street and giving them a speed. - /// If the error probability is not zero, the agents can move to a random street. - /// If the agent is in the destination node, it is removed from the simulation (and then reinserted if reinsert_agents is true) - /// - Cycle over agents and update their times - /// @param reinsert_agents If true, the agents are reinserted in the simulation after they reach their destination - void evolve(bool const reinsert_agents = false); - /// @brief Optimize the traffic lights by changing the green and red times - /// @param optimizationType TrafficLightOptimization, The type of optimization. Default is DOUBLE_TAIL - /// @param logFile The file into which write the logs (default is empty, meaning no logging) - /// @param percentage double, the maximum amount (percentage) of the green time to change (default is 0.3) - /// @param threshold double, The ratio between the self-density and neighbour density to trigger the non-local optimization (default is 1.3) - /// @details The local optimization is done by changing the green time of each traffic light, trying to make it proportional to the - /// queue lengths at each phase. The non-local optimization is done by synchronizing the traffic lights which are congested over threshold. - void optimizeTrafficLights( - TrafficLightOptimization optimizationType = TrafficLightOptimization::DOUBLE_TAIL, - const std::string& logFile = std::string(), - double const percentage = 0.3, - double const threshold = 1.3); - - /// @brief Get the itineraries - /// @return const std::unordered_map&, The itineraries - inline auto const& itineraries() const noexcept { return m_itineraries; } - /// @brief Get the origin nodes of the graph - /// @return std::vector> const& The origin nodes of the graph - inline std::vector> const& originNodes() const noexcept { - return m_originNodes; - } - /// @brief Get the origin nodes of the graph - /// @return std::vector>& The origin nodes of the graph - inline std::vector>& originNodes() noexcept { - return m_originNodes; - } - /// @brief Get the destination nodes of the graph - /// @return std::vector> const& The destination nodes of the graph - inline std::vector> const& destinationNodes() const noexcept { - return m_destinationNodes; - } - /// @brief Get the destination nodes of the graph - /// @return std::vector>>& The destination nodes of the graph - inline std::vector>& destinationNodes() noexcept { - return m_destinationNodes; - } - /// @brief Get the agents - /// @return const std::unordered_map>&, The agents - inline const std::vector>& agents() const noexcept { - return m_agents; - } - /// @brief Get the number of agents currently in the simulation - /// @return std::size_t The number of agents - inline auto nAgents() const { return m_nAgents.load(); }; - - /// @brief Get the mean travel time of the agents in \f$s\f$ - /// @param clearData If true, the travel times are cleared after the computation - /// @return Measurement The mean travel time of the agents and the standard deviation - Measurement meanTravelTime(bool clearData = false); - /// @brief Get the mean travel distance of the agents in \f$m\f$ - /// @param clearData If true, the travel distances are cleared after the computation - /// @return Measurement The mean travel distance of the agents and the standard deviation - Measurement meanTravelDistance(bool clearData = false); - /// @brief Get the mean travel speed of the agents in \f$m/s\f$ - /// @param clearData If true, the travel times and distances are cleared after the computation - /// @return Measurement The mean travel speed of the agents and the standard deviation - Measurement meanTravelSpeed(bool clearData = false); - /// @brief Get the turn counts of the agents - /// @return const std::unordered_map>& The turn counts. The outer map's key is the street id, the inner map's key is the next street id and the value is the number of counts - inline std::unordered_map> const& turnCounts() - const noexcept { - return m_turnCounts; - }; - /// @brief Get the normalized turn counts of the agents - /// @return const std::unordered_map>& The normalized turn counts. The outer map's key is the street id, the inner map's key is the next street id and the value is the normalized number of counts - std::unordered_map> const normalizedTurnCounts() - const noexcept; - - std::unordered_map> turnMapping() const { - return m_turnMapping; - } - - /// @brief Get the origin counts of the agents - /// @param bReset If true, the origin counts are cleared (default is true) - tbb::concurrent_unordered_map originCounts( - bool const bReset = true) noexcept; - /// @brief Get the destination counts of the agents - /// @param bReset If true, the destination counts are cleared (default is true) - tbb::concurrent_unordered_map destinationCounts( - bool const bReset = true) noexcept; - - /// @brief Get the mean density of the streets in \f$m^{-1}\f$ - /// @return Measurement The mean density of the streets and the standard deviation - Measurement streetMeanDensity(bool normalized = false) const; - /// @brief Get the mean flow of the streets in \f$s^{-1}\f$ - /// @return Measurement The mean flow of the streets and the standard deviation - Measurement streetMeanFlow() const; - /// @brief Get the mean flow of the streets in \f$s^{-1}\f$ - /// @param threshold The density threshold to consider - /// @param above If true, the function returns the mean flow of the streets with a density above the threshold, otherwise below - /// @return Measurement The mean flow of the streets and the standard deviation - Measurement streetMeanFlow(double threshold, bool above) const; - - /// @brief Print a summary of the dynamics to an output stream - /// @param os The output stream to write to (default is std::cout) - /// @details The summary includes: - /// - The RoadNetwork description (nodes, edges, capacity, intersections, traffic lights, roundabouts, coil sensors) - /// - Number of inserted agents - /// - Number of added agents - /// - Number of arrived agents - /// - Number of killed agents - /// - Current number of agents in the simulation - void summary(std::ostream& os = std::cout) const; - }; - - template - requires(is_numeric_v) - RoadDynamics::RoadDynamics(RoadNetwork& graph, - bool useCache, - std::optional seed, - PathWeight const weightFunction, - std::optional weightTreshold) - : Dynamics(graph, seed), m_bCacheEnabled{useCache} { - this->setWeightFunction(weightFunction, weightTreshold); - if (m_bCacheEnabled) { - if (!std::filesystem::exists(CACHE_FOLDER)) { - std::filesystem::create_directory(CACHE_FOLDER); - } - spdlog::info("Cache enabled (default folder is {})", CACHE_FOLDER); - } - for (auto const& [nodeId, pNode] : this->graph().nodes()) { - m_nodeIndices.push_back(nodeId); - } - for (auto const& [nodeId, weight] : this->m_destinationNodes) { - m_itineraries.emplace(nodeId, std::make_shared(nodeId, nodeId)); - } - std::for_each( - this->graph().edges().cbegin(), - this->graph().edges().cend(), - [this](auto const& pair) { - auto const& pEdge{pair.second}; - auto const edgeId{pair.first}; - // fill turn mapping as [pair.first, [left street Id, straight street Id, right street Id, U self street Id]] - m_turnMapping.emplace(edgeId, std::array{-1, -1, -1, -1}); - // Turn mappings - const auto& srcNodeId = pEdge->target(); - for (auto const& outEdgeId : this->graph().node(srcNodeId)->outgoingEdges()) { - auto const& pStreet{this->graph().edge(outEdgeId)}; - auto const previousStreetId = pStreet->id(); - auto const& delta{pEdge->deltaAngle(pStreet->angle())}; - if (std::abs(delta) < std::numbers::pi) { - if (delta < 0.) { - m_turnMapping[edgeId][dsf::Direction::RIGHT] = previousStreetId; // right - } else if (delta > 0.) { - m_turnMapping[edgeId][dsf::Direction::LEFT] = previousStreetId; // left - } else { - m_turnMapping[edgeId][dsf::Direction::STRAIGHT] = - previousStreetId; // straight - } - } else { - m_turnMapping[edgeId][dsf::Direction::UTURN] = previousStreetId; // U - } - } - }); - } - - template - requires(is_numeric_v) - std::unique_ptr RoadDynamics::m_killAgent( - std::unique_ptr pAgent) { - spdlog::trace("Killing agent {}", *pAgent); - m_travelDTs.push_back({pAgent->distance(), - static_cast(this->time_step() - pAgent->spawnTime())}); - --m_nAgents; - ++m_nKilledAgents; - auto const& streetId = pAgent->streetId(); - if (streetId.has_value()) { - auto const& pStreet{this->graph().edge(streetId.value())}; - auto const& pNode{this->graph().node(pStreet->target())}; - auto [it, bInserted] = m_destinationCounts.insert({pNode->id(), 1}); - if (!bInserted) { - ++it->second; - } - } - return pAgent; - } - - template - requires(is_numeric_v) - void RoadDynamics::m_updatePath(std::shared_ptr const& pItinerary) { - if (m_bCacheEnabled) { - auto const& file = std::format("{}{}.ity", CACHE_FOLDER, pItinerary->id()); - if (std::filesystem::exists(file)) { - pItinerary->load(file); - spdlog::debug("Loaded cached path for itinerary {}", pItinerary->id()); - return; - } - } - auto const oldSize{pItinerary->path().size()}; - - auto const& path{this->graph().allPathsTo( - pItinerary->destination(), m_weightFunction, m_weightTreshold)}; - pItinerary->setPath(path); - auto const newSize{pItinerary->path().size()}; - if (oldSize > 0 && newSize != oldSize) { - spdlog::debug("Path for itinerary {} changed size from {} to {}", - pItinerary->id(), - oldSize, - newSize); - } - if (m_bCacheEnabled) { - pItinerary->save(std::format("{}{}.ity", CACHE_FOLDER, pItinerary->id())); - spdlog::debug("Saved path in cache for itinerary {}", pItinerary->id()); - } - } - template - requires(is_numeric_v) - void RoadDynamics::m_addAgentsRandom(std::size_t nAgents) { - m_nAddedAgents += nAgents; - std::uniform_real_distribution uniformDist{0., 1.}; - std::exponential_distribution distDist{1. / - m_meanTravelDistance.value_or(1.)}; - std::exponential_distribution timeDist{1. / m_meanTravelTime.value_or(1.)}; - auto const bUniformSpawn{m_originNodes.empty()}; - if (m_originNodes.size() == 1) { - auto [originId, weight] = m_originNodes.at(0); - this->addAgents(nAgents, nullptr, originId); - return; - } - while (nAgents--) { - if (bUniformSpawn) { - this->addAgent(); - } else { - auto randValue{uniformDist(this->m_generator)}; - for (auto const& [origin, weight] : m_originNodes) { - if (randValue < weight) { - this->addAgent(nullptr, origin); - break; - } - randValue -= weight; - } - } - if (m_meanTravelDistance.has_value()) { - auto const& pAgent{this->m_agents.back()}; - pAgent->setMaxDistance(distDist(this->m_generator)); - } - if (m_meanTravelTime.has_value()) { - auto const& pAgent{this->m_agents.back()}; - pAgent->setMaxTime(timeDist(this->m_generator)); - } - } - } - template - requires(is_numeric_v) - void RoadDynamics::m_addAgentsODs(std::size_t nAgents) { - if (m_ODs.empty()) { - throw std::runtime_error( - "RoadDynamics::m_addAgentsODs: No origin-destination pairs provided"); - } - m_nAddedAgents += nAgents; - if (m_ODs.size() == 1) { - auto [originId, destinationId, weight] = m_ODs.at(0); - this->addAgents(nAgents, this->itineraries().at(destinationId), originId); - return; - } - std::uniform_real_distribution uniformDist{ - 0., 1.}; // Weight distribution should be normalized to 1 - while (nAgents--) { - Id originId{0}, destinationId{0}; - auto randValue = uniformDist(this->m_generator); - for (auto const& [origin, destination, weight] : m_ODs) { - if (randValue < weight) { - originId = origin; - destinationId = destination; - break; - } - randValue -= weight; - } - this->addAgent(this->itineraries().at(destinationId), originId); - } - } - template - requires(is_numeric_v) - void RoadDynamics::m_addAgentsRandomODs(std::size_t nAgents) { - m_nAddedAgents += nAgents; - if (m_timeToleranceFactor.has_value() && !m_agents.empty()) { - auto const nStagnantAgents{m_agents.size()}; - spdlog::debug( - "Removing {} stagnant agents that were not inserted since the previous call to " - "addAgentsRandomly().", - nStagnantAgents); - m_agents.clear(); - m_nAgents -= nStagnantAgents; - } - auto const& nSources{m_originNodes.size()}; - auto const& nDestinations{m_destinationNodes.size()}; - spdlog::debug("Init addAgentsRandomly for {} agents from {} nodes to {} nodes.", - nAgents, - nSources, - nDestinations); - if (nSources == 1 && nDestinations == 1 && - std::get(m_originNodes.at(0)) == std::get(m_destinationNodes.at(0))) { - throw std::invalid_argument( - std::format("The only source node {} is also the only destination node.", - std::get(m_originNodes.at(0)))); - } - std::uniform_int_distribution nodeDist{ - 0, static_cast(this->graph().nNodes() - 1)}; - std::uniform_real_distribution uniformDist{0., 1.}; - spdlog::debug("Adding {} agents at time {}.", nAgents, this->time_step()); - while (nAgents--) { - std::optional srcId{std::nullopt}, dstId{std::nullopt}; - - // Select source using weighted random selection - if (nSources == 1) { - srcId = std::get(m_originNodes.at(0)); - } else { - auto randValue = uniformDist(this->m_generator); - for (const auto& [id, weight] : m_originNodes) { - if (randValue < weight) { - srcId = id; - break; - } - randValue -= weight; - } - } - - // Select destination using weighted random selection - if (nDestinations == 1) { - dstId = std::get(m_destinationNodes.at(0)); - } else { - auto randValue = uniformDist(this->m_generator); - for (const auto& [id, weight] : m_destinationNodes) { - if (randValue < weight) { - dstId = id; - break; - } - randValue -= weight; - } - } - - // Fallback to random nodes if selection failed - if (!srcId.has_value()) { - auto nodeIt{this->graph().nodes().begin()}; - std::advance(nodeIt, nodeDist(this->m_generator)); - srcId = nodeIt->first; - } - if (!dstId.has_value()) { - auto nodeIt{this->graph().nodes().begin()}; - std::advance(nodeIt, nodeDist(this->m_generator)); - dstId = nodeIt->first; - } - - // Find the itinerary with the given destination - auto itineraryIt{std::find_if(this->itineraries().cbegin(), - this->itineraries().cend(), - [dstId](const auto& itinerary) { - return itinerary.second->destination() == *dstId; - })}; - if (itineraryIt == this->itineraries().cend()) { - spdlog::error("Itinerary with destination {} not found. Skipping agent.", *dstId); - continue; - } - - // Check if destination is reachable from source - auto const& itinerary = itineraryIt->second; - if (!itinerary->path().contains(*srcId)) { - spdlog::debug("Destination {} not reachable from source {}. Skipping agent.", - *dstId, - *srcId); - continue; - } - - this->addAgent(itineraryIt->second, *srcId); - } - } - - template - requires(is_numeric_v) - std::optional RoadDynamics::m_nextStreetId( - const std::unique_ptr& pAgent, const std::unique_ptr& pNode) { - spdlog::trace("Computing m_nextStreetId for {}", *pAgent); - auto const& outgoingEdges = pNode->outgoingEdges(); - - // Get current street information - std::optional previousNodeId = std::nullopt; - std::set forbiddenTurns; - double speedCurrent{1.0}; - double lengthCurrent{1.0}; - double stationaryWeightCurrent = 1.0; - double bcCurrent{1.0}; - if (pAgent->streetId().has_value()) { - auto const& pStreetCurrent{this->graph().edge(pAgent->streetId().value())}; - previousNodeId = pStreetCurrent->source(); - forbiddenTurns = pStreetCurrent->forbiddenTurns(); - speedCurrent = pStreetCurrent->maxSpeed(); - lengthCurrent = pStreetCurrent->length(); - stationaryWeightCurrent = pStreetCurrent->stationaryWeight(); - bcCurrent = pStreetCurrent->betweennessCentrality().value_or(1.0); - } - - // Get path targets for non-random agents - std::vector pathTargets; - if (!pAgent->isRandom()) { - pathTargets = pAgent->itinerary()->path().at(pNode->id()); - } - - // Calculate transition probabilities for all valid outgoing edges - std::unordered_map transitionProbabilities; - double cumulativeProbability = 0.0; - - for (const auto outEdgeId : outgoingEdges) { - auto const& pStreetOut{this->graph().edge(outEdgeId)}; - - // Check if this is a valid path target for non-random agents - bool bIsPathTarget = false; - if (!pathTargets.empty()) { - bIsPathTarget = - std::find(pathTargets.cbegin(), pathTargets.cend(), pStreetOut->target()) != - pathTargets.cend(); - } - - if (forbiddenTurns.contains(outEdgeId) && !bIsPathTarget) { - continue; - } - - if (!pathTargets.empty()) { - if (!this->m_errorProbability.has_value() && !bIsPathTarget) { - continue; - } - } - - // Calculate base probability - auto const speedNext{pStreetOut->maxSpeed()}; - auto const lengthNext{pStreetOut->length()}; - auto const bcNext{pStreetOut->betweennessCentrality().value_or(1.0)}; - double const stationaryWeightNext = pStreetOut->stationaryWeight(); - auto const weightRatio{stationaryWeightNext / - stationaryWeightCurrent}; // SQRT (p_i / p_j) - double probability = - std::sqrt((bcCurrent * bcNext) * (speedCurrent / lengthCurrent) * - (speedNext / lengthNext) * weightRatio); - - // Apply error probability for non-random agents - if (this->m_errorProbability.has_value() && !pathTargets.empty()) { - probability *= - (bIsPathTarget - ? (1. - this->m_errorProbability.value()) - : this->m_errorProbability.value() / - static_cast(outgoingEdges.size() - pathTargets.size())); - } - - // Handle U-turns - if (previousNodeId.has_value() && pStreetOut->target() == previousNodeId.value()) { - if (pNode->isRoundabout()) { - probability *= U_TURN_PENALTY_FACTOR; - } else if (!bIsPathTarget) { - continue; // No U-turns allowed - } - } - - transitionProbabilities[pStreetOut->id()] = probability; - cumulativeProbability += probability; - } - - // Select street based on weighted probabilities - if (transitionProbabilities.empty()) { - spdlog::debug("No valid transitions found for {} at {}", *pAgent, *pNode); - return std::nullopt; - } - if (transitionProbabilities.size() == 1) { - auto const& onlyStreetId = transitionProbabilities.cbegin()->first; - spdlog::debug("Only one valid transition for {} at {}: street {}", - *pAgent, - *pNode, - onlyStreetId); - return onlyStreetId; - } - - std::uniform_real_distribution uniformDist{0., cumulativeProbability}; - auto const randValue = uniformDist(this->m_generator); - Id fallbackStreetId; - double accumulated = 0.0; - for (const auto& [targetStreetId, probability] : transitionProbabilities) { - accumulated += probability; - fallbackStreetId = targetStreetId; - if (randValue < accumulated) { - return targetStreetId; - } - } - // Return last one as fallback - spdlog::debug( - "Fallback selection for {} at {}: street {}", *pAgent, *pNode, fallbackStreetId); - return fallbackStreetId; - } - - template - requires(is_numeric_v) - void RoadDynamics::m_evolveStreet(const std::unique_ptr& pStreet, - bool reinsert_agents) { - auto const nLanes = pStreet->nLanes(); - // Enqueue moving agents if their free time is up - while (!pStreet->movingAgents().empty()) { - auto const& pAgent{pStreet->movingAgents().top()}; - if (pAgent->freeTime() < this->time_step()) { - break; - } - pAgent->setSpeed(0.); - bool bArrived{false}; - if (!pAgent->isRandom()) { - if (pAgent->itinerary()->destination() == pStreet->target()) { - pAgent->updateItinerary(); - } - if (pAgent->itinerary()->destination() == pStreet->target()) { - bArrived = true; - } - } - if (bArrived) { - std::uniform_int_distribution laneDist{0, - static_cast(nLanes - 1)}; - pStreet->enqueue(laneDist(this->m_generator)); - continue; - } - auto const nextStreetId = - this->m_nextStreetId(pAgent, this->graph().node(pStreet->target())); - if (!nextStreetId.has_value()) { - spdlog::debug( - "No next street found for agent {} at node {}", *pAgent, pStreet->target()); - if (pAgent->isRandom()) { - std::uniform_int_distribution laneDist{0, - static_cast(nLanes - 1)}; - pStreet->enqueue(laneDist(this->m_generator)); - continue; - } - this->m_killAgent(pStreet->dequeueMovingAgent()); - continue; - // Grufoony - 09/03/2026 - // The agent is now killed. The old behavior (throw exception) is kept here: - // - // throw std::runtime_error(std::format( - // "No next street found for agent {} at node {}", *pAgent, pStreet->target())); - } - auto const& pNextStreet{this->graph().edge(nextStreetId.value())}; - pAgent->setNextStreetId(pNextStreet->id()); - if (nLanes == 1) { - pStreet->enqueue(0); - continue; - } - auto const direction{pNextStreet->turnDirection(pStreet->angle())}; - switch (direction) { - case Direction::UTURN: - case Direction::LEFT: - pStreet->enqueue(nLanes - 1); - break; - case Direction::RIGHT: - pStreet->enqueue(0); - break; - default: - std::vector weights; - for (auto const& queue : pStreet->exitQueues()) { - weights.push_back(1. / (queue.size() + 1)); - } - // If all weights are the same, make the last 0 - if (std::all_of(weights.begin(), weights.end(), [&](double w) { - return std::abs(w - weights.front()) < - std::numeric_limits::epsilon(); - })) { - weights.back() = 0.; - if (nLanes > 2) { - weights.front() = 0.; - } - } - // Normalize the weights - auto const sum = std::accumulate(weights.begin(), weights.end(), 0.); - for (auto& w : weights) { - w /= sum; - } - std::discrete_distribution laneDist{weights.begin(), weights.end()}; - pStreet->enqueue(laneDist(this->m_generator)); - } - } - auto const& transportCapacity{pStreet->transportCapacity()}; - std::uniform_real_distribution uniformDist{0., 1.}; - for (auto i = 0; i < std::ceil(transportCapacity); ++i) { - if (i == std::ceil(transportCapacity) - 1) { - double integral; - double fractional = std::modf(transportCapacity, &integral); - if (fractional != 0. && uniformDist(this->m_generator) > fractional) { - spdlog::trace("Skipping due to fractional capacity {:.2f} < random value", - fractional); - continue; - } - } - for (auto queueIndex = 0; queueIndex < nLanes; ++queueIndex) { - if (pStreet->queue(queueIndex).empty()) { - continue; - } - // Logger::debug("Taking temp agent"); - auto const& pAgentTemp{pStreet->queue(queueIndex).front()}; - if (pAgentTemp->freeTime() > this->time_step()) { - spdlog::trace("Skipping due to time {} < free time {}", - this->time_step(), - pAgentTemp->freeTime()); - continue; - } - - if (m_timeToleranceFactor.has_value()) { - auto const timeDiff{this->time_step() - pAgentTemp->freeTime()}; - auto const timeTolerance{m_timeToleranceFactor.value() * - std::ceil(pStreet->length() / pStreet->maxSpeed())}; - if (timeDiff > timeTolerance) { - spdlog::debug( - "Time-step {} - {} currently on {} ({} turn - Traffic Light? {}), " - "has been still for more than {} seconds ({} seconds). Killing it.", - this->time_step(), - *pAgentTemp, - *pStreet, - directionToString.at(pStreet->laneMapping().at(queueIndex)), - this->graph().node(pStreet->target())->isTrafficLight(), - timeTolerance, - timeDiff); - // Kill the agent - this->m_killAgent(pStreet->dequeue(queueIndex, this->time_step())); - continue; - } - } - pAgentTemp->setSpeed(0.); - const auto& destinationNode{this->graph().node(pStreet->target())}; - if (destinationNode->isFull()) { - spdlog::trace("Skipping due to full destination node {}", *destinationNode); - continue; - } - if (destinationNode->isTrafficLight()) { - auto& tl = dynamic_cast(*destinationNode); - auto const direction{pStreet->laneMapping().at(queueIndex)}; - if (!tl.isGreen(pStreet->id(), direction)) { - spdlog::trace("Skipping due to red light on street {} and direction {}", - pStreet->id(), - directionToString.at(direction)); - continue; - } - spdlog::debug("Green light on street {} and direction {}", - pStreet->id(), - directionToString.at(direction)); - } else if (destinationNode->isIntersection() && - pAgentTemp->nextStreetId().has_value()) { - auto& intersection = static_cast(*destinationNode); - bool bCanPass{true}; - if (!intersection.streetPriorities().empty()) { - spdlog::debug("Checking priorities for street {} -> {}", - pStreet->source(), - pStreet->target()); - auto const& thisDirection{this->graph() - .edge(pAgentTemp->nextStreetId().value()) - ->turnDirection(pStreet->angle())}; - if (!intersection.streetPriorities().contains(pStreet->id())) { - // I have to check if the agent has right of way - auto const& inNeighbours{destinationNode->ingoingEdges()}; - for (auto const& inEdgeId : inNeighbours) { - auto const& pStreetTemp{this->graph().edge(inEdgeId)}; - if (pStreetTemp->id() == pStreet->id()) { - continue; - } - if (pStreetTemp->nExitingAgents() == 0) { - continue; - } - if (intersection.streetPriorities().contains(pStreetTemp->id())) { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to right of way.", - pStreet->source(), - pStreet->target()); - bCanPass = false; - break; - } else if (thisDirection >= Direction::LEFT) { - // Check if the agent has right of way using direction - // The problem arises only when you have to turn left - for (auto i{0}; i < pStreetTemp->nLanes(); ++i) { - // check queue is not empty and take the top agent - if (pStreetTemp->queue(i).empty()) { - continue; - } - auto const& pAgentTemp2{pStreetTemp->queue(i).front()}; - if (!pAgentTemp2->nextStreetId().has_value()) { - continue; - } - auto const& otherDirection{ - this->graph() - .edge(pAgentTemp2->nextStreetId().value()) - ->turnDirection(this->graph() - .edge(pAgentTemp2->streetId().value()) - ->angle())}; - if (otherDirection < Direction::LEFT) { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to right of " - "way with other agents.", - pStreet->source(), - pStreet->target()); - bCanPass = false; - break; - } - } - } - } - } else if (thisDirection >= Direction::LEFT) { - for (auto const& streetId : intersection.streetPriorities()) { - if (streetId == pStreet->id()) { - continue; - } - auto const& pStreetTemp{this->graph().edge(streetId)}; - for (auto i{0}; i < pStreetTemp->nLanes(); ++i) { - // check queue is not empty and take the top agent - if (pStreetTemp->queue(i).empty()) { - continue; - } - auto const& pAgentTemp2{pStreetTemp->queue(i).front()}; - if (!pAgentTemp2->streetId().has_value()) { - continue; - } - auto const& otherDirection{ - this->graph() - .edge(pAgentTemp2->nextStreetId().value()) - ->turnDirection(this->graph() - .edge(pAgentTemp2->streetId().value()) - ->angle())}; - if (otherDirection < thisDirection) { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to right of " - "way with other agents.", - pStreet->source(), - pStreet->target()); - bCanPass = false; - break; - } - } - } - } - } - if (!bCanPass) { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to right of way", - pStreet->source(), - pStreet->target()); - continue; - } - } - bool bArrived{false}; - if (!(uniformDist(this->m_generator) < - m_passageProbability.value_or(std::numeric_limits::max()))) { - if (pAgentTemp->isRandom()) { - bArrived = true; - } else { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to passage " - "probability", - pStreet->source(), - pStreet->target()); - continue; - } - } - if (!pAgentTemp->isRandom()) { - if (destinationNode->id() == pAgentTemp->itinerary()->destination()) { - bArrived = true; - spdlog::debug("Agent {} has arrived at destination node {}", - pAgentTemp->id(), - destinationNode->id()); - } - } else { - if (!pAgentTemp->nextStreetId().has_value()) { - bArrived = true; - spdlog::debug("Random agent {} has arrived at destination node {}", - pAgentTemp->id(), - destinationNode->id()); - } else if (pAgentTemp->hasArrived(this->time_step())) { - bArrived = true; - } - } - if (bArrived) { - auto pAgent = - this->m_killAgent(pStreet->dequeue(queueIndex, this->time_step())); - ++m_nArrivedAgents; - if (reinsert_agents) { - // reset Agent's values - pAgent->reset(this->time_step()); - this->addAgent(std::move(pAgent)); - } - continue; - } - if (!pAgentTemp->streetId().has_value()) { - spdlog::error("{} has no street id", *pAgentTemp); - } - auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())}; - if (nextStreet->isFull()) { - spdlog::debug( - "Skipping agent emission from street {} -> {} due to full " - "next street: {}", - pStreet->source(), - pStreet->target(), - *nextStreet); - continue; - } - auto pAgent{pStreet->dequeue(queueIndex, this->time_step())}; - spdlog::debug( - "{} at time {} has been dequeued from street {} and enqueued on street {} " - "with free time {}.", - *pAgent, - this->time_step(), - pStreet->id(), - nextStreet->id(), - pAgent->freeTime()); - assert(destinationNode->id() == nextStreet->source()); - if (destinationNode->isIntersection()) { - auto& intersection = dynamic_cast(*destinationNode); - auto const delta{nextStreet->deltaAngle(pStreet->angle())}; - intersection.addAgent(delta, std::move(pAgent)); - } else if (destinationNode->isRoundabout()) { - auto& roundabout = dynamic_cast(*destinationNode); - roundabout.enqueue(std::move(pAgent)); - } - } - } - } - - template - requires(is_numeric_v) - void RoadDynamics::m_evolveNode(const std::unique_ptr& pNode) { - auto const transportCapacity{pNode->transportCapacity()}; - for (auto i{0}; i < std::ceil(transportCapacity); ++i) { - if (i == std::ceil(transportCapacity) - 1) { - std::uniform_real_distribution uniformDist{0., 1.}; - double integral; - double fractional = std::modf(transportCapacity, &integral); - if (fractional != 0. && uniformDist(this->m_generator) > fractional) { - spdlog::debug("Skipping dequeue from node {} due to transport capacity {}", - pNode->id(), - transportCapacity); - return; - } - } - if (pNode->isIntersection()) { - auto& intersection = dynamic_cast(*pNode); - if (intersection.agents().empty()) { - return; - } - for (auto it{intersection.agents().begin()}; it != intersection.agents().end();) { - auto& pAgent{it->second}; - auto const& nextStreet{this->graph().edge(pAgent->nextStreetId().value())}; - if (nextStreet->isFull()) { - spdlog::debug("Next street is full: {}", *nextStreet); - if (m_forcePriorities) { - spdlog::debug("Forcing priority from {} on {}", *pNode, *nextStreet); - return; - } - ++it; - continue; - } - if (!m_turnCounts.empty() && pAgent->streetId().has_value()) { - ++m_turnCounts[*(pAgent->streetId())][nextStreet->id()]; - } - pAgent->setStreetId(); - this->setAgentSpeed(pAgent); - pAgent->setFreeTime(this->time_step() + - std::ceil(nextStreet->length() / pAgent->speed())); - spdlog::debug( - "{} at time {} has been dequeued from intersection {} and " - "enqueued on street {} with free time {}.", - *pAgent, - this->time_step(), - pNode->id(), - nextStreet->id(), - pAgent->freeTime()); - nextStreet->addAgent(std::move(pAgent), this->time_step()); - it = intersection.agents().erase(it); - break; - } - } else if (pNode->isRoundabout()) { - auto& roundabout = dynamic_cast(*pNode); - if (roundabout.agents().empty()) { - return; - } - auto const& pAgentTemp{roundabout.agents().front()}; - auto const& nextStreet{this->graph().edge(pAgentTemp->nextStreetId().value())}; - if (!(nextStreet->isFull())) { - if (!m_turnCounts.empty() && pAgentTemp->streetId().has_value()) { - ++m_turnCounts[*(pAgentTemp->streetId())][nextStreet->id()]; - } - auto pAgent{roundabout.dequeue()}; - pAgent->setStreetId(); - this->setAgentSpeed(pAgent); - pAgent->setFreeTime(this->time_step() + - std::ceil(nextStreet->length() / pAgent->speed())); - spdlog::debug( - "An agent at time {} has been dequeued from roundabout {} and " - "enqueued on street {} with free time {}: {}", - this->time_step(), - pNode->id(), - nextStreet->id(), - pAgent->freeTime(), - *pAgent); - nextStreet->addAgent(std::move(pAgent), this->time_step()); - } else { - return; - } - } - } - } - template - requires(is_numeric_v) - void RoadDynamics::m_evolveAgents() { - if (m_agents.empty()) { - spdlog::trace("No agents to process."); - return; - } - std::uniform_int_distribution nodeDist{ - 0, static_cast(this->graph().nNodes() - 1)}; - spdlog::debug("Processing {} agents", m_agents.size()); - for (auto itAgent{m_agents.begin()}; itAgent != m_agents.end();) { - auto& pAgent{*itAgent}; - if (!pAgent->srcNodeId().has_value()) { - auto nodeIt{this->graph().nodes().begin()}; - std::advance(nodeIt, nodeDist(this->m_generator)); - pAgent->setSrcNodeId(nodeIt->second->id()); - } - auto const& pSourceNode{this->graph().node(*(pAgent->srcNodeId()))}; - if (pSourceNode->isFull()) { - spdlog::debug("Skipping {} due to full source {}", *pAgent, *pSourceNode); - ++itAgent; - continue; - } - if (!pAgent->nextStreetId().has_value()) { - spdlog::debug("No next street id, generating a random one"); - auto const nextStreetId{this->m_nextStreetId(pAgent, pSourceNode)}; - if (!nextStreetId.has_value()) { - spdlog::debug( - "No next street found for agent {} at node {}", *pAgent, pSourceNode->id()); - itAgent = m_agents.erase(itAgent); - continue; - } - pAgent->setNextStreetId(nextStreetId.value()); - } - // spdlog::debug("Checking next street {}", pAgent->nextStreetId().value()); - auto const& nextStreet{ - this->graph().edge(pAgent->nextStreetId().value())}; // next street - if (nextStreet->isFull()) { - ++itAgent; - spdlog::debug("Skipping {} due to full input {}", *pAgent, *nextStreet); - continue; - } - // spdlog::debug("Adding agent on the source node"); - if (pSourceNode->isIntersection()) { - auto& intersection = dynamic_cast(*pSourceNode); - intersection.addAgent(0., std::move(pAgent)); - } else if (pSourceNode->isRoundabout()) { - auto& roundabout = dynamic_cast(*pSourceNode); - roundabout.enqueue(std::move(pAgent)); - } - itAgent = m_agents.erase(itAgent); - } - spdlog::debug("There are {} agents left in the list.", m_agents.size()); - } - - template - requires(is_numeric_v) - void RoadDynamics::m_initStreetTable() const { - if (!this->database()) { - throw std::runtime_error( - "No database connected. Call connectDataBase() before saving data."); - } - // Create table if it doesn't exist - this->database()->exec( - "CREATE TABLE IF NOT EXISTS road_data (" - "id INTEGER PRIMARY KEY AUTOINCREMENT, " - "simulation_id INTEGER NOT NULL, " - "datetime TEXT NOT NULL, " - "time_step INTEGER NOT NULL, " - "street_id INTEGER NOT NULL, " - "coil TEXT, " - "density_vpk REAL, " - "avg_speed_kph REAL, " - "std_speed_kph REAL, " - "n_observations INTEGER, " - "counts INTEGER, " - "queue_length INTEGER)"); - - spdlog::info("Initialized road_data table in the database."); - } - template - requires(is_numeric_v) - void RoadDynamics::m_initAvgStatsTable() const { - if (!this->database()) { - throw std::runtime_error( - "No database connected. Call connectDataBase() before saving data."); - } - // Create table if it doesn't exist - this->database()->exec( - "CREATE TABLE IF NOT EXISTS avg_stats (" - "id INTEGER PRIMARY KEY AUTOINCREMENT, " - "simulation_id INTEGER NOT NULL, " - "datetime TEXT NOT NULL, " - "time_step INTEGER NOT NULL, " - "n_ghost_agents INTEGER NOT NULL, " - "n_agents INTEGER NOT NULL, " - "mean_speed_kph REAL, " - "std_speed_kph REAL, " - "mean_density_vpk REAL NOT NULL, " - "std_density_vpk REAL NOT NULL)"); - - spdlog::info("Initialized avg_stats table in the database."); - } - template - requires(is_numeric_v) - void RoadDynamics::m_initTravelDataTable() const { - if (!this->database()) { - throw std::runtime_error( - "No database connected. Call connectDataBase() before saving data."); - } - // Create table if it doesn't exist - this->database()->exec( - "CREATE TABLE IF NOT EXISTS travel_data (" - "id INTEGER PRIMARY KEY AUTOINCREMENT, " - "simulation_id INTEGER NOT NULL, " - "datetime TEXT NOT NULL, " - "time_step INTEGER NOT NULL, " - "distance_m REAL NOT NULL, " - "travel_time_s REAL NOT NULL)"); - - spdlog::info("Initialized travel_data table in the database."); - } - template - requires(is_numeric_v) - void RoadDynamics::m_dumpNetwork() const { - if (!this->database()) { - throw std::runtime_error( - "No database connected. Call connectDataBase() before saving data."); - } - // Check if edges and nodes tables already exists - SQLite::Statement edgesQuery( - *this->database(), - "SELECT name FROM sqlite_master WHERE type='table' AND name='edges';"); - SQLite::Statement nodesQuery( - *this->database(), - "SELECT name FROM sqlite_master WHERE type='table' AND name='nodes';"); - bool edgesTableExists = edgesQuery.executeStep(); - bool nodesTableExists = nodesQuery.executeStep(); - if (edgesTableExists && nodesTableExists) { - spdlog::info( - "Edges and nodes tables already exist in the database. Skipping network dump."); - return; - } - - // Create edges table - this->database()->exec( - "CREATE TABLE IF NOT EXISTS edges (" - "id INTEGER PRIMARY KEY, " - "source INTEGER NOT NULL, " - "target INTEGER NOT NULL, " - "length REAL NOT NULL, " - "maxspeed REAL NOT NULL, " - "name TEXT, " - "nlanes INTEGER NOT NULL, " - "geometry TEXT NOT NULL)"); - // Create nodes table - this->database()->exec( - "CREATE TABLE IF NOT EXISTS nodes (" - "id INTEGER PRIMARY KEY, " - "type TEXT, " - "geometry TEXT)"); - - // Insert edges - SQLite::Statement insertEdgeStmt(*this->database(), - "INSERT INTO edges (id, source, target, length, " - "maxspeed, name, nlanes, geometry) " - "VALUES (?, ?, ?, ?, ?, ?, ?, ?);"); - for (const auto& [edgeId, pEdge] : this->graph().edges()) { - insertEdgeStmt.bind(1, static_cast(edgeId)); - insertEdgeStmt.bind(2, static_cast(pEdge->source())); - insertEdgeStmt.bind(3, static_cast(pEdge->target())); - insertEdgeStmt.bind(4, pEdge->length()); - insertEdgeStmt.bind(5, pEdge->maxSpeed()); - insertEdgeStmt.bind(6, pEdge->name()); - insertEdgeStmt.bind(7, pEdge->nLanes()); - insertEdgeStmt.bind(8, std::format("{}", pEdge->geometry())); - insertEdgeStmt.exec(); - insertEdgeStmt.reset(); - } - // Insert nodes - SQLite::Statement insertNodeStmt( - *this->database(), "INSERT INTO nodes (id, type, geometry) VALUES (?, ?, ?);"); - for (const auto& [nodeId, pNode] : this->graph().nodes()) { - insertNodeStmt.bind(1, static_cast(nodeId)); - if (pNode->isTrafficLight()) { - insertNodeStmt.bind(2, "traffic_light"); - } else if (pNode->isRoundabout()) { - insertNodeStmt.bind(2, "roundabout"); - } else { - insertNodeStmt.bind(2); - } - if (pNode->geometry().has_value()) { - insertNodeStmt.bind(3, std::format("{}", *pNode->geometry())); - } else { - insertNodeStmt.bind(3); - } - insertNodeStmt.exec(); - insertNodeStmt.reset(); - } - } - - template - requires(is_numeric_v) - void RoadDynamics::setErrorProbability(double errorProbability) { - if (errorProbability < 0. || errorProbability > 1.) { - throw std::invalid_argument( - std::format("The error probability ({}) must be in [0, 1]", errorProbability)); - } - m_errorProbability = errorProbability; - } - template - requires(is_numeric_v) - void RoadDynamics::setPassageProbability(double passageProbability) { - if (passageProbability < 0. || passageProbability > 1.) { - throw std::invalid_argument(std::format( - "The passage probability ({}) must be in [0, 1]", passageProbability)); - } - m_passageProbability = passageProbability; - } - template - requires(is_numeric_v) - void RoadDynamics::killStagnantAgents(double timeToleranceFactor) { - if (timeToleranceFactor <= 0.) { - throw std::invalid_argument(std::format( - "The time tolerance factor ({}) must be positive", timeToleranceFactor)); - } - m_timeToleranceFactor = timeToleranceFactor; - } - template - requires(is_numeric_v) - void RoadDynamics::setWeightFunction(PathWeight const pathWeight, - std::optional weightTreshold) { - m_pathWeight = pathWeight; - switch (pathWeight) { - case PathWeight::LENGTH: - m_weightFunction = [](std::unique_ptr const& pStreet) { - return pStreet->length(); - }; - m_weightTreshold = weightTreshold.value_or(1.); - break; - case PathWeight::TRAVELTIME: - m_weightFunction = [this](std::unique_ptr const& pStreet) { - return this->m_streetEstimatedTravelTime(pStreet); - }; - m_weightTreshold = weightTreshold.value_or(0.0069); - break; - case PathWeight::WEIGHT: - m_weightFunction = [](std::unique_ptr const& pStreet) { - return pStreet->weight(); - }; - m_weightTreshold = weightTreshold.value_or(1.); - break; - default: - spdlog::error("Invalid weight function. Defaulting to traveltime"); - m_weightFunction = [this](std::unique_ptr const& pStreet) { - return this->m_streetEstimatedTravelTime(pStreet); - }; - m_weightTreshold = weightTreshold.value_or(0.0069); - break; - } - } - template - requires(is_numeric_v) - void RoadDynamics::setOriginNodes( - std::unordered_map const& originNodes) { - m_originNodes.clear(); - m_originNodes.reserve(originNodes.size()); - if (originNodes.empty()) { - // If no origin nodes are provided, try to set origin nodes basing on streets' stationary weights - double totalStationaryWeight = 0.0; - for (auto const& [edgeId, pEdge] : this->graph().edges()) { - auto const& weight = pEdge->stationaryWeight(); - if (weight <= 0.) { - continue; - } - m_originNodes.push_back({pEdge->source(), weight}); - totalStationaryWeight += weight; - } - for (auto& [nodeId, weight] : m_originNodes) { - weight /= totalStationaryWeight; - } - return; - } - auto const sumWeights = std::accumulate( - originNodes.begin(), originNodes.end(), 0., [](double sum, auto const& pair) { - return sum + pair.second; - }); - if (sumWeights <= 0.) { - throw std::invalid_argument( - std::format("The sum of the weights ({}) must be positive", sumWeights)); - } - if (sumWeights == 1.) { - std::copy( - originNodes.begin(), originNodes.end(), std::back_inserter(m_originNodes)); - return; - } - std::transform(originNodes.begin(), - originNodes.end(), - std::back_inserter(m_originNodes), - [sumWeights](auto const& pair) -> std::pair { - return {pair.first, pair.second / sumWeights}; - }); - } - template - requires(is_numeric_v) - void RoadDynamics::setDestinationNodes( - std::unordered_map const& destinationNodes) { - m_itineraries.clear(); - m_destinationNodes.clear(); - m_destinationNodes.reserve(destinationNodes.size()); - auto sumWeights{0.}; - std::for_each(destinationNodes.begin(), - destinationNodes.end(), - [this, &sumWeights](auto const& pair) -> void { - sumWeights += pair.second; - this->addItinerary(pair.first, pair.first); - }); - if (sumWeights <= 0.) { - throw std::invalid_argument( - std::format("The sum of the weights ({}) must be positive", sumWeights)); - } - if (sumWeights == 1.) { - std::copy(destinationNodes.begin(), - destinationNodes.end(), - std::back_inserter(m_destinationNodes)); - return; - } - std::transform(destinationNodes.begin(), - destinationNodes.end(), - std::back_inserter(m_destinationNodes), - [sumWeights](auto const& pair) -> std::pair { - return {pair.first, pair.second / sumWeights}; - }); - } - - template - requires(is_numeric_v) - void RoadDynamics::initTurnCounts() { - if (!m_turnCounts.empty()) { - throw std::runtime_error("Turn counts have already been initialized."); - } - for (auto const& [edgeId, pEdge] : this->graph().edges()) { - auto const& pTargetNode{this->graph().node(pEdge->target())}; - for (auto const& nextEdgeId : pTargetNode->outgoingEdges()) { - spdlog::debug("Initializing turn count for edge {} -> {}", edgeId, nextEdgeId); - m_turnCounts[edgeId][nextEdgeId] = 0; - } - } - } - // You may wonder why not just use one function... - // Never trust the user! - // Jokes aside, the init is necessary because it allocates the memory for the first time and - // turn counts are not incremented if the map is empty for performance reasons. - template - requires(is_numeric_v) - void RoadDynamics::resetTurnCounts() { - if (m_turnCounts.empty()) { - throw std::runtime_error("Turn counts have not been initialized."); - } - for (auto const& [edgeId, pEdge] : this->graph().edges()) { - auto const& pTargetNode{this->graph().node(pEdge->target())}; - for (auto const& nextEdgeId : pTargetNode->outgoingEdges()) { - m_turnCounts[edgeId][nextEdgeId] = 0; - } - } - } - - template - requires(is_numeric_v) - void RoadDynamics::saveData(std::time_t const savingInterval, - bool const saveAverageStats, - bool const saveStreetData, - bool const saveTravelData) { - m_savingInterval = savingInterval; - m_bSaveAverageStats = saveAverageStats; - m_bSaveStreetData = saveStreetData; - m_bSaveTravelData = saveTravelData; - - // Initialize the required tables - if (saveStreetData) { - m_initStreetTable(); - } - if (saveAverageStats) { - m_initAvgStatsTable(); - } - if (saveTravelData) { - m_initTravelDataTable(); - } - - this->m_dumpSimInfo(); - this->m_dumpNetwork(); - - spdlog::info( - "Data saving configured: interval={}s, avg_stats={}, street_data={}, " - "travel_data={}", - savingInterval, - saveAverageStats, - saveStreetData, - saveTravelData); - } - - template - requires(is_numeric_v) - void RoadDynamics::setDestinationNodes( - std::initializer_list destinationNodes) { - m_itineraries.clear(); - auto const numNodes{destinationNodes.size()}; - m_destinationNodes.clear(); - m_destinationNodes.reserve(numNodes); - std::for_each(destinationNodes.begin(), - destinationNodes.end(), - [this, &numNodes](auto const& nodeId) -> void { - this->m_destinationNodes.push_back({nodeId, 1. / numNodes}); - this->addItinerary(nodeId, nodeId); - }); - } - template - requires(is_numeric_v) - template - requires(std::is_convertible_v) - void RoadDynamics::setDestinationNodes(TContainer const& destinationNodes) { - m_itineraries.clear(); - auto const numNodes{destinationNodes.size()}; - m_destinationNodes.clear(); - m_destinationNodes.reserve(numNodes); - std::for_each(destinationNodes.begin(), - destinationNodes.end(), - [this, &numNodes](auto const& nodeId) -> void { - this->m_destinationNodes.push_back({nodeId, 1. / numNodes}); - this->addItinerary(nodeId, nodeId); - }); - } - template - requires(is_numeric_v) - void RoadDynamics::setODs(std::vector> const& ODs) { - m_ODs.clear(); - auto const sumWeights = std::accumulate( - ODs.begin(), ODs.end(), 0., [this](double sum, auto const& tuple) { - // Add itineraries while summing weights - if (!this->itineraries().contains(std::get<1>(tuple))) { - this->addItinerary(std::get<1>(tuple), std::get<1>(tuple)); - } - return sum + std::get<2>(tuple); - }); - if (sumWeights <= 0.) { - throw std::invalid_argument( - std::format("The sum of the weights ({}) must be positive", sumWeights)); - } - if (sumWeights == 1.) { - std::copy(ODs.begin(), ODs.end(), std::back_inserter(m_ODs)); - return; - } - // Copy but divide by weights sum - std::transform(ODs.begin(), - ODs.end(), - std::back_inserter(m_ODs), - [sumWeights](auto const& tuple) { - return std::make_tuple(std::get<0>(tuple), - std::get<1>(tuple), - std::get<2>(tuple) / sumWeights); - }); - } - - template - requires(is_numeric_v) - void RoadDynamics::updatePaths(bool const throw_on_empty) { - spdlog::debug("Init updating paths..."); - tbb::concurrent_vector emptyItineraries; - tbb::parallel_for_each( - this->itineraries().cbegin(), - this->itineraries().cend(), - [this, throw_on_empty, &emptyItineraries](auto const& pair) -> void { - auto const& pItinerary{pair.second}; - this->m_updatePath(pItinerary); - if (pItinerary->empty()) { - if (!throw_on_empty) { - spdlog::warn("No path found for itinerary {} with destination node {}", - pItinerary->id(), - pItinerary->destination()); - emptyItineraries.push_back(pItinerary->id()); - return; - } - throw std::runtime_error( - std::format("No path found for itinerary {} with destination node {}", - pItinerary->id(), - pItinerary->destination())); - } - }); - if (!emptyItineraries.empty()) { - spdlog::warn("Removing {} itineraries with no valid path from the dynamics.", - emptyItineraries.size()); - for (auto const& id : emptyItineraries) { - auto const destination = m_itineraries.at(id)->destination(); - std::erase_if(m_ODs, [destination](auto const& tuple) { - return std::get<1>(tuple) == destination; - }); - std::erase_if(m_destinationNodes, [destination](auto const& tuple) { - return std::get<0>(tuple) == destination; - }); - std::erase_if(m_originNodes, [destination](auto const& tuple) { - return std::get<0>(tuple) == destination; - }); - m_itineraries.erase(id); - } - } - spdlog::debug("End updating paths."); - } - - template - requires(is_numeric_v) - void RoadDynamics::addAgentsUniformly(std::size_t nAgents, - std::optional optItineraryId) { - m_nAddedAgents += nAgents; - if (m_timeToleranceFactor.has_value() && !m_agents.empty()) { - auto const nStagnantAgents{m_agents.size()}; - spdlog::debug( - "Removing {} stagnant agents that were not inserted since the previous call to " - "addAgentsUniformly().", - nStagnantAgents); - m_agents.clear(); - m_nAgents -= nStagnantAgents; - } - if (optItineraryId.has_value() && !this->itineraries().contains(*optItineraryId)) { - throw std::invalid_argument( - std::format("No itineraries available. Cannot add agents with itinerary id {}", - optItineraryId.value())); - } - bool const bRandomItinerary{!optItineraryId.has_value() && - !this->itineraries().empty()}; - std::shared_ptr pItinerary; - std::uniform_int_distribution itineraryDist{ - 0, this->itineraries().size() - 1}; - std::uniform_int_distribution streetDist{0, this->graph().nEdges() - 1}; - if (this->nAgents() + nAgents > this->graph().capacity()) { - throw std::overflow_error(std::format( - "Cannot add {} agents. The graph has currently {} with a maximum capacity of " - "{}.", - nAgents, - this->nAgents(), - this->graph().capacity())); - } - for (std::size_t i{0}; i < nAgents; ++i) { - if (bRandomItinerary) { - auto itineraryIt{this->itineraries().cbegin()}; - std::advance(itineraryIt, itineraryDist(this->m_generator)); - pItinerary = itineraryIt->second; - } - auto streetIt = this->graph().edges().begin(); - while (true) { - auto step = streetDist(this->m_generator); - std::advance(streetIt, step); - if (!(streetIt->second->isFull())) { - break; - } - streetIt = this->graph().edges().begin(); - } - auto const& street{streetIt->second}; - this->addAgent(pItinerary, street->source()); - auto& pAgent{this->m_agents.back()}; - pAgent->setStreetId(street->id()); - this->setAgentSpeed(pAgent); - pAgent->setFreeTime(this->time_step() + - std::ceil(street->length() / pAgent->speed())); - street->addAgent(std::move(pAgent), this->time_step()); - this->m_agents.pop_back(); - } - } - - template - requires(is_numeric_v) - void RoadDynamics::addAgent(std::unique_ptr pAgent) { - m_agents.push_back(std::move(pAgent)); - ++m_nAgents; - ++m_nInsertedAgents; - spdlog::trace("Added {}", *m_agents.back()); - auto const& optNodeId{m_agents.back()->srcNodeId()}; - if (optNodeId.has_value()) { - auto [it, bInserted] = m_originCounts.insert({*optNodeId, 1}); - if (!bInserted) { - ++it->second; - } - } - } - - template - requires(is_numeric_v) - template - requires(std::is_constructible_v) - void RoadDynamics::addAgent(TArgs&&... args) { - addAgent(std::make_unique( - this->m_nInsertedAgents, this->time_step(), std::forward(args)...)); - } - - template - requires(is_numeric_v) - template - requires(std::is_constructible_v) - void RoadDynamics::addAgents(std::size_t const nAgents, TArgs&&... args) { - for (size_t i{0}; i < nAgents; ++i) { - addAgent(std::make_unique( - this->m_nInsertedAgents, this->time_step(), std::forward(args)...)); - } - } - template - requires(is_numeric_v) - void RoadDynamics::addAgents(std::size_t const nAgents, - AgentInsertionMethod const mode) { - switch (mode) { - case AgentInsertionMethod::RANDOM: - this->m_addAgentsRandom(nAgents); - break; - case AgentInsertionMethod::ODS: - this->m_addAgentsODs(nAgents); - break; - case AgentInsertionMethod::RANDOM_ODS: - this->m_addAgentsRandomODs(nAgents); - break; - default: - throw std::runtime_error( - "Cannot add agents without a valid insertion methods. Possible values are " - "\"RANDOM\", \"ODS\" and \"RANDOM_ODS\""); - } - } - - template - requires(is_numeric_v) - template - requires(std::is_constructible_v) - void RoadDynamics::addItinerary(TArgs&&... args) { - addItinerary(std::make_shared(std::forward(args)...)); - } - - template - requires(is_numeric_v) - void RoadDynamics::addItinerary(std::shared_ptr itinerary) { - if (m_itineraries.contains(itinerary->id())) { - throw std::invalid_argument( - std::format("Itinerary with id {} already exists.", itinerary->id())); - } - m_itineraries.emplace(itinerary->id(), std::move(itinerary)); - } - - template - requires(is_numeric_v) - void RoadDynamics::evolve(bool const reinsert_agents) { - auto const n_threads{std::max(1, this->concurrency())}; - std::atomic mean_speed{0.}, mean_density{0.}; - std::atomic std_speed{0.}, std_density{0.}; - std::atomic nValidEdges{0}; - bool const bComputeStats = this->database() != nullptr && - m_savingInterval.has_value() && - (m_savingInterval.value() == 0 || - this->time_step() % m_savingInterval.value() == 0); - - // Struct to collect street data for batch insert after parallel section - struct StreetDataRecord { - Id streetId; - std::optional coilName; - double density; - std::optional avgSpeed; - std::optional stdSpeed; - std::optional nObservations; - std::optional counts; - std::size_t queueLength; - }; - tbb::concurrent_vector streetDataRecords; - - spdlog::debug("Init evolve at time {}", this->time_step()); - // move the first agent of each street queue, if possible, putting it in the next node - bool const bUpdateData = m_dataUpdatePeriod.has_value() && - this->time_step() % m_dataUpdatePeriod.value() == 0; - auto const numNodes{this->graph().nNodes()}; - auto const numEdges{this->graph().nEdges()}; - - const auto grainSize = std::max(1, numNodes / n_threads); - this->m_taskArena.execute([&] { - tbb::parallel_for( - tbb::blocked_range(0, numNodes, grainSize), - [&](const tbb::blocked_range& range) { - for (std::size_t i = range.begin(); i != range.end(); ++i) { - auto const& pNode = this->graph().node(m_nodeIndices[i]); - for (auto const& inEdgeId : pNode->ingoingEdges()) { - auto const& pStreet{this->graph().edge(inEdgeId)}; - if (bUpdateData && pNode->isTrafficLight()) { - if (!m_queuesAtTrafficLights.contains(inEdgeId)) { - auto& tl = dynamic_cast(*pNode); - assert(!tl.cycles().empty()); - for (auto const& [id, pair] : tl.cycles()) { - for (auto const& [direction, cycle] : pair) { - m_queuesAtTrafficLights[id].emplace(direction, 0.); - } - } - } - for (auto& [direction, value] : m_queuesAtTrafficLights.at(inEdgeId)) { - value += pStreet->nExitingAgents(direction, true); - } - } - m_evolveStreet(pStreet, reinsert_agents); - if (bComputeStats) { - auto const& density{pStreet->density() * 1e3}; - - auto const speedMeasure = pStreet->meanSpeed(true); - if (speedMeasure.is_valid) { - auto const speed = speedMeasure.mean * 3.6; // to kph - auto const speed_std = speedMeasure.std * 3.6; - if (m_bSaveAverageStats) { - mean_speed.fetch_add(speed, std::memory_order_relaxed); - std_speed.fetch_add(speed * speed + speed_std * speed_std, - std::memory_order_relaxed); - - ++nValidEdges; - } - } - if (m_bSaveAverageStats) { - mean_density.fetch_add(density, std::memory_order_relaxed); - std_density.fetch_add(density * density, std::memory_order_relaxed); - } - - if (m_bSaveStreetData) { - // Collect data for batch insert after parallel section - StreetDataRecord record; - record.streetId = pStreet->id(); - record.density = density; - if (pStreet->hasCoil()) { - record.coilName = pStreet->counterName(); - record.counts = pStreet->counts(); - pStreet->resetCounter(); - } - if (speedMeasure.is_valid) { - record.avgSpeed = speedMeasure.mean * 3.6; // to kph - record.stdSpeed = speedMeasure.std * 3.6; - record.nObservations = speedMeasure.n; - } - record.queueLength = pStreet->nExitingAgents(); - streetDataRecords.push_back(record); - } - } - } - } - }); - }); - spdlog::debug("Pre-nodes"); - // Move transport capacity agents from each node - this->m_taskArena.execute([&] { - tbb::parallel_for(tbb::blocked_range(0, numNodes, grainSize), - [&](const tbb::blocked_range& range) { - for (size_t i = range.begin(); i != range.end(); ++i) { - const auto& pNode = this->graph().node(m_nodeIndices[i]); - m_evolveNode(pNode); - if (pNode->isTrafficLight()) { - auto& tl = dynamic_cast(*pNode); - ++tl; - } - } - }); - }); - this->m_evolveAgents(); - - if (bComputeStats) { - // Batch insert street data collected during parallel section - if (m_bSaveStreetData) { - SQLite::Transaction transaction(*this->database()); - SQLite::Statement insertStmt( - *this->database(), - "INSERT INTO road_data (datetime, time_step, simulation_id, street_id, " - "coil, density_vpk, avg_speed_kph, std_speed_kph, n_observations, counts, " - "queue_length) " - "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"); - - for (auto const& record : streetDataRecords) { - insertStmt.bind(1, this->strDateTime()); - insertStmt.bind(2, static_cast(this->time_step())); - insertStmt.bind(3, static_cast(this->id())); - insertStmt.bind(4, static_cast(record.streetId)); - if (record.coilName.has_value()) { - insertStmt.bind(5, record.coilName.value()); - } else { - insertStmt.bind(5); - } - insertStmt.bind(6, record.density); - if (record.avgSpeed.has_value()) { - insertStmt.bind(7, record.avgSpeed.value()); - insertStmt.bind(8, record.stdSpeed.value()); - } else { - insertStmt.bind(7); - insertStmt.bind(8); - } - insertStmt.bind(9, static_cast(record.nObservations.value_or(0))); - if (record.counts.has_value()) { - insertStmt.bind(10, static_cast(record.counts.value())); - } else { - insertStmt.bind(10); - } - insertStmt.bind(11, static_cast(record.queueLength)); - insertStmt.exec(); - insertStmt.reset(); - } - transaction.commit(); - } - - if (m_bSaveTravelData) { // Begin transaction for better performance - SQLite::Transaction transaction(*this->database()); - SQLite::Statement insertStmt(*this->database(), - "INSERT INTO travel_data (datetime, time_step, " - "simulation_id, distance_m, travel_time_s) " - "VALUES (?, ?, ?, ?, ?)"); - - for (auto const& [distance, time] : m_travelDTs) { - insertStmt.bind(1, this->strDateTime()); - insertStmt.bind(2, static_cast(this->time_step())); - insertStmt.bind(3, static_cast(this->id())); - insertStmt.bind(4, distance); - insertStmt.bind(5, time); - insertStmt.exec(); - insertStmt.reset(); - } - transaction.commit(); - m_travelDTs.clear(); - } - - if (m_bSaveAverageStats) { // Average Stats Table - mean_speed.store(mean_speed.load() / nValidEdges.load()); - mean_density.store(mean_density.load() / numEdges); - { - double std_speed_val = std_speed.load(); - double mean_speed_val = mean_speed.load(); - std_speed.store(std::sqrt(std_speed_val / nValidEdges.load() - - mean_speed_val * mean_speed_val)); - } - { - double std_density_val = std_density.load(); - double mean_density_val = mean_density.load(); - std_density.store(std::sqrt(std_density_val / numEdges - - mean_density_val * mean_density_val)); - } - SQLite::Statement insertStmt( - *this->database(), - "INSERT INTO avg_stats (" - "simulation_id, datetime, time_step, n_ghost_agents, n_agents, " - "mean_speed_kph, std_speed_kph, mean_density_vpk, std_density_vpk) " - "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?)"); - insertStmt.bind(1, static_cast(this->id())); - insertStmt.bind(2, this->strDateTime()); - insertStmt.bind(3, static_cast(this->time_step())); - insertStmt.bind(4, static_cast(m_agents.size())); - insertStmt.bind(5, static_cast(this->nAgents())); - if (nValidEdges.load() > 0) { - insertStmt.bind(6, mean_speed); - insertStmt.bind(7, std_speed); - } else { - insertStmt.bind(6); - insertStmt.bind(7); - } - insertStmt.bind(8, mean_density); - insertStmt.bind(9, std_density); - insertStmt.exec(); - } - // Special case: if m_savingInterval == 0, it was a triggered saveData() call, so we need to reset all flags - if (m_savingInterval.value() == 0) { - m_savingInterval.reset(); - m_bSaveStreetData = false; - m_bSaveTravelData = false; - m_bSaveAverageStats = false; - } - } - - Dynamics::m_evolve(); - } - - template - requires(is_numeric_v) - void RoadDynamics::m_trafficlightSingleTailOptimizer( - double const& beta, std::optional& logStream) { - assert(beta >= 0. && beta <= 1.); - if (logStream.has_value()) { - *logStream << std::format( - "Init Traffic Lights optimization (SINGLE TAIL) - Time {} - Alpha {}\n", - this->time_step(), - beta); - } - for (auto const& [nodeId, pNode] : this->graph().nodes()) { - if (!pNode->isTrafficLight()) { - continue; - } - auto& tl = dynamic_cast(*pNode); - - auto const& inNeighbours{pNode->ingoingEdges()}; - - // Default is RIGHTANDSTRAIGHT - LEFT phases for both priority and non-priority - std::array inputPrioritySum{0., 0.}, inputNonPrioritySum{0., 0.}; - bool isPrioritySinglePhase{false}, isNonPrioritySinglePhase{false}; - - for (const auto& streetId : inNeighbours) { - if (tl.cycles().at(streetId).contains(Direction::ANY)) { - tl.streetPriorities().contains(streetId) ? isPrioritySinglePhase = true - : isNonPrioritySinglePhase = true; - } - } - if (isPrioritySinglePhase && logStream.has_value()) { - *logStream << "\tFound a single phase for priority streets.\n"; - } - if (isNonPrioritySinglePhase && logStream.has_value()) { - *logStream << "\tFound a single phase for non-priority streets.\n"; - } - - for (const auto& streetId : inNeighbours) { - for (auto const& [direction, tail] : m_queuesAtTrafficLights.at(streetId)) { - if (tl.streetPriorities().contains(streetId)) { - if (isPrioritySinglePhase) { - inputPrioritySum[0] += tail; - } else { - if (direction == Direction::LEFT || - direction == Direction::LEFTANDSTRAIGHT) { - inputPrioritySum[1] += tail; - } else { - inputPrioritySum[0] += tail; - } - } - } else { - if (isNonPrioritySinglePhase) { - inputNonPrioritySum[0] += tail; - } else { - if (direction == Direction::LEFT || - direction == Direction::LEFTANDSTRAIGHT) { - inputNonPrioritySum[1] += tail; - } else { - inputNonPrioritySum[0] += tail; - } - } - } - } - } - { - // Sum normalization - auto const sum{inputPrioritySum[0] + inputPrioritySum[1] + - inputNonPrioritySum[0] + inputNonPrioritySum[1]}; - if (sum == 0.) { - continue; - } - inputPrioritySum[0] /= sum; - inputPrioritySum[1] /= sum; - inputNonPrioritySum[0] /= sum; - inputNonPrioritySum[1] /= sum; - - // int const cycleTime{(1. - alpha) * tl.cycleTime()}; - - inputPrioritySum[0] *= beta; - inputPrioritySum[1] *= beta; - inputNonPrioritySum[0] *= beta; - inputNonPrioritySum[1] *= beta; - } - - if (logStream.has_value()) { - *logStream << std::format( - "\tInput cycle queue ratios are {:.2f} {:.2f} - {:.2f} {:.2f}\n", - inputPrioritySum[0], - inputPrioritySum[1], - inputNonPrioritySum[0], - inputNonPrioritySum[1]); - } - - tl.resetCycles(); - auto cycles{tl.cycles()}; - std::array n{0, 0, 0, 0}; - std::array greenTimes{0., 0., 0., 0.}; - - for (auto const& [streetId, pair] : cycles) { - for (auto const& [direction, cycle] : pair) { - if (tl.streetPriorities().contains(streetId)) { - if (isPrioritySinglePhase) { - greenTimes[0] += cycle.greenTime(); - ++n[0]; - } else { - if (direction == Direction::LEFT || - direction == Direction::LEFTANDSTRAIGHT) { - greenTimes[1] += cycle.greenTime(); - ++n[1]; - } else { - greenTimes[0] += cycle.greenTime(); - ++n[0]; - } - } - } else { - if (isNonPrioritySinglePhase) { - greenTimes[2] += cycle.greenTime(); - ++n[2]; - } else { - if (direction == Direction::LEFT || - direction == Direction::LEFTANDSTRAIGHT) { - greenTimes[3] += cycle.greenTime(); - ++n[3]; - } else { - greenTimes[2] += cycle.greenTime(); - ++n[2]; - } - } - } - } - } - - if (logStream.has_value()) { - *logStream << std::format("\tGreen times are {} {} - {} {}\n", - greenTimes[0], - greenTimes[1], - greenTimes[2], - greenTimes[3]); - } - - for (auto i{0}; i < 4; ++i) { - if (n[i] > 1) { - greenTimes[i] /= n[i]; - } - } - - { - auto sum{0.}; - for (auto const& greenTime : greenTimes) { - sum += greenTime; - } - if (sum == 0.) { - continue; - } - for (auto& greenTime : greenTimes) { - greenTime /= sum; - } - } - for (auto& el : greenTimes) { - el *= (1. - beta); - } - - int inputPriorityR{static_cast( - std::floor((inputPrioritySum[0] + greenTimes[0]) * tl.cycleTime()))}; - int inputPriorityS{inputPriorityR}; - int inputPriorityL{static_cast( - std::floor((inputPrioritySum[1] + greenTimes[1]) * tl.cycleTime()))}; - - int inputNonPriorityR{static_cast( - std::floor((inputNonPrioritySum[0] + greenTimes[2]) * tl.cycleTime()))}; - int inputNonPriorityS{inputNonPriorityR}; - int inputNonPriorityL{static_cast( - std::floor((inputNonPrioritySum[1] + greenTimes[3]) * tl.cycleTime()))}; - - { - // Adjust phases to have the sum equal to the cycle time - // To do this, first add seconds to the priority streets, then to the - // non-priority streets - auto total{static_cast(inputPriorityR + inputPriorityL + - inputNonPriorityR + inputNonPriorityL)}; - size_t idx{0}; - while (total < tl.cycleTime()) { - switch (idx % 4) { - case 0: - ++inputPriorityR; - ++inputPriorityS; - break; - case 1: - ++inputPriorityL; - break; - case 2: - ++inputNonPriorityR; - ++inputNonPriorityS; - break; - case 3: - ++inputNonPriorityL; - break; - } - ++idx; - ++total; - } - } - - if (isPrioritySinglePhase) { - inputPriorityR = 0; - inputPriorityL = 0; - } - if (isNonPrioritySinglePhase) { - inputNonPriorityR = 0; - inputNonPriorityL = 0; - } - - // Logger::info(std::format( - // "Cycle time: {} - Current sum: {}", - // tl.cycleTime(), - // inputPriorityRS + inputPriorityL + inputNonPriorityRS + inputNonPriorityL)); - assert(inputPriorityS + inputPriorityL + inputNonPriorityS + inputNonPriorityL == - tl.cycleTime()); - - std::unordered_map priorityCycles; - priorityCycles.emplace(Direction::RIGHT, - TrafficLightCycle{static_cast(inputPriorityR), 0}); - priorityCycles.emplace(Direction::STRAIGHT, - TrafficLightCycle{static_cast(inputPriorityS), 0}); - priorityCycles.emplace(Direction::RIGHTANDSTRAIGHT, - TrafficLightCycle{static_cast(inputPriorityS), 0}); - priorityCycles.emplace( - Direction::ANY, - TrafficLightCycle{static_cast(inputPriorityS + inputPriorityL), 0}); - priorityCycles.emplace(Direction::LEFT, - TrafficLightCycle{static_cast(inputPriorityL), - static_cast(inputPriorityS)}); - - std::unordered_map nonPriorityCycles; - nonPriorityCycles.emplace( - Direction::RIGHT, - TrafficLightCycle{static_cast(inputNonPriorityR), - static_cast(inputPriorityS + inputPriorityL)}); - nonPriorityCycles.emplace( - Direction::STRAIGHT, - TrafficLightCycle{static_cast(inputNonPriorityS), - static_cast(inputPriorityS + inputPriorityL)}); - nonPriorityCycles.emplace( - Direction::RIGHTANDSTRAIGHT, - TrafficLightCycle{static_cast(inputNonPriorityS), - static_cast(inputPriorityS + inputPriorityL)}); - nonPriorityCycles.emplace( - Direction::ANY, - TrafficLightCycle{static_cast(inputNonPriorityS + inputNonPriorityL), - static_cast(inputPriorityS + inputPriorityL)}); - nonPriorityCycles.emplace( - Direction::LEFT, - TrafficLightCycle{ - static_cast(inputNonPriorityL), - static_cast(inputPriorityS + inputPriorityL + inputNonPriorityS)}); - nonPriorityCycles.emplace( - Direction::LEFTANDSTRAIGHT, - TrafficLightCycle{ - static_cast(inputNonPriorityL + inputNonPriorityS), - static_cast(inputPriorityS + inputPriorityL + inputNonPriorityR)}); - - std::vector streetIds; - std::set forbiddenLeft; - - for (auto const& pair : cycles) { - streetIds.push_back(pair.first); - } - for (auto const streetId : streetIds) { - auto const& pStreet{this->graph().edge(streetId)}; - if (tl.streetPriorities().contains(streetId)) { - for (auto& [dir, cycle] : cycles.at(streetId)) { - if (isPrioritySinglePhase) { - cycle = priorityCycles.at(Direction::STRAIGHT); - } else { - cycle = priorityCycles.at(dir); - } - } - if (cycles.at(streetId).contains(Direction::RIGHT) && - cycles.at(streetId).contains(Direction::STRAIGHT)) { - TrafficLightCycle freecycle{ - static_cast(inputPriorityS + inputPriorityL), 0}; - // Logger::info(std::format("Free cycle (RIGHT) for {} -> {}: {} {}", - // pStreet->source(), - // pStreet->target(), - // freecycle.greenTime(), - // freecycle.phase())); - cycles.at(streetId).at(Direction::RIGHT) = freecycle; - } - } else { - for (auto& [dir, cycle] : cycles.at(streetId)) { - if (isNonPrioritySinglePhase) { - cycle = nonPriorityCycles.at(Direction::STRAIGHT); - } else { - cycle = nonPriorityCycles.at(dir); - } - } - if (cycles.at(streetId).contains(Direction::RIGHT) && - cycles.at(streetId).contains(Direction::STRAIGHT)) { - TrafficLightCycle freecycle{ - static_cast(inputNonPriorityS + inputNonPriorityL), - static_cast(inputPriorityS + inputPriorityL)}; - // Logger::info(std::format("Free cycle (RIGHT) for {} -> {}: {} {}", - // pStreet->source(), - // pStreet->target(), - // freecycle.greenTime(), - // freecycle.phase())); - cycles.at(streetId).at(Direction::RIGHT) = freecycle; - } - } - bool found{false}; - for (auto const dir : pStreet->laneMapping()) { - if (dir == Direction::LEFT || dir == Direction::LEFTANDSTRAIGHT || - dir == Direction::ANY) { - found = true; - break; - } - } - if (!found) { - forbiddenLeft.insert(streetId); - // Logger::info(std::format("Street {} -> {} has forbidden left turn.", - // pStreet->source(), - // pStreet->target())); - } - } - for (auto const forbiddenLeftStreetId : forbiddenLeft) { - for (auto const streetId : streetIds) { - if (streetId == forbiddenLeftStreetId) { - continue; - } - if (tl.streetPriorities().contains(streetId) && - tl.streetPriorities().contains(forbiddenLeftStreetId)) { - TrafficLightCycle freecycle{ - static_cast(inputPriorityS + inputPriorityL), 0}; - for (auto& [direction, cycle] : cycles.at(streetId)) { - if (direction == Direction::RIGHT || direction == Direction::STRAIGHT || - direction == Direction::RIGHTANDSTRAIGHT) { - auto const& pStreet{this->graph().edge(streetId)}; - if (logStream.has_value()) { - *logStream << std::format("\tFree cycle for {} -> {}: dir {} - {}\n", - pStreet->source(), - pStreet->target(), - directionToString[direction], - freecycle); - } - cycle = freecycle; - } - } - } else if (!tl.streetPriorities().contains(streetId) && - !tl.streetPriorities().contains(forbiddenLeftStreetId)) { - TrafficLightCycle freecycle{ - static_cast(inputNonPriorityS + inputNonPriorityL), - static_cast(inputPriorityS + inputPriorityL)}; - for (auto& [direction, cycle] : cycles.at(streetId)) { - if (direction == Direction::RIGHT || direction == Direction::STRAIGHT || - direction == Direction::RIGHTANDSTRAIGHT) { - auto const& pStreet{this->graph().edge(streetId)}; - if (logStream.has_value()) { - *logStream << std::format("Free cycle ({}) for {} -> {}: {} {}\n", - directionToString[direction], - pStreet->source(), - pStreet->target(), - freecycle.greenTime(), - freecycle.phase()); - } - cycle = freecycle; - } - } - } - } - } - - tl.setCycles(cycles); - if (logStream.has_value()) { - *logStream << std::format("\nNew cycles for {}", tl); - } - } - if (logStream.has_value()) { - *logStream << std::format("End Traffic Lights optimization - Time {}\n", - this->time_step()); - } - } - - template - requires(is_numeric_v) - void RoadDynamics::optimizeTrafficLights( - TrafficLightOptimization const optimizationType, - const std::string& logFile, - double const percentage, - double const threshold) { - std::optional logStream; - if (!logFile.empty()) { - logStream.emplace(logFile, std::ios::app); - if (!logStream->is_open()) { - spdlog::error("Could not open log file: {}", logFile); - } - } - this->m_trafficlightSingleTailOptimizer(percentage, logStream); - if (optimizationType == TrafficLightOptimization::DOUBLE_TAIL) { - // Try to synchronize congested traffic lights - std::unordered_map densities; - for (auto const& [nodeId, pNode] : this->graph().nodes()) { - if (!pNode->isTrafficLight()) { - continue; - } - double density{0.}, n{0.}; - auto const& inNeighbours{pNode->ingoingEdges()}; - for (auto const& inEdgeId : inNeighbours) { - auto const& pStreet{this->graph().edge(inEdgeId)}; - auto const& pSourceNode{this->graph().node(pStreet->source())}; - if (!pSourceNode->isTrafficLight()) { - continue; - } - density += pStreet->density(true); - ++n; - } - density /= n; - densities[nodeId] = density; - } - // Sort densities map from big to small values - std::vector> sortedDensities(densities.begin(), - densities.end()); - - // Sort by density descending - std::sort(sortedDensities.begin(), - sortedDensities.end(), - [](auto const& a, auto const& b) { return a.second > b.second; }); - std::unordered_set optimizedNodes; - - for (auto const& [nodeId, density] : sortedDensities) { - auto const& inNeighbours{this->graph().node(nodeId)->ingoingEdges()}; - for (auto const& inEdgeId : inNeighbours) { - auto const& pStreet{this->graph().edge(inEdgeId)}; - auto const& sourceId{pStreet->source()}; - if (!densities.contains(sourceId) || optimizedNodes.contains(sourceId)) { - continue; - } - auto const& neighbourDensity{densities.at(sourceId)}; - if (neighbourDensity < threshold * density) { - continue; - } - // Try to green-wave the situation - auto& tl{dynamic_cast(*this->graph().node(sourceId))}; - tl.increasePhases(pStreet->length() / - (pStreet->maxSpeed() * (1. - 0.6 * pStreet->density(true)))); - optimizedNodes.insert(sourceId); - if (logStream.has_value()) { - *logStream << std::format("\nNew cycles for {}", tl); - } - } - } - } - // Cleaning variables - for (auto& [streetId, pair] : m_queuesAtTrafficLights) { - for (auto& [direction, value] : pair) { - value = 0.; - } - } - m_previousOptimizationTime = this->time_step(); - if (logStream.has_value()) { - logStream->close(); - } - } - - template - requires(is_numeric_v) - Measurement RoadDynamics::meanTravelTime(bool clearData) { - std::vector travelTimes; - if (!m_travelDTs.empty()) { - travelTimes.reserve(m_travelDTs.size()); - for (auto const& [distance, time] : m_travelDTs) { - travelTimes.push_back(time); - } - if (clearData) { - m_travelDTs.clear(); - } - } - return Measurement(travelTimes); - } - template - requires(is_numeric_v) - Measurement RoadDynamics::meanTravelDistance(bool clearData) { - if (m_travelDTs.empty()) { - return Measurement(); - } - std::vector travelDistances; - travelDistances.reserve(m_travelDTs.size()); - for (auto const& [distance, time] : m_travelDTs) { - travelDistances.push_back(distance); - } - if (clearData) { - m_travelDTs.clear(); - } - return Measurement(travelDistances); - } - template - requires(is_numeric_v) - Measurement RoadDynamics::meanTravelSpeed(bool clearData) { - std::vector travelSpeeds; - travelSpeeds.reserve(m_travelDTs.size()); - for (auto const& [distance, time] : m_travelDTs) { - travelSpeeds.push_back(distance / time); - } - if (clearData) { - m_travelDTs.clear(); - } - return Measurement(travelSpeeds); - } - template - requires(is_numeric_v) - std::unordered_map> const - RoadDynamics::normalizedTurnCounts() const noexcept { - std::unordered_map> normalizedTurnCounts; - for (auto const& [fromId, map] : m_turnCounts) { - auto const sum{ - std::accumulate(map.begin(), map.end(), 0., [](auto const sum, auto const& p) { - return sum + static_cast(p.second); - })}; - for (auto const& [toId, count] : map) { - normalizedTurnCounts[fromId][toId] = - sum == 0. ? 0. : static_cast(count) / sum; - } - } - return normalizedTurnCounts; - } - - template - requires(is_numeric_v) - tbb::concurrent_unordered_map RoadDynamics::originCounts( - bool const bReset) noexcept { - if (!bReset) { - return m_originCounts; - } - auto const tempCounts{std::move(m_originCounts)}; - m_originCounts.clear(); - return tempCounts; - } - template - requires(is_numeric_v) - tbb::concurrent_unordered_map RoadDynamics::destinationCounts( - bool const bReset) noexcept { - if (!bReset) { - return m_destinationCounts; - } - auto const tempCounts{std::move(m_destinationCounts)}; - m_destinationCounts.clear(); - return tempCounts; - } - - template - requires(is_numeric_v) - Measurement RoadDynamics::streetMeanDensity(bool normalized) const { - if (this->graph().edges().empty()) { - return Measurement(); - } - std::vector densities; - densities.reserve(this->graph().nEdges()); - if (normalized) { - for (const auto& [streetId, pStreet] : this->graph().edges()) { - densities.push_back(pStreet->density(true)); - } - } else { - double sum{0.}; - for (const auto& [streetId, pStreet] : this->graph().edges()) { - densities.push_back(pStreet->density(false) * pStreet->length()); - sum += pStreet->length(); - } - if (sum == 0) { - return Measurement(); - } - auto meanDensity{std::accumulate(densities.begin(), densities.end(), 0.) / sum}; - return Measurement(meanDensity, 0., densities.size()); - } - return Measurement(densities); - } - - template - requires(is_numeric_v) - Measurement RoadDynamics::streetMeanFlow() const { - std::vector flows; - flows.reserve(this->graph().nEdges()); - for (const auto& [streetId, pStreet] : this->graph().edges()) { - auto const speedMeasure = pStreet->meanSpeed(); - if (speedMeasure.is_valid) { - flows.push_back(pStreet->density() * speedMeasure.mean); - } - } - return Measurement(flows); - } - - template - requires(is_numeric_v) - Measurement RoadDynamics::streetMeanFlow(double threshold, - bool above) const { - std::vector flows; - flows.reserve(this->graph().nEdges()); - for (const auto& [streetId, pStreet] : this->graph().edges()) { - auto const speedMeasure = pStreet->meanSpeed(); - if (!speedMeasure.is_valid) { - continue; - } - if (above && (pStreet->density(true) > threshold)) { - flows.push_back(pStreet->density() * speedMeasure.mean); - } else if (!above && (pStreet->density(true) < threshold)) { - flows.push_back(pStreet->density() * speedMeasure.mean); - } - } - return Measurement(flows); - } - - template - requires(is_numeric_v) - void RoadDynamics::summary(std::ostream& os) const { - os << "RoadDynamics Summary:\n"; - this->graph().describe(os); - os << "\nNumber of added agents: " << m_nAddedAgents << '\n' - << "Number of inserted agents: " << m_nInsertedAgents << '\n' - << "Number of arrived agents: " << m_nArrivedAgents << '\n' - << "Number of killed agents: " << m_nKilledAgents << '\n' - << "Current number of agents: " << this->nAgents() << '\n'; - } -} // namespace dsf::mobility diff --git a/src/dsf/utility/Typedef.hpp b/src/dsf/utility/Typedef.hpp index 589af78b..7d7c908f 100644 --- a/src/dsf/utility/Typedef.hpp +++ b/src/dsf/utility/Typedef.hpp @@ -20,6 +20,7 @@ namespace dsf { using Delay = uint16_t; enum class PathWeight : uint8_t { LENGTH = 0, TRAVELTIME = 1, WEIGHT = 2 }; + enum class SpeedFunction : uint8_t { CUSTOM = 0, LINEAR = 1 }; enum Direction : uint8_t { RIGHT = 0, // delta < 0 STRAIGHT = 1, // delta == 0 diff --git a/test/mobility/Test_dynamics.cpp b/test/mobility/Test_dynamics.cpp index 74e9f938..189f9e37 100644 --- a/test/mobility/Test_dynamics.cpp +++ b/test/mobility/Test_dynamics.cpp @@ -68,8 +68,8 @@ TEST_CASE("FirstOrderDynamics") { SUBCASE("Constructor") { GIVEN("A graph object") { WHEN("A dynamics object is created") { - FirstOrderDynamics dynamics{ - defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); THEN("The node and the street sets are the same") { CHECK_EQ(dynamics.graph().nNodes(), 120); CHECK_EQ(dynamics.graph().nEdges(), 436); @@ -111,7 +111,7 @@ TEST_CASE("FirstOrderDynamics") { dynamics.summary(oss); std::string summaryStr = oss.str(); THEN("The summary contains expected information") { - CHECK(summaryStr.find("RoadDynamics Summary") != std::string::npos); + CHECK(summaryStr.find("FirstOrderDynamics Summary") != std::string::npos); CHECK(summaryStr.find("RoadNetwork with 120 nodes and 436 edges") != std::string::npos); CHECK(summaryStr.find("Number of inserted agents") != std::string::npos); @@ -147,7 +147,8 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("setDestinationNodes") { GIVEN("A dynamics object and a destination node") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); WHEN("We add a span of destination nodes") { std::array nodes{0, 1, 2}; dynamics.setDestinationNodes(nodes); @@ -163,7 +164,8 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("addAgent") { GIVEN("A dynamics object, a source node and a destination node") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); dynamics.addItinerary(2, 2); WHEN("We add the agent") { dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -179,7 +181,9 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("addAgentsUniformly") { GIVEN("A dynamics object and an itinerary") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); WHEN("We add a random agent") { dynamics.addItinerary(2, 2); dynamics.addAgentsUniformly(1); @@ -191,7 +195,9 @@ TEST_CASE("FirstOrderDynamics") { } } GIVEN("A dynamics object and many itineraries") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.addItinerary(1, 1); WHEN("We add many agents") { @@ -229,7 +235,8 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("addAgentsRandomly") { GIVEN("A graph object") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); WHEN("We add one agent for existing itinerary") { std::unordered_map src{{0, 1.}}; std::unordered_map dst{{2, 1.}}; @@ -265,7 +272,8 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("addAgentsODs") { GIVEN("A graph object") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); WHEN("We add agents with a single OD pair") { std::vector> ods{{0, 2, 1.}}; dynamics.setODs(ods); @@ -313,7 +321,9 @@ TEST_CASE("FirstOrderDynamics") { auto const p{0.1}; auto const n{100}; // graph.adjustNodeCapacities(); - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); #ifdef __APPLE__ { std::time_t const t{0}; @@ -365,7 +375,8 @@ TEST_CASE("FirstOrderDynamics") { } SUBCASE("addAgents") { GIVEN("A dynamics object and one itinerary") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); dynamics.addItinerary(2, 2); WHEN("We add an agent with itinerary 2") { dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -389,7 +400,8 @@ TEST_CASE("FirstOrderDynamics") { Street s3{2, std::make_pair(0, 2), 10.}; RoadNetwork graph2; graph2.addStreets(s1, s2, s3); - FirstOrderDynamics dynamics{graph2, false, 69, 0.0, dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); WHEN("We add an itinerary and update the paths") { dynamics.addItinerary(std::make_shared(0, 2)); dynamics.updatePaths(); @@ -416,7 +428,9 @@ TEST_CASE("FirstOrderDynamics") { graph2.addEdge(4, std::make_pair(4, 1), 10.); graph2.addEdge(5, std::make_pair(4, 5), 10.); graph2.addEdge(6, std::make_pair(5, 4), 10.); - FirstOrderDynamics dynamics{graph2, false, 69, 0.}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); + dynamics.setWeightFunction(dsf::PathWeight::TRAVELTIME); WHEN("We add an iitinerary to node 3 and update paths") { dynamics.addItinerary(3, 3); dynamics.updatePaths(); @@ -441,7 +455,8 @@ TEST_CASE("FirstOrderDynamics") { GIVEN( "A dynamics objects, many streets and many itinearies with same " "destination") { - FirstOrderDynamics dynamics{defaultNetwork, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{defaultNetwork, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); dynamics.addItinerary(0, 118); dynamics.addItinerary(1, 118); dynamics.addItinerary(2, 118); @@ -471,7 +486,8 @@ TEST_CASE("FirstOrderDynamics") { Street s4{3, std::make_pair(3, 2), 5.}; RoadNetwork graph; graph.addStreets(s1, s2, s3, s4); - FirstOrderDynamics dynamics{graph, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); dynamics.addItinerary(0, 2); WHEN("We update the paths") { dynamics.updatePaths(); @@ -499,7 +515,9 @@ TEST_CASE("FirstOrderDynamics") { Street s4{3, std::make_pair(3, 2), 10., 100.}; RoadNetwork graph; graph.addStreets(s1, s2, s3, s4); - FirstOrderDynamics dynamics{graph, false, 69, 0., dsf::PathWeight::TRAVELTIME, 1.}; + FirstOrderDynamics dynamics{graph, false, 69}; + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); + dynamics.setWeightFunction(dsf::PathWeight::TRAVELTIME, 1.); dynamics.addItinerary(0, 2); WHEN("We update the paths") { dynamics.updatePaths(); @@ -518,7 +536,8 @@ TEST_CASE("FirstOrderDynamics") { Street s1{0, std::make_pair(0, 1), 10.}; RoadNetwork graph; graph.addStreets(s1); - FirstOrderDynamics dynamics{graph, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); WHEN( "We add an impossible itinerary (to source node) and update paths with " @@ -543,7 +562,9 @@ TEST_CASE("FirstOrderDynamics") { Street s3{2, std::make_pair(0, 2), 10.}; RoadNetwork graph; graph.addStreets(s1, s2, s3); - FirstOrderDynamics dynamics{graph, false, 69, 0.0, dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.updatePaths(); WHEN("We add an agent randomly and evolve the dynamics") { @@ -578,7 +599,9 @@ TEST_CASE("FirstOrderDynamics") { Street s2{1, std::make_pair(1, 0), 13.8888888889}; RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(1, 1); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(1), 0); @@ -604,7 +627,9 @@ TEST_CASE("FirstOrderDynamics") { s1.setTransportCapacity(0.3); RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(std::make_shared(0, 1)); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(0), 0); @@ -635,7 +660,9 @@ TEST_CASE("FirstOrderDynamics") { Street s2{1, std::make_pair(1, 0), 13.8888888889}; RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(1, 1); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(1), 0); @@ -667,7 +694,9 @@ TEST_CASE("FirstOrderDynamics") { RoadNetwork graph2; graph2.addStreets(s0_1, s1_0, s1_2, s2_1); graph2.makeRoundabout(2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.setDestinationNodes({1, 2}); dynamics.updatePaths(); std::vector> trip{dynamics.itineraries().at(2), @@ -703,7 +732,9 @@ TEST_CASE("FirstOrderDynamics") { Street s0{0, std::make_pair(0, 1), 13.8888888889}; RoadNetwork graph2; graph2.addStreets(s0); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); // Manually construct an itinerary whose path leads through node 1, but // node 1 has no outgoing edges. m_nextStreetId will return nullopt and // the agent must be killed instead of throwing an exception. @@ -744,7 +775,9 @@ TEST_CASE("FirstOrderDynamics") { auto& tl = graph2.node(1); tl.setCycle(1, dsf::Direction::ANY, {2, 0}); tl.setCycle(16, dsf::Direction::ANY, {2, 2}); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -804,7 +837,9 @@ TEST_CASE("FirstOrderDynamics") { graph2.addStreets(s0_1, s1_0, s1_2, s2_1, s3_1, s1_3, s4_1, s1_4); graph2.adjustNodeCapacities(); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.setDestinationNodes({0, 2, 3, 4}); dynamics.updatePaths(); @@ -868,7 +903,9 @@ TEST_CASE("FirstOrderDynamics") { graph2.adjustNodeCapacities(); graph2.autoMapStreetLanes(); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.setDestinationNodes({0, 2, 3, 4}); dynamics.updatePaths(); @@ -922,7 +959,9 @@ TEST_CASE("FirstOrderDynamics") { tl.setCycle(11, dsf::Direction::ANY, {4, 0}); tl.setComplementaryCycle(16, 11); tl.setComplementaryCycle(21, 11); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.setDestinationNodes({0, 2, 3, 4}); dynamics.updatePaths(); WHEN("We evolve the dynamics and optimize traffic lights") { @@ -977,7 +1016,9 @@ TEST_CASE("FirstOrderDynamics") { graph2.addStreets(s1, s2, s3, s4); auto& rb = graph2.makeRoundabout(1); graph2.adjustNodeCapacities(); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(0, 0); dynamics.addItinerary(2, 2); dynamics.updatePaths(); @@ -1021,7 +1062,9 @@ TEST_CASE("FirstOrderDynamics") { Street s2{5, std::make_pair(1, 2), 1.}; RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -1047,7 +1090,9 @@ TEST_CASE("FirstOrderDynamics") { s1.setTransportCapacity(0.3); RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -1083,7 +1128,9 @@ TEST_CASE("FirstOrderDynamics") { graph2.addEdge(10, std::make_pair(2, 0), 10., 10.); graph2.addEdge(15, std::make_pair(3, 0), 10., 10.); graph2.addEdge(20, std::make_pair(4, 0), 10., 10.); - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH, 0.}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH, 0.); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.graph().node(0)->setCapacity(3); dynamics.graph().node(0)->setTransportCapacity(1); auto& nodeO{dynamic_cast(*dynamics.graph().node(0))}; @@ -1162,7 +1209,9 @@ TEST_CASE("FirstOrderDynamics") { RoadNetwork graph2; graph2.addStreets(s1, s2); graph2.addCoil(0); // Add coil for testing road_data with coils - FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph2, false, 69}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.addItinerary(2, 2); dynamics.updatePaths(); dynamics.addAgent(dynamics.itineraries().at(2), 0); @@ -1452,7 +1501,9 @@ TEST_CASE("FirstOrderDynamics") { graph.autoMapStreetLanes(); graph.adjustNodeCapacities(); - FirstOrderDynamics dynamics{graph, false, 42, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph, false, 42}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dynamics.setOriginNodes({{0, 1.0}}); WHEN("We add multiple random agents and evolve the system") { @@ -1501,7 +1552,9 @@ TEST_CASE("FirstOrderDynamics") { graph.autoMapStreetLanes(); graph.adjustNodeCapacities(); - FirstOrderDynamics dynamics{graph, false, 123, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dynamics{graph, false, 123}; + dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.); dynamics.setOriginNodes({{0, 1.0}}); WHEN("We add multiple random agents and evolve the system") { @@ -1591,7 +1644,8 @@ TEST_CASE("Stationary Weights Impact on Random Navigation") { network.adjustNodeCapacities(); // Initialize dynamics - FirstOrderDynamics dynamics(network, false, 42, 0.0); + FirstOrderDynamics dynamics(network, false, 42); + dynamics.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); // Add many random agents to Street 0 for (int i = 0; i < numAgents; ++i) { @@ -1647,7 +1701,9 @@ TEST_CASE("RoadDynamics Configuration") { Street s2{1, std::make_pair(1, 2), 13.8888888889}; RoadNetwork graph2; graph2.addStreets(s1, s2); - FirstOrderDynamics dyn{graph2, false, 42, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dyn{graph2, false, 42}; + dyn.setWeightFunction(dsf::PathWeight::LENGTH); + dyn.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dyn.addItinerary(2, 2); dyn.updatePaths(); @@ -1697,7 +1753,9 @@ TEST_CASE("RoadDynamics Configuration") { Street s1_3{2, std::make_pair(1, 3), 13.8888888889}; RoadNetwork graph2; graph2.addStreets(s0_1, s1_2, s1_3); - FirstOrderDynamics dyn{graph2, false, 42, 0., dsf::PathWeight::LENGTH}; + FirstOrderDynamics dyn{graph2, false, 42}; + dyn.setWeightFunction(dsf::PathWeight::LENGTH); + dyn.setSpeedFunction(dsf::SpeedFunction::LINEAR, 0.8); dyn.setDestinationNodes({2, 3}); dyn.updatePaths(); From e37a9f1350423aa2460dfbc9232e9c03e3f0a9b7 Mon Sep 17 00:00:00 2001 From: Grufoony Date: Wed, 11 Mar 2026 17:41:30 +0100 Subject: [PATCH 2/5] Prova --- examples/simulate_city.py | 23 ++- src/dsf/bindings.cpp | 185 ++++++++++++++---------- src/dsf/mobility/FirstOrderDynamics.hpp | 8 +- 3 files changed, 138 insertions(+), 78 deletions(-) diff --git a/examples/simulate_city.py b/examples/simulate_city.py index 0e4b7ef5..1ba98a65 100644 --- a/examples/simulate_city.py +++ b/examples/simulate_city.py @@ -3,7 +3,13 @@ import logging from dsf.cartography import get_cartography -from dsf.mobility import RoadNetwork, Dynamics, AgentInsertionMethod +from dsf.mobility import ( + RoadNetwork, + Dynamics, + AgentInsertionMethod, + PathWeight, + SpeedFunction, +) from tqdm import trange import numpy as np @@ -13,6 +19,17 @@ level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" ) +from numba import cfunc, float64 + + +@cfunc(float64(float64, float64, float64)) +def nico_speed(max_speed, density, length): + if density < 0.35: + return max_speed * (0.9 - 0.1 * density) + else: + return max_speed * (1.2 - 0.7 * density) + + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument( @@ -75,7 +92,9 @@ vehicle_input = np.random.randint(0, 50, size=8640) # Create a dynamics model for the road network - dynamics = Dynamics(road_network, seed=args.seed, alpha=0.8) + dynamics = Dynamics(road_network, seed=args.seed) + dynamics.setWeightFunction(PathWeight.TRAVELTIME) + dynamics.setSpeedFunction(SpeedFunction.CUSTOM, nico_speed.address) # Get epoch time of today at midnight epoch_time = int( datetime.combine(datetime.today(), datetime.min.time()).timestamp() diff --git a/src/dsf/bindings.cpp b/src/dsf/bindings.cpp index 942d4deb..2590ebd4 100644 --- a/src/dsf/bindings.cpp +++ b/src/dsf/bindings.cpp @@ -464,14 +464,31 @@ PYBIND11_MODULE(dsf_cpp, m) { double alpha = arg.cast(); self.setSpeedFunction(dsf::SpeedFunction::LINEAR, alpha); } else if (speedFunction == dsf::SpeedFunction::CUSTOM) { - auto pyFunc = arg.cast>(); - self.setSpeedFunction( - dsf::SpeedFunction::CUSTOM, - [pyFunc]( - std::unique_ptr const& pStreet) -> double { - return pyFunc( - pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); - }); + if (pybind11::isinstance(arg)) { + // Numba cfunc path: raw C function pointer, zero Python overhead + auto ptr_addr = arg.cast(); + auto* func_ptr = + reinterpret_cast(ptr_addr); + self.setSpeedFunction( + dsf::SpeedFunction::CUSTOM, + [func_ptr]( + std::unique_ptr const& pStreet) -> double { + // No GIL needed — this is pure C + return func_ptr( + pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); + }); + } else { + // Fallback: slow Python callable path (keep for flexibility) + auto pyFunc = arg.cast>(); + self.setSpeedFunction( + dsf::SpeedFunction::CUSTOM, + [pyFunc]( + std::unique_ptr const& pStreet) -> double { + pybind11::gil_scoped_acquire gil; + return pyFunc( + pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); + }); + } } }, pybind11::arg("speedFunction"), @@ -508,44 +525,47 @@ PYBIND11_MODULE(dsf_cpp, m) { &dsf::mobility::FirstOrderDynamics::connectDataBase, pybind11::arg("dbPath"), dsf::g_docstrings.at("dsf::Dynamics::connectDataBase").c_str()) - .def( - "setForcePriorities", - &dsf::mobility::FirstOrderDynamics::setForcePriorities, - pybind11::arg("forcePriorities"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setForcePriorities").c_str()) + .def("setForcePriorities", + &dsf::mobility::FirstOrderDynamics::setForcePriorities, + pybind11::arg("forcePriorities"), + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setForcePriorities") + .c_str()) .def( "setDataUpdatePeriod", [](dsf::mobility::FirstOrderDynamics& self, int dataUpdatePeriod) { self.setDataUpdatePeriod(static_cast(dataUpdatePeriod)); }, pybind11::arg("dataUpdatePeriod"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setDataUpdatePeriod").c_str()) - .def("setMeanTravelDistance", - &dsf::mobility::FirstOrderDynamics::setMeanTravelDistance, - pybind11::arg("meanDistance"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setMeanTravelDistance") - .c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setDataUpdatePeriod") + .c_str()) + .def( + "setMeanTravelDistance", + &dsf::mobility::FirstOrderDynamics::setMeanTravelDistance, + pybind11::arg("meanDistance"), + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setMeanTravelDistance") + .c_str()) .def( "setMeanTravelTime", [](dsf::mobility::FirstOrderDynamics& self, uint64_t meanTravelTime) { self.setMeanTravelTime(static_cast(meanTravelTime)); }, pybind11::arg("meanTravelTime"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setMeanTravelTime").c_str()) - .def( - "setErrorProbability", - &dsf::mobility::FirstOrderDynamics::setErrorProbability, - pybind11::arg("errorProbability"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setErrorProbability").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setMeanTravelTime") + .c_str()) + .def("setErrorProbability", + &dsf::mobility::FirstOrderDynamics::setErrorProbability, + pybind11::arg("errorProbability"), + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setErrorProbability") + .c_str()) .def("setWeightFunction", &dsf::mobility::FirstOrderDynamics::setWeightFunction, pybind11::arg("weightFunction"), pybind11::arg("weightThreshold") = std::nullopt) - .def( - "killStagnantAgents", - &dsf::mobility::FirstOrderDynamics::killStagnantAgents, - pybind11::arg("timeToleranceFactor") = 3., - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::killStagnantAgents").c_str()) + .def("killStagnantAgents", + &dsf::mobility::FirstOrderDynamics::killStagnantAgents, + pybind11::arg("timeToleranceFactor") = 3., + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::killStagnantAgents") + .c_str()) .def( "setDestinationNodes", [](dsf::mobility::FirstOrderDynamics& self, @@ -553,7 +573,8 @@ PYBIND11_MODULE(dsf_cpp, m) { self.setDestinationNodes(destinationNodes); }, pybind11::arg("destinationNodes"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setDestinationNodes").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setDestinationNodes") + .c_str()) .def( "setOriginNodes", [](dsf::mobility::FirstOrderDynamics& self, @@ -561,7 +582,8 @@ PYBIND11_MODULE(dsf_cpp, m) { self.setOriginNodes(originNodes); }, pybind11::arg("originNodes") = std::unordered_map(), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setOriginNodes").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setOriginNodes") + .c_str()) .def( "setOriginNodes", [](dsf::mobility::FirstOrderDynamics& self, @@ -576,7 +598,8 @@ PYBIND11_MODULE(dsf_cpp, m) { self.setOriginNodes(nodeWeights); }, pybind11::arg("originNodes"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setOriginNodes").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setOriginNodes") + .c_str()) .def( "setDestinationNodes", [](dsf::mobility::FirstOrderDynamics& self, @@ -588,7 +611,8 @@ PYBIND11_MODULE(dsf_cpp, m) { self.setDestinationNodes(nodes); }, pybind11::arg("destinationNodes"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setDestinationNodes").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setDestinationNodes") + .c_str()) .def( "setDestinationNodes", [](dsf::mobility::FirstOrderDynamics& self, @@ -596,53 +620,65 @@ PYBIND11_MODULE(dsf_cpp, m) { self.setDestinationNodes(destinationNodes); }, pybind11::arg("destinationNodes"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setDestinationNodes").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setDestinationNodes") + .c_str()) .def("setODs", &dsf::mobility::FirstOrderDynamics::setODs, pybind11::arg("ods"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::setODs").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::setODs").c_str()) .def("initTurnCounts", &dsf::mobility::FirstOrderDynamics::initTurnCounts, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::initTurnCounts").c_str()) - .def("updatePaths", - &dsf::mobility::FirstOrderDynamics::updatePaths, - pybind11::arg("throw_on_empty") = true, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::updatePaths").c_str()) - .def( - "addAgentsUniformly", - &dsf::mobility::FirstOrderDynamics::addAgentsUniformly, - pybind11::arg("nAgents"), - pybind11::arg("itineraryId") = std::nullopt, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::addAgentsUniformly").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::initTurnCounts") + .c_str()) + .def( + "updatePaths", + [](dsf::mobility::FirstOrderDynamics& self, bool throw_on_empty) { + pybind11::gil_scoped_release release; + self.updatePaths(throw_on_empty); + }, + pybind11::arg("throw_on_empty") = true, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::updatePaths").c_str()) + .def("addAgentsUniformly", + &dsf::mobility::FirstOrderDynamics::addAgentsUniformly, + pybind11::arg("nAgents"), + pybind11::arg("itineraryId") = std::nullopt, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::addAgentsUniformly") + .c_str()) .def( "addAgents", [](dsf::mobility::FirstOrderDynamics& self, std::size_t nAgents, dsf::mobility::AgentInsertionMethod insertionMethod) { + pybind11::gil_scoped_release release; self.addAgents(nAgents, insertionMethod); }, pybind11::arg("nAgents"), pybind11::arg("insertionMethod"), - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::addAgents").c_str()) - .def("evolve", - &dsf::mobility::FirstOrderDynamics::evolve, - pybind11::arg("reinsert_agents") = false, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::evolve").c_str()) - .def("optimizeTrafficLights", - &dsf::mobility::FirstOrderDynamics::optimizeTrafficLights, - pybind11::arg("optimizationType") = dsf::TrafficLightOptimization::DOUBLE_TAIL, - pybind11::arg("logFile") = "", - pybind11::arg("threshold") = 0., - pybind11::arg("ratio") = 1.3, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::optimizeTrafficLights") - .c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::addAgents").c_str()) + .def( + "evolve", + [](dsf::mobility::FirstOrderDynamics& self, bool reinsert_agents) { + pybind11::gil_scoped_release release; // release before simulation step + self.evolve(reinsert_agents); + }, + pybind11::arg("reinsert_agents") = false, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::evolve").c_str()) + .def( + "optimizeTrafficLights", + &dsf::mobility::FirstOrderDynamics::optimizeTrafficLights, + pybind11::arg("optimizationType") = dsf::TrafficLightOptimization::DOUBLE_TAIL, + pybind11::arg("logFile") = "", + pybind11::arg("threshold") = 0., + pybind11::arg("ratio") = 1.3, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::optimizeTrafficLights") + .c_str()) .def("graph", &dsf::mobility::FirstOrderDynamics::graph, pybind11::return_value_policy::reference_internal, dsf::g_docstrings.at("dsf::Dynamics::graph").c_str()) .def("nAgents", &dsf::mobility::FirstOrderDynamics::nAgents, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::nAgents").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::nAgents").c_str()) .def("time", &dsf::mobility::FirstOrderDynamics::time, dsf::g_docstrings.at("dsf::Dynamics::time").c_str()) @@ -655,16 +691,18 @@ PYBIND11_MODULE(dsf_cpp, m) { .def("meanTravelTime", &dsf::mobility::FirstOrderDynamics::meanTravelTime, pybind11::arg("clearData") = false, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::meanTravelTime").c_str()) - .def( - "meanTravelDistance", - &dsf::mobility::FirstOrderDynamics::meanTravelDistance, - pybind11::arg("clearData") = false, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::meanTravelDistance").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::meanTravelTime") + .c_str()) + .def("meanTravelDistance", + &dsf::mobility::FirstOrderDynamics::meanTravelDistance, + pybind11::arg("clearData") = false, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::meanTravelDistance") + .c_str()) .def("meanTravelSpeed", &dsf::mobility::FirstOrderDynamics::meanTravelSpeed, pybind11::arg("clearData") = false, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::meanTravelSpeed").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::meanTravelSpeed") + .c_str()) .def( "turnCounts", [](const dsf::mobility::FirstOrderDynamics& self) { @@ -679,7 +717,7 @@ PYBIND11_MODULE(dsf_cpp, m) { } return py_result; }, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::turnCounts").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::turnCounts").c_str()) .def( "normalizedTurnCounts", [](const dsf::mobility::FirstOrderDynamics& self) { @@ -694,7 +732,7 @@ PYBIND11_MODULE(dsf_cpp, m) { } return py_result; }, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::normalizedTurnCounts") + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::normalizedTurnCounts") .c_str()) .def( "originCounts", @@ -707,7 +745,7 @@ PYBIND11_MODULE(dsf_cpp, m) { return py_result; }, pybind11::arg("reset") = true, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::originCounts").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::originCounts").c_str()) .def( "destinationCounts", [](dsf::mobility::FirstOrderDynamics& self, bool reset) { @@ -719,7 +757,8 @@ PYBIND11_MODULE(dsf_cpp, m) { return py_result; }, pybind11::arg("reset") = true, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::destinationCounts").c_str()) + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::destinationCounts") + .c_str()) .def("saveData", &dsf::mobility::FirstOrderDynamics::saveData, pybind11::arg("saving_interval"), @@ -739,7 +778,7 @@ PYBIND11_MODULE(dsf_cpp, m) { [](dsf::mobility::FirstOrderDynamics& self) { self.summary(); // Uses default std::cout }, - dsf::g_docstrings.at("dsf::mobility::RoadDynamics::summary").c_str()); + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::summary").c_str()); // Bind TrajectoryCollection class to mdt submodule pybind11::class_(mdt, "TrajectoryCollection") diff --git a/src/dsf/mobility/FirstOrderDynamics.hpp b/src/dsf/mobility/FirstOrderDynamics.hpp index 30b2d5cf..2c2277da 100644 --- a/src/dsf/mobility/FirstOrderDynamics.hpp +++ b/src/dsf/mobility/FirstOrderDynamics.hpp @@ -448,11 +448,13 @@ namespace dsf::mobility { throw std::invalid_argument(std::format( "Linear speed function requires exactly one argument, but {} were provided", sizeof...(args))); - } else if constexpr (!std::is_same_v>, - double>) { + } else if constexpr (!std::is_convertible_v< + std::tuple_element_t<0, std::tuple>, + double>) { throw std::invalid_argument(std::format( "Linear speed function requires a double argument, but {} was provided", - typeid(std::tuple_element_t<0, std::tuple>).name())); + typeid(std::remove_cvref_t>>) + .name())); } else { double alpha = std::get<0>(std::forward_as_tuple(args...)); if (alpha < 0. || alpha > 1.) { From 154dc0c92065810e835a284d97903cee3e852597 Mon Sep 17 00:00:00 2001 From: grufoony Date: Thu, 12 Mar 2026 10:52:42 +0100 Subject: [PATCH 3/5] Final fix --- examples/requirements.txt | 2 +- examples/simulate_city.py | 22 +++++----- src/dsf/bindings.cpp | 55 ++++++++----------------- src/dsf/mobility/FirstOrderDynamics.cpp | 48 +++++++++++---------- src/dsf/mobility/FirstOrderDynamics.hpp | 23 +++++++++++ test/mobility/Test_dynamics.cpp | 3 +- 6 files changed, 76 insertions(+), 77 deletions(-) diff --git a/examples/requirements.txt b/examples/requirements.txt index 362c92bc..1f2d9f6e 100644 --- a/examples/requirements.txt +++ b/examples/requirements.txt @@ -1,4 +1,4 @@ dsf_mobility networkx -numpy +numba tqdm diff --git a/examples/simulate_city.py b/examples/simulate_city.py index 1ba98a65..64d52d5d 100644 --- a/examples/simulate_city.py +++ b/examples/simulate_city.py @@ -12,23 +12,19 @@ ) from tqdm import trange +from numba import cfunc, float64 import numpy as np import networkx as nx -logging.basicConfig( - level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" -) - -from numba import cfunc, float64 - - -@cfunc(float64(float64, float64, float64)) -def nico_speed(max_speed, density, length): +@cfunc(float64(float64, float64), nopython=True, cache=True) +def custom_speed(max_speed, density): if density < 0.35: return max_speed * (0.9 - 0.1 * density) - else: - return max_speed * (1.2 - 0.7 * density) + return max_speed * (1.2 - 0.7 * density) +logging.basicConfig( + level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" +) if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -94,7 +90,9 @@ def nico_speed(max_speed, density, length): # Create a dynamics model for the road network dynamics = Dynamics(road_network, seed=args.seed) dynamics.setWeightFunction(PathWeight.TRAVELTIME) - dynamics.setSpeedFunction(SpeedFunction.CUSTOM, nico_speed.address) + dynamics.setSpeedFunction(SpeedFunction.LINEAR, 0.8) + # To use a custom speed function, you must pass the pointer to the compiled function using the address attribute + # dynamics.setSpeedFunction(SpeedFunction.CUSTOM, custom_speed.address) # Get epoch time of today at midnight epoch_time = int( datetime.combine(datetime.today(), datetime.min.time()).timestamp() diff --git a/src/dsf/bindings.cpp b/src/dsf/bindings.cpp index 2590ebd4..6f0cffaf 100644 --- a/src/dsf/bindings.cpp +++ b/src/dsf/bindings.cpp @@ -460,35 +460,25 @@ PYBIND11_MODULE(dsf_cpp, m) { [](dsf::mobility::FirstOrderDynamics& self, dsf::SpeedFunction speedFunction, pybind11::object arg) { - if (speedFunction == dsf::SpeedFunction::LINEAR) { - double alpha = arg.cast(); - self.setSpeedFunction(dsf::SpeedFunction::LINEAR, alpha); - } else if (speedFunction == dsf::SpeedFunction::CUSTOM) { - if (pybind11::isinstance(arg)) { + switch (speedFunction) { + case dsf::SpeedFunction::LINEAR: + self.setSpeedFunction(dsf::SpeedFunction::LINEAR, arg.cast()); + break; + case dsf::SpeedFunction::CUSTOM: { // Numba cfunc path: raw C function pointer, zero Python overhead - auto ptr_addr = arg.cast(); auto* func_ptr = - reinterpret_cast(ptr_addr); + reinterpret_cast(arg.cast()); self.setSpeedFunction( dsf::SpeedFunction::CUSTOM, [func_ptr]( std::unique_ptr const& pStreet) -> double { // No GIL needed — this is pure C - return func_ptr( - pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); - }); - } else { - // Fallback: slow Python callable path (keep for flexibility) - auto pyFunc = arg.cast>(); - self.setSpeedFunction( - dsf::SpeedFunction::CUSTOM, - [pyFunc]( - std::unique_ptr const& pStreet) -> double { - pybind11::gil_scoped_acquire gil; - return pyFunc( - pStreet->maxSpeed(), pStreet->density(true), pStreet->length()); + return func_ptr(pStreet->maxSpeed(), pStreet->density(true)); }); + break; } + default: + throw std::invalid_argument("Invalid speed function type"); } }, pybind11::arg("speedFunction"), @@ -630,14 +620,10 @@ PYBIND11_MODULE(dsf_cpp, m) { &dsf::mobility::FirstOrderDynamics::initTurnCounts, dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::initTurnCounts") .c_str()) - .def( - "updatePaths", - [](dsf::mobility::FirstOrderDynamics& self, bool throw_on_empty) { - pybind11::gil_scoped_release release; - self.updatePaths(throw_on_empty); - }, - pybind11::arg("throw_on_empty") = true, - dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::updatePaths").c_str()) + .def("updatePaths", + &dsf::mobility::FirstOrderDynamics::updatePaths, + pybind11::arg("throw_on_empty") = true, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::updatePaths").c_str()) .def("addAgentsUniformly", &dsf::mobility::FirstOrderDynamics::addAgentsUniformly, pybind11::arg("nAgents"), @@ -649,20 +635,15 @@ PYBIND11_MODULE(dsf_cpp, m) { [](dsf::mobility::FirstOrderDynamics& self, std::size_t nAgents, dsf::mobility::AgentInsertionMethod insertionMethod) { - pybind11::gil_scoped_release release; self.addAgents(nAgents, insertionMethod); }, pybind11::arg("nAgents"), pybind11::arg("insertionMethod"), dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::addAgents").c_str()) - .def( - "evolve", - [](dsf::mobility::FirstOrderDynamics& self, bool reinsert_agents) { - pybind11::gil_scoped_release release; // release before simulation step - self.evolve(reinsert_agents); - }, - pybind11::arg("reinsert_agents") = false, - dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::evolve").c_str()) + .def("evolve", + &dsf::mobility::FirstOrderDynamics::evolve, + pybind11::arg("reinsert_agents") = false, + dsf::g_docstrings.at("dsf::mobility::FirstOrderDynamics::evolve").c_str()) .def( "optimizeTrafficLights", &dsf::mobility::FirstOrderDynamics::optimizeTrafficLights, diff --git a/src/dsf/mobility/FirstOrderDynamics.cpp b/src/dsf/mobility/FirstOrderDynamics.cpp index 342c0114..f026b85c 100644 --- a/src/dsf/mobility/FirstOrderDynamics.cpp +++ b/src/dsf/mobility/FirstOrderDynamics.cpp @@ -896,8 +896,7 @@ namespace dsf::mobility { "CREATE TABLE IF NOT EXISTS simulations (" "id INTEGER PRIMARY KEY, " "name TEXT, " - "alpha REAL, " - "speed_fluctuation_std REAL, " + "speed_function TEXT, " "weight_function TEXT, " "weight_threshold REAL NOT NULL, " "error_probability REAL, " @@ -913,56 +912,55 @@ namespace dsf::mobility { // Insert simulation parameters into the simulations table SQLite::Statement insertSimStmt( *this->database(), - "INSERT INTO simulations (id, name, alpha, speed_fluctuation_std, " - "weight_function, weight_threshold, error_probability, passage_probability, " + "INSERT INTO simulations (id, name, speed_function, weight_function, " + "weight_threshold, error_probability, passage_probability, " "mean_travel_distance_m, mean_travel_time_s, stagnant_tolerance_factor, " "force_priorities, save_avg_stats, save_road_data, save_travel_data) " - "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"); + "VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)"); insertSimStmt.bind(1, static_cast(this->id())); insertSimStmt.bind(2, this->name()); - insertSimStmt.bind(3); // alpha (removed) - insertSimStmt.bind(4); // speed_fluctuation_std (removed) + insertSimStmt.bind(3, this->m_speedFunctionDescription); switch (this->m_pathWeight) { case PathWeight::LENGTH: - insertSimStmt.bind(5, "LENGTH"); + insertSimStmt.bind(4, "LENGTH"); break; case PathWeight::TRAVELTIME: - insertSimStmt.bind(5, "TRAVELTIME"); + insertSimStmt.bind(4, "TRAVELTIME"); break; case PathWeight::WEIGHT: - insertSimStmt.bind(5, "WEIGHT"); + insertSimStmt.bind(4, "WEIGHT"); break; } - insertSimStmt.bind(6, this->m_weightTreshold); + insertSimStmt.bind(5, this->m_weightTreshold); if (this->m_errorProbability.has_value()) { - insertSimStmt.bind(7, *this->m_errorProbability); + insertSimStmt.bind(6, *this->m_errorProbability); } else { - insertSimStmt.bind(7); + insertSimStmt.bind(6); } if (this->m_passageProbability.has_value()) { - insertSimStmt.bind(8, *this->m_passageProbability); + insertSimStmt.bind(7, *this->m_passageProbability); } else { - insertSimStmt.bind(8); + insertSimStmt.bind(7); } if (this->m_meanTravelDistance.has_value()) { - insertSimStmt.bind(9, *this->m_meanTravelDistance); + insertSimStmt.bind(8, *this->m_meanTravelDistance); } else { - insertSimStmt.bind(9); + insertSimStmt.bind(8); } if (this->m_meanTravelTime.has_value()) { - insertSimStmt.bind(10, static_cast(*this->m_meanTravelTime)); + insertSimStmt.bind(9, static_cast(*this->m_meanTravelTime)); } else { - insertSimStmt.bind(10); + insertSimStmt.bind(9); } if (this->m_timeToleranceFactor.has_value()) { - insertSimStmt.bind(11, *this->m_timeToleranceFactor); + insertSimStmt.bind(10, *this->m_timeToleranceFactor); } else { - insertSimStmt.bind(11); + insertSimStmt.bind(10); } - insertSimStmt.bind(12, this->m_forcePriorities); - insertSimStmt.bind(13, this->m_bSaveAverageStats); - insertSimStmt.bind(14, this->m_bSaveStreetData); - insertSimStmt.bind(15, this->m_bSaveTravelData); + insertSimStmt.bind(11, this->m_forcePriorities); + insertSimStmt.bind(12, this->m_bSaveAverageStats); + insertSimStmt.bind(13, this->m_bSaveStreetData); + insertSimStmt.bind(14, this->m_bSaveTravelData); insertSimStmt.exec(); } void FirstOrderDynamics::m_dumpNetwork() const { diff --git a/src/dsf/mobility/FirstOrderDynamics.hpp b/src/dsf/mobility/FirstOrderDynamics.hpp index 2c2277da..d1bb160a 100644 --- a/src/dsf/mobility/FirstOrderDynamics.hpp +++ b/src/dsf/mobility/FirstOrderDynamics.hpp @@ -62,6 +62,7 @@ namespace dsf::mobility { std::atomic m_nAgents{0}, m_nAddedAgents{0}, m_nInsertedAgents{0}, m_nKilledAgents{0}, m_nArrivedAgents{0}; std::function const&)> m_speedFunction; + std::string m_speedFunctionDescription; protected: std::unordered_map> m_turnCounts; @@ -167,6 +168,26 @@ namespace dsf::mobility { /// - travel_time_s: The travel time of the agent in seconds void m_initTravelDataTable() const; + /// @brief Dump simulation metadata into the database. + /// @details Ensures the `simulations` table exists and inserts one row with the + /// current simulation configuration. If no database is connected, this function + /// returns immediately. + /// + /// Stored fields are: + /// - id + /// - name + /// - speed_function (identified by a string description, e.g. "LINEAR(alpha=0.5)" or "CUSTOM") + /// - weight_function + /// - weight_threshold + /// - error_probability + /// - passage_probability + /// - mean_travel_distance_m + /// - mean_travel_time_s + /// - stagnant_tolerance_factor + /// - force_priorities + /// - save_avg_stats + /// - save_road_data + /// - save_travel_data void m_dumpSimInfo() const; void m_dumpNetwork() const; @@ -440,6 +461,7 @@ namespace dsf::mobility { "double(std::unique_ptr const&)"); } else { m_speedFunction = std::get<0>(std::forward_as_tuple(args...)); + m_speedFunctionDescription = "CUSTOM"; } break; case SpeedFunction::LINEAR: @@ -464,6 +486,7 @@ namespace dsf::mobility { m_speedFunction = [alpha](std::unique_ptr const& pStreet) { return pStreet->maxSpeed() * (1. - alpha * pStreet->density(true)); }; + m_speedFunctionDescription = std::format("LINEAR(alpha={})", alpha); } break; } diff --git a/test/mobility/Test_dynamics.cpp b/test/mobility/Test_dynamics.cpp index 189f9e37..4608bc43 100644 --- a/test/mobility/Test_dynamics.cpp +++ b/test/mobility/Test_dynamics.cpp @@ -1395,8 +1395,7 @@ TEST_CASE("FirstOrderDynamics") { } CHECK(simColumns.count("id") == 1); CHECK(simColumns.count("name") == 1); - CHECK(simColumns.count("alpha") == 1); - CHECK(simColumns.count("speed_fluctuation_std") == 1); + CHECK(simColumns.count("speed_function") == 1); CHECK(simColumns.count("weight_function") == 1); CHECK(simColumns.count("weight_threshold") == 1); CHECK(simColumns.count("error_probability") == 1); From 48a7e5771243d82b727d573ea4767f779fb3d255 Mon Sep 17 00:00:00 2001 From: grufoony Date: Thu, 12 Mar 2026 10:55:38 +0100 Subject: [PATCH 4/5] Format --- examples/simulate_city.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/simulate_city.py b/examples/simulate_city.py index 64d52d5d..caeeb909 100644 --- a/examples/simulate_city.py +++ b/examples/simulate_city.py @@ -16,12 +16,14 @@ import numpy as np import networkx as nx + @cfunc(float64(float64, float64), nopython=True, cache=True) def custom_speed(max_speed, density): if density < 0.35: return max_speed * (0.9 - 0.1 * density) return max_speed * (1.2 - 0.7 * density) + logging.basicConfig( level=logging.INFO, format="%(asctime)s - %(levelname)s - %(message)s" ) From 437126e7ed6af1b6c8c82bdf4aaf3010af41cc57 Mon Sep 17 00:00:00 2001 From: grufoony Date: Thu, 12 Mar 2026 11:46:46 +0100 Subject: [PATCH 5/5] Fixing all --- examples/slow_charge_rb.cpp | 4 +--- examples/slow_charge_tl.cpp | 3 +-- examples/stalingrado.cpp | 3 +-- src/dsf/bindings.cpp | 6 +++--- src/dsf/mobility/FirstOrderDynamics.cpp | 3 +++ src/dsf/mobility/FirstOrderDynamics.hpp | 7 +++++-- 6 files changed, 14 insertions(+), 12 deletions(-) diff --git a/examples/slow_charge_rb.cpp b/examples/slow_charge_rb.cpp index a3ebe053..b0ddd3e6 100644 --- a/examples/slow_charge_rb.cpp +++ b/examples/slow_charge_rb.cpp @@ -97,8 +97,7 @@ int main(int argc, char** argv) { std::cout << "Creating dynamics...\n"; - Dynamics dynamics{graph, false, SEED, 0.6}; - + Dynamics dynamics{graph, false, SEED}; { std::vector destinationNodes; for (auto const& [nodeId, pNode] : dynamics.graph().nodes()) { @@ -114,7 +113,6 @@ int main(int argc, char** argv) { dynamics.setErrorProbability(0.05); dynamics.setPassageProbability(0.7707); // dynamics.setForcePriorities(true); - dynamics.setSpeedFluctuationSTD(0.1); // Connect database for saving data dynamics.connectDataBase(OUT_FOLDER + "simulation_data.db"); diff --git a/examples/slow_charge_tl.cpp b/examples/slow_charge_tl.cpp index 4b4c51de..3edb59f8 100644 --- a/examples/slow_charge_tl.cpp +++ b/examples/slow_charge_tl.cpp @@ -152,7 +152,7 @@ int main(int argc, char** argv) { std::cout << "Creating dynamics...\n"; - Dynamics dynamics{graph, false, SEED, 0.6}; + Dynamics dynamics{graph, false, SEED}; { std::vector destinationNodes; for (auto const& [nodeId, pNode] : dynamics.graph().nodes()) { @@ -168,7 +168,6 @@ int main(int argc, char** argv) { dynamics.setErrorProbability(ERROR_PROBABILITY); // dynamics.setMaxFlowPercentage(0.69); // dynamics.setForcePriorities(false); - dynamics.setSpeedFluctuationSTD(0.1); if (OPTIMIZE) dynamics.setDataUpdatePeriod(30); // Store data every 30 time steps diff --git a/examples/stalingrado.cpp b/examples/stalingrado.cpp index 32642ddd..4eef36c0 100644 --- a/examples/stalingrado.cpp +++ b/examples/stalingrado.cpp @@ -77,8 +77,7 @@ int main() { auto const& coil = graph.edge(19); // Create the dynamics - FirstOrderDynamics dynamics{graph, false, 69, 0.6}; - dynamics.setSpeedFluctuationSTD(0.2); + FirstOrderDynamics dynamics{graph, false, 69}; dynamics.addItinerary(4, 4); dynamics.updatePaths(); diff --git a/src/dsf/bindings.cpp b/src/dsf/bindings.cpp index 6f0cffaf..c55a4ed2 100644 --- a/src/dsf/bindings.cpp +++ b/src/dsf/bindings.cpp @@ -487,9 +487,9 @@ PYBIND11_MODULE(dsf_cpp, m) { "Args:\n" " speedFunction (SpeedFunction): The speed function type (LINEAR or " "CUSTOM)\n" - " arg: For LINEAR, a float alpha in [0., 1.]. " - "For CUSTOM, a callable(max_speed: float, density: float, length: float) -> " - "float") + " arg: For LINEAR, a float alpha in [0., 1.). " + "For CUSTOM, an integer address (uintptr_t) of a C function with signature " + "double(double max_speed, double density).") .def("setName", &dsf::mobility::FirstOrderDynamics::setName, pybind11::arg("name"), diff --git a/src/dsf/mobility/FirstOrderDynamics.cpp b/src/dsf/mobility/FirstOrderDynamics.cpp index f026b85c..7a744e1b 100644 --- a/src/dsf/mobility/FirstOrderDynamics.cpp +++ b/src/dsf/mobility/FirstOrderDynamics.cpp @@ -8,6 +8,9 @@ namespace dsf::mobility { bool useCache, std::optional seed) : Dynamics(graph, seed), m_bCacheEnabled{useCache} { + // Set defaults for weight and speed functions + this->setWeightFunction(PathWeight::TRAVELTIME); + this->setSpeedFunction(SpeedFunction::LINEAR, 0.8); if (m_bCacheEnabled) { if (!std::filesystem::exists(CACHE_FOLDER)) { std::filesystem::create_directory(CACHE_FOLDER); diff --git a/src/dsf/mobility/FirstOrderDynamics.hpp b/src/dsf/mobility/FirstOrderDynamics.hpp index d1bb160a..7f68eaee 100644 --- a/src/dsf/mobility/FirstOrderDynamics.hpp +++ b/src/dsf/mobility/FirstOrderDynamics.hpp @@ -220,6 +220,9 @@ namespace dsf::mobility { /// @param weightThreshold The weight threshold for updating the paths (default is std::nullopt) void setWeightFunction(PathWeight const pathWeight, std::optional weightThreshold = std::nullopt); + /// @brief Set the speed function. Options are: + /// - (LINEAR, alpha): speed = max_speed * (1 - alpha * density), where alpha is a parameter in [0, 1) + /// - (CUSTOM, func): speed = func(pointer to a street), where func is a callable provided by the user that takes the street's pointer. template void setSpeedFunction(SpeedFunction const speedFunction, TArgs&&... args); /// @brief Set the force priorities flag @@ -479,9 +482,9 @@ namespace dsf::mobility { .name())); } else { double alpha = std::get<0>(std::forward_as_tuple(args...)); - if (alpha < 0. || alpha > 1.) { + if (alpha < 0. || alpha >= 1.) { throw std::invalid_argument( - std::format("The alpha parameter ({}) must be in [0., 1]", alpha)); + std::format("The alpha parameter ({}) must be in [0., 1)", alpha)); } m_speedFunction = [alpha](std::unique_ptr const& pStreet) { return pStreet->maxSpeed() * (1. - alpha * pStreet->density(true));