Rework next streetid computation

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);

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<Id> const& Road::forbiddenTurns() const { return m_forbiddenTurns; }
 };  // namespace dsf::mobility

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<Id> 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<Id> 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

src/dsf/mobility/RoadDynamics.hpp

@@ -80,13 +80,11 @@
     void m_updatePath(std::unique_ptr<Itinerary> 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<Id> m_nextStreetId(std::unique_ptr<Agent> const& pAgent,
-                                     Id NodeId,
-                                     std::optional<Id> streetId = std::nullopt);
+    std::optional<Id> m_nextStreetId(const std::unique_ptr<Agent>& pAgent,
+                                     const std::unique_ptr<RoadJunction>& 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 @@
     m_travelDTs.push_back({pAgent->distance(),
                            static_cast<double>(this->time_step() - pAgent->spawnTime())});
     --m_nAgents;
-    return std::move(pAgent);
+    return pAgent;
   }
 
   template <typename delay_t>
@@ -495,146 +493,108 @@
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   std::optional<Id> RoadDynamics<delay_t>::m_nextStreetId(
-      std::unique_ptr<Agent> const& pAgent, Id nodeId, std::optional<Id> streetId) {
-    // Get outgoing edges directly - avoid storing targets separately
-    auto const& pNode{this->graph().node(nodeId)};
+      const std::unique_ptr<Agent>& pAgent, const std::unique_ptr<RoadJunction>& 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<Id, double> 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<double> 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<double> 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<Id> possibleEdgeIds;
-    possibleEdgeIds.reserve(outgoingEdges.size());  // Pre-allocate to avoid reallocations
-
-    // Build forbidden target nodes set efficiently
-    std::unordered_set<Id> 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<Id> previousNodeId = std::nullopt;
+    std::set<Id> 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<Id> 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<Id, double> transitionProbabilities;
+    double cumulativeProbability = 0.0;
 
-    // For non-random agents, get allowed targets from itinerary
-    std::unordered_set<Id> allowedTargets;
-    bool hasItineraryConstraints = false;
+    for (const auto outEdgeId : outgoingEdges) {
+      if (forbiddenTurns.contains(outEdgeId)) {
+        continue;
+      }
 
-    if (!this->itineraries().empty()) {
-      std::uniform_real_distribution<double> 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<double>(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<Size> moveDist{
-        0, static_cast<Size>(possibleEdgeIds.size() - 1)};
-    return possibleEdgeIds[moveDist(this->m_generator)];
+    std::uniform_real_distribution<double> 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 <typename delay_t>
@@ -667,7 +627,7 @@
         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 @@
                 timeTolerance,
                 timeDiff);
             // Kill the agent
-            this->m_killAgent(std::move(pStreet->dequeue(queueIndex)));
+            this->m_killAgent(pStreet->dequeue(queueIndex));
             continue;
           }
         }
@@ -926,7 +886,7 @@
           }
         }
         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 @@
   template <typename delay_t>
     requires(is_numeric_v<delay_t>)
   void RoadDynamics<delay_t>::m_evolveAgents() {
+    if (m_agents.empty()) {
+      spdlog::trace("No agents to process.");
+      return;
+    }
     std::uniform_int_distribution<Id> nodeDist{
         0, static_cast<Id>(this->graph().nNodes() - 1)};
     spdlog::debug("Processing {} agents", m_agents.size());
@@ -1073,8 +1037,7 @@
       }
       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());
@@ -1305,7 +1268,7 @@
     }
     bool const bRandomItinerary{!optItineraryId.has_value() &&
                                 !this->itineraries().empty()};
     std::optional<Id> itineraryId{std::nullopt};
     std::uniform_int_distribution<Size> itineraryDist{
         0, static_cast<Size>(this->itineraries().size() - 1)};
     std::uniform_int_distribution<Size> streetDist{

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

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