diff --git a/src/dsf/dsf.hpp b/src/dsf/dsf.hpp index cd723225..41db3b36 100644 --- a/src/dsf/dsf.hpp +++ b/src/dsf/dsf.hpp @@ -6,7 +6,7 @@ static constexpr uint8_t DSF_VERSION_MAJOR = 4; static constexpr uint8_t DSF_VERSION_MINOR = 4; -static constexpr uint8_t DSF_VERSION_PATCH = 3; +static constexpr uint8_t DSF_VERSION_PATCH = 4; static auto const DSF_VERSION = std::format("{}.{}.{}", DSF_VERSION_MAJOR, DSF_VERSION_MINOR, DSF_VERSION_PATCH); diff --git a/src/dsf/mobility/Road.cpp b/src/dsf/mobility/Road.cpp index 76efb042..ce219cd3 100644 --- a/src/dsf/mobility/Road.cpp +++ b/src/dsf/mobility/Road.cpp @@ -101,13 +101,4 @@ namespace dsf::mobility { } return Direction::LEFT; } - - double Road::length() const { return m_length; } - double Road::maxSpeed() const { return m_maxSpeed; } - int Road::nLanes() const { return m_nLanes; } - int Road::capacity() const { return m_capacity; } - double Road::transportCapacity() const { return m_transportCapacity; } - std::string Road::name() const { return m_name; } - int Road::priority() const { return m_priority; } - std::set const& Road::forbiddenTurns() const { return m_forbiddenTurns; } }; // namespace dsf::mobility \ No newline at end of file diff --git a/src/dsf/mobility/Road.hpp b/src/dsf/mobility/Road.hpp index f8e8fc4a..9b5578f9 100644 --- a/src/dsf/mobility/Road.hpp +++ b/src/dsf/mobility/Road.hpp @@ -71,30 +71,30 @@ namespace dsf::mobility { /// @brief Get the length, in meters /// @return double The length, in meters - double length() const; + inline auto length() const noexcept { return m_length; } /// @brief Get the maximum speed, in meters per second /// @return double The maximum speed, in meters per second - double maxSpeed() const; + inline auto maxSpeed() const noexcept { return m_maxSpeed; } /// @brief Get the number of lanes /// @return int The number of lanes - int nLanes() const; + inline auto nLanes() const noexcept { return m_nLanes; } /// @brief Get the road's capacity, in number of agents /// @return int The road's capacity, in number of agents - int capacity() const; + inline auto capacity() const noexcept { return m_capacity; } /// @brief Get the road's transport capacity, in number of agents /// @return double The road's transport capacity, in number of agents - double transportCapacity() const; + inline auto transportCapacity() const noexcept { return m_transportCapacity; } /// @brief Get the name /// @return std::string The name - std::string name() const; + inline auto const& name() const noexcept { return m_name; } /// @brief Get the priority /// @return int The priority - int priority() const; + inline auto priority() const noexcept { return m_priority; } /// @brief Get the road's forbidden turns /// @return std::set The road's forbidden turns /// @details The forbidden turns are the road ids that are not allowed to be used by the agents /// when they are on the road. - std::set const& forbiddenTurns() const; + inline auto const& forbiddenTurns() const noexcept { return m_forbiddenTurns; } /// @brief Get the road's turn direction given the previous road angle /// @param previousStreetAngle The angle of the previous road /// @return Direction The turn direction diff --git a/src/dsf/mobility/RoadDynamics.hpp b/src/dsf/mobility/RoadDynamics.hpp index 1b268aa2..730c797c 100644 --- a/src/dsf/mobility/RoadDynamics.hpp +++ b/src/dsf/mobility/RoadDynamics.hpp @@ -80,13 +80,11 @@ namespace dsf::mobility { void m_updatePath(std::unique_ptr const& pItinerary); /// @brief Get the next street id - /// @param agentId The id of the agent - /// @param NodeId The id of the node - /// @param streetId The id of the incoming street + /// @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(std::unique_ptr const& pAgent, - Id NodeId, - std::optional streetId = std::nullopt); + 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 @@ -454,7 +452,7 @@ namespace dsf::mobility { m_travelDTs.push_back({pAgent->distance(), static_cast(this->time_step() - pAgent->spawnTime())}); --m_nAgents; - return std::move(pAgent); + return pAgent; } template @@ -495,146 +493,108 @@ namespace dsf::mobility { template requires(is_numeric_v) std::optional RoadDynamics::m_nextStreetId( - std::unique_ptr const& pAgent, Id nodeId, std::optional streetId) { - // Get outgoing edges directly - avoid storing targets separately - auto const& pNode{this->graph().node(nodeId)}; + const std::unique_ptr& pAgent, const std::unique_ptr& pNode) { + spdlog::trace("Computing m_nextStreetId for {}", *pAgent); auto const& outgoingEdges = pNode->outgoingEdges(); - if (outgoingEdges.size() == 1) { - return outgoingEdges[0]; - } - if (pAgent->isRandom()) { // Try to use street transition probabilities - spdlog::trace("Computing m_nextStreetId for {}", *pAgent); - if (streetId.has_value()) { - auto const& pStreetCurrent{this->graph().edge(streetId.value())}; - auto const speedCurrent{pStreetCurrent->maxSpeed() /** - this->m_speedFactor(pStreetCurrent->density())*/}; - double cumulativeProbability = 0.0; - std::unordered_map transitionProbabilities; - for (const auto outEdgeId : outgoingEdges) { - auto const& pStreetOut{this->graph().edge(outEdgeId)}; - auto const speed{pStreetOut->maxSpeed() /** - this->m_speedFactor(pStreetOut->density())*/}; - double probability = speed * speedCurrent; - if (pStreetOut->target() == pStreetCurrent->source()) { - if (pNode->isRoundabout()) { - probability *= U_TURN_PENALTY_FACTOR; // Discourage U-TURNS - } else { - continue; // No U-TURNS - } - } - transitionProbabilities[pStreetOut->id()] = probability; - cumulativeProbability += probability; - } - std::uniform_real_distribution uniformDist{0., cumulativeProbability}; - auto const randValue = uniformDist(this->m_generator); - cumulativeProbability = 0.0; - for (const auto& [targetStreetId, probability] : transitionProbabilities) { - cumulativeProbability += probability; - if (randValue <= cumulativeProbability) { - return targetStreetId; - } - } - // auto const& transitionProbabilities = pStreetCurrent->transitionProbabilities(); - // if (!transitionProbabilities.empty()) { - // std::uniform_real_distribution uniformDist{0., 1.}; - // auto const randValue = uniformDist(this->m_generator); - // double cumulativeProbability = 0.0; - // for (const auto& [targetStreetId, probability] : transitionProbabilities) { - // cumulativeProbability += probability; - // if (randValue <= cumulativeProbability) { - // return targetStreetId; - // } - // } - // } - } - } - std::vector possibleEdgeIds; - possibleEdgeIds.reserve(outgoingEdges.size()); // Pre-allocate to avoid reallocations - - // Build forbidden target nodes set efficiently - std::unordered_set forbiddenTargetNodes; - if (streetId.has_value()) { - auto const& pStreet{this->graph().edge(*streetId)}; - const auto& forbiddenTurns = pStreet->forbiddenTurns(); - forbiddenTargetNodes.insert(forbiddenTurns.begin(), forbiddenTurns.end()); - - // Avoid U-TURNS, if possible - if (!(this->graph().node(nodeId)->isRoundabout()) && - (outgoingEdges.size() > forbiddenTurns.size() + 1)) { - auto const& pOppositeStreet{ - this->graph().street(pStreet->target(), pStreet->source())}; - if (pOppositeStreet) { - forbiddenTargetNodes.insert(pOppositeStreet->get()->id()); + + // Get current street information + std::optional previousNodeId = std::nullopt; + std::set forbiddenTurns; + double speedCurrent = 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(); + } + + // Get path targets for non-random agents + std::vector pathTargets; + if (!pAgent->isRandom()) { + try { + pathTargets = m_itineraries.at(pAgent->itineraryId())->path().at(pNode->id()); + } catch (const std::out_of_range&) { + if (!m_itineraries.contains(pAgent->itineraryId())) { + throw std::runtime_error( + std::format("No itinerary found with id {}", pAgent->itineraryId())); } + throw std::runtime_error(std::format( + "No path found for itinerary {} at node {}. To solve this error, consider " + "using ODs extracted from a fully-connected subnetwork of your whole road " + "network or, alternatively, set an error probability.", + pAgent->itineraryId(), + pNode->id())); } } - // Log forbidden turns if any - if (!forbiddenTargetNodes.empty()) { - spdlog::debug("Excluding {} forbidden turns", forbiddenTargetNodes.size()); - } + // Calculate transition probabilities for all valid outgoing edges + std::unordered_map transitionProbabilities; + double cumulativeProbability = 0.0; - // For non-random agents, get allowed targets from itinerary - std::unordered_set allowedTargets; - bool hasItineraryConstraints = false; + for (const auto outEdgeId : outgoingEdges) { + if (forbiddenTurns.contains(outEdgeId)) { + continue; + } - if (!this->itineraries().empty()) { - std::uniform_real_distribution uniformDist{0., 1.}; - if (!(m_errorProbability.has_value() && - uniformDist(this->m_generator) < m_errorProbability)) { - const auto& it = this->itineraries().at(pAgent->itineraryId()); - if (it->destination() != nodeId) { - try { - const auto pathTargets = it->path().at(nodeId); - allowedTargets.insert(pathTargets.begin(), pathTargets.end()); - hasItineraryConstraints = true; + auto const& pStreetOut{this->graph().edge(outEdgeId)}; - // Remove forbidden nodes from allowed targets - for (const auto& forbiddenNodeId : forbiddenTargetNodes) { - allowedTargets.erase(forbiddenNodeId); - } - // Catch unordered_map::at exceptions - } catch (const std::out_of_range&) { - throw std::runtime_error(std::format( - "No path from node {} to destination {}", nodeId, it->destination())); - } + // Check if this is a valid path target for non-random agents + bool bIsPathTarget = true; + if (!pathTargets.empty()) { + bIsPathTarget = + std::find(pathTargets.cbegin(), pathTargets.cend(), pStreetOut->target()) != + pathTargets.cend(); + if (!this->m_errorProbability.has_value() && !bIsPathTarget) { + continue; } } - } - // Single pass through outgoing edges with efficient filtering - for (const auto outEdgeId : outgoingEdges) { - const Id targetNode = this->graph().edge(outEdgeId)->target(); + // Calculate base probability + auto const speedNext{pStreetOut->maxSpeed()}; + double probability = speedCurrent * speedNext; - // Skip if target is forbidden - if (forbiddenTargetNodes.count(targetNode)) { - continue; + // 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())); } - // For non-random agents with itinerary constraints - if (hasItineraryConstraints) { - if (allowedTargets.count(targetNode)) { - possibleEdgeIds.push_back(outEdgeId); + // Handle U-turns + if (previousNodeId.has_value() && pStreetOut->target() == previousNodeId.value()) { + if (pNode->isRoundabout()) { + probability *= U_TURN_PENALTY_FACTOR; + } else { + continue; // No U-turns allowed } - } else { - // For random agents or when no itinerary constraints apply - possibleEdgeIds.push_back(outEdgeId); } + + transitionProbabilities[pStreetOut->id()] = probability; + cumulativeProbability += probability; } - if (possibleEdgeIds.empty()) { + // Select street based on weighted probabilities + if (transitionProbabilities.empty()) { + spdlog::trace("No valid transitions found for {} at {}", *pAgent, *pNode); return std::nullopt; - // throw std::runtime_error( - // std::format("No possible moves for agent {} at node {}.", *pAgent, nodeId)); } - - if (possibleEdgeIds.size() == 1) { - return possibleEdgeIds[0]; + if (transitionProbabilities.size() == 1) { + return transitionProbabilities.cbegin()->first; } - std::uniform_int_distribution moveDist{ - 0, static_cast(possibleEdgeIds.size() - 1)}; - return possibleEdgeIds[moveDist(this->m_generator)]; + std::uniform_real_distribution uniformDist{0., cumulativeProbability}; + auto const randValue = uniformDist(this->m_generator); + double accumulated = 0.0; + for (const auto& [targetStreetId, probability] : transitionProbabilities) { + accumulated += probability; + if (randValue < accumulated) { + return targetStreetId; + } + } + // Return last one as fallback + return std::prev(transitionProbabilities.cend())->first; } template @@ -667,7 +627,7 @@ namespace dsf::mobility { continue; } auto const nextStreetId = - this->m_nextStreetId(pAgent, pStreet->target(), pStreet->id()); + 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()); @@ -767,7 +727,7 @@ namespace dsf::mobility { timeTolerance, timeDiff); // Kill the agent - this->m_killAgent(std::move(pStreet->dequeue(queueIndex))); + this->m_killAgent(pStreet->dequeue(queueIndex)); continue; } } @@ -926,7 +886,7 @@ namespace dsf::mobility { } } if (bArrived) { - auto pAgent = this->m_killAgent(std::move(pStreet->dequeue(queueIndex))); + auto pAgent = this->m_killAgent(pStreet->dequeue(queueIndex)); if (reinsert_agents) { // reset Agent's values pAgent->reset(this->time_step()); @@ -1055,6 +1015,10 @@ namespace dsf::mobility { 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()); @@ -1073,8 +1037,7 @@ namespace dsf::mobility { } if (!pAgent->nextStreetId().has_value()) { spdlog::debug("No next street id, generating a random one"); - auto const nextStreetId{ - this->m_nextStreetId(pAgent, pSourceNode->id(), pAgent->streetId())}; + 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()); diff --git a/src/dsf/mobility/RoadJunction.cpp b/src/dsf/mobility/RoadJunction.cpp index 9e4431d5..2122ff50 100644 --- a/src/dsf/mobility/RoadJunction.cpp +++ b/src/dsf/mobility/RoadJunction.cpp @@ -19,8 +19,4 @@ namespace dsf::mobility { double RoadJunction::transportCapacity() const { return m_transportCapacity; } double RoadJunction::density() const { return 0.; } bool RoadJunction::isFull() const { return true; } - - bool RoadJunction::isIntersection() const noexcept { return false; } - bool RoadJunction::isTrafficLight() const noexcept { return false; } - bool RoadJunction::isRoundabout() const noexcept { return false; } } // namespace dsf::mobility \ No newline at end of file diff --git a/src/dsf/mobility/RoadJunction.hpp b/src/dsf/mobility/RoadJunction.hpp index f407346e..3a66c105 100644 --- a/src/dsf/mobility/RoadJunction.hpp +++ b/src/dsf/mobility/RoadJunction.hpp @@ -42,9 +42,9 @@ namespace dsf::mobility { virtual double density() const; virtual bool isFull() const; - virtual bool isIntersection() const noexcept; - virtual bool isTrafficLight() const noexcept; - virtual bool isRoundabout() const noexcept; + virtual constexpr bool isIntersection() const noexcept { return false; } + virtual constexpr bool isTrafficLight() const noexcept { return false; } + virtual constexpr bool isRoundabout() const noexcept { return false; } }; } // namespace dsf::mobility diff --git a/src/dsf/mobility/Street.hpp b/src/dsf/mobility/Street.hpp index 96ecb58d..c4dce920 100644 --- a/src/dsf/mobility/Street.hpp +++ b/src/dsf/mobility/Street.hpp @@ -167,7 +167,7 @@ namespace dsf::mobility { constexpr bool hasCoil() const { return m_counter.has_value(); }; /// @brief Check if the street is stochastic /// @return bool True if the street is stochastic, false otherwise - virtual bool isStochastic() const { return false; }; + virtual constexpr bool isStochastic() const noexcept { return false; }; }; /// @brief A stochastic street is a street with a flow rate parameter @@ -200,7 +200,7 @@ namespace dsf::mobility { inline auto flowRate() const { return m_flowRate; } /// @brief Check if the street is stochastic /// @return bool True if the street is stochastic, false otherwise - constexpr bool isStochastic() const final { return true; }; + constexpr bool isStochastic() const noexcept final { return true; }; }; }; // namespace dsf::mobility diff --git a/test/mobility/Test_dynamics.cpp b/test/mobility/Test_dynamics.cpp index 1909986d..61e2f991 100644 --- a/test/mobility/Test_dynamics.cpp +++ b/test/mobility/Test_dynamics.cpp @@ -549,12 +549,14 @@ TEST_CASE("FirstOrderDynamics") { } } GIVEN("A simple network and an agent with forced itinerary") { + spdlog::set_level(spdlog::level::trace); Street s0_1{1, std::make_pair(0, 1), 30., 15.}; Street s1_0{3, std::make_pair(1, 0), 30., 15.}; Street s1_2{5, std::make_pair(1, 2), 30., 15.}; Street s2_1{7, std::make_pair(2, 1), 30., 15.}; RoadNetwork graph2; graph2.addStreets(s0_1, s1_0, s1_2, s2_1); + graph2.makeRoundabout(2); FirstOrderDynamics dynamics{graph2, false, 69, 0., dsf::PathWeight::LENGTH}; dynamics.setDestinationNodes({1, 2}); dynamics.updatePaths(); @@ -584,6 +586,7 @@ TEST_CASE("FirstOrderDynamics") { CHECK_EQ(dynamics.nAgents(), 0); } } + spdlog::set_level(spdlog::level::info); } } SUBCASE("TrafficLights") {