Add stationary probability for random vehicles movement

src/dsf/bindings.cpp

@@ -172,6 +172,11 @@ PYBIND11_MODULE(dsf_cpp, m) {
       .def("autoMapStreetLanes",
            &dsf::mobility::RoadNetwork::autoMapStreetLanes,
            dsf::g_docstrings.at("dsf::mobility::RoadNetwork::autoMapStreetLanes").c_str())
+      .def("setStreetStationaryWeights",
+           &dsf::mobility::RoadNetwork::setStreetStationaryWeights,
+           pybind11::arg("weights"),
+           dsf::g_docstrings.at("dsf::mobility::RoadNetwork::setStreetStationaryWeights")
+               .c_str())
       .def(
           "importEdges",
           [](dsf::mobility::RoadNetwork& self, const std::string& fileName) {
@@ -531,6 +536,10 @@ PYBIND11_MODULE(dsf_cpp, m) {
            pybind11::arg("ratio") = 1.3,
            dsf::g_docstrings.at("dsf::mobility::RoadDynamics::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())

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 = 9;
+static constexpr uint8_t DSF_VERSION_PATCH = 10;
 
 static auto const DSF_VERSION =
     std::format("{}.{}.{}", DSF_VERSION_MAJOR, DSF_VERSION_MINOR, DSF_VERSION_PATCH);

src/dsf/mobility/RoadDynamics.hpp

@@ -497,11 +497,13 @@ namespace dsf::mobility {
     std::optional<Id> previousNodeId = std::nullopt;
     std::set<Id> forbiddenTurns;
     double speedCurrent = 1.0;
+    double stationaryWeightCurrent = 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();
+      stationaryWeightCurrent = pStreetCurrent->stationaryWeight();
     }
 
     // Get path targets for non-random agents
@@ -550,7 +552,10 @@ namespace dsf::mobility {
 
       // Calculate base probability
       auto const speedNext{pStreetOut->maxSpeed()};
-      double probability = speedCurrent * speedNext;
+      double const stationaryWeightNext = pStreetOut->stationaryWeight();
+      auto const weightRatio{stationaryWeightNext /
+                             stationaryWeightCurrent};  // SQRT (p_i / p_j)
+      double probability = speedCurrent * speedNext * std::sqrt(weightRatio);
 
       // Apply error probability for non-random agents
       if (this->m_errorProbability.has_value() && !pathTargets.empty()) {

src/dsf/mobility/RoadNetwork.cpp

@@ -263,18 +263,6 @@ namespace dsf::mobility {
           addCoil(edge_id);
         }
       }
-      // Check for transition probabilities
-      // if (!edge_properties.at_key("transition_probabilities").error() &&
-      //     edge_properties["transition_probabilities"].has_value()) {
-      //   auto const& tp = edge_properties["transition_probabilities"];
-      //   std::unordered_map<Id, double> transitionProbabilities;
-      //   for (auto const& [key, value] : tp.get_object()) {
-      //     auto const targetStreetId = static_cast<Id>(std::stoull(std::string(key)));
-      //     auto const probability = static_cast<double>(value.get_double());
-      //     transitionProbabilities.emplace(targetStreetId, probability);
-      //   }
-      //   edge(edge_id)->setTransitionProbabilities(transitionProbabilities);
-      // }
     }
     this->m_nodes.rehash(0);
     this->m_edges.rehash(0);
@@ -843,6 +831,22 @@ namespace dsf::mobility {
     addEdge<Street>(std::move(street));
   }
 
+  void RoadNetwork::setStreetStationaryWeights(
+      std::unordered_map<Id, double> const& weights) {
+    std::for_each(DSF_EXECUTION m_edges.cbegin(),
+                  m_edges.cend(),
+                  [this, &weights](auto const& pair) {
+                    auto const streetId = pair.first;
+                    auto const& pStreet = pair.second;
+                    auto it = weights.find(streetId);
+                    if (it != weights.end()) {
+                      pStreet->setStationaryWeight(it->second);
+                    } else {
+                      pStreet->setStationaryWeight(1.0);
+                    }
+                  });
+  }
+
   const std::unique_ptr<Street>* RoadNetwork::street(Id source, Id destination) const {
     // Get the iterator at id m_cantorPairingHashing(source, destination)
     try {

src/dsf/mobility/RoadNetwork.hpp

@@ -65,6 +65,12 @@ namespace dsf::mobility {
     /// @brief Construct a new RoadNetwork object
     /// @param adj An adjacency matrix made by a SparseMatrix representing the graph's adjacency matrix
     RoadNetwork(AdjacencyMatrix const& adj);
+    // Disable copy constructor and copy assignment operator
+    RoadNetwork(const RoadNetwork&) = delete;
+    RoadNetwork& operator=(const RoadNetwork&) = delete;
+    // Enable move constructor and move assignment operator
+    RoadNetwork(RoadNetwork&&) = default;
+    RoadNetwork& operator=(RoadNetwork&&) = default;
 
     /// @brief Get the graph's number of coil streets
     /// @return The number of coil streets
@@ -184,6 +190,9 @@ namespace dsf::mobility {
       requires is_street_v<std::remove_reference_t<T1>> &&
                (is_street_v<std::remove_reference_t<Tn>> && ...)
     void addStreets(T1&& street, Tn&&... streets);
+    /// @brief Set the streets' stationary weights
+    /// @param streetWeights A map where the key is the street id and the value is the street stationary weight. If a street id is not present in the map, its stationary weight is set to 1.0.
+    void setStreetStationaryWeights(std::unordered_map<Id, double> const& streetWeights);
 
     /// @brief Get a street from the graph
     /// @param source The source node

src/dsf/mobility/Street.hpp

@@ -48,8 +48,8 @@
                         AgentComparator>
         m_movingAgents;
     std::vector<Direction> m_laneMapping;
-    // std::unordered_map<Id, double> m_transitionProbabilities;
     std::optional<Counter> m_counter;
+    double m_stationaryWeight{1.0};
 
   public:
     /// @brief Construct a new Street object
@@ -84,12 +84,12 @@
     /// @param meanVehicleLength The mean vehicle length
     /// @throw std::invalid_argument If the mean vehicle length is negative
     static void setMeanVehicleLength(double meanVehicleLength);
-    // /// @brief Set the street's transition probabilities
-    // /// @param transitionProbabilities The street's transition probabilities
-    // inline void setTransitionProbabilities(
-    //     std::unordered_map<Id, double> const& transitionProbabilities) {
-    //   m_transitionProbabilities = transitionProbabilities;
-    // };
+    /// @brief Set the street's stationary weight
+    /// @param weight The street's stationary weight
+    inline void setStationaryWeight(double const weight) {
+      weight > 0. ? m_stationaryWeight = weight
+                  : throw std::invalid_argument("Stationary weight must be positive");
+    }
     /// @brief Enable a coil (dsf::Counter sensor) on the street
     /// @param name The name of the counter (default is "Coil_<street_id>")
     void enableCounter(std::string name = std::string());
@@ -117,10 +117,9 @@
     /// @brief Check if the street is full
     /// @return bool, True if the street is full, false otherwise
     inline bool isFull() const final { return this->nAgents() == this->m_capacity; }
-
-    // inline auto const& transitionProbabilities() const {
-    //   return m_transitionProbabilities;
-    // }
+    /// @brief Get the street's stationary weight
+    /// @return double The street's stationary weight
+    inline auto stationaryWeight() const noexcept { return m_stationaryWeight; }
     /// @brief Get the name of the counter
     /// @return std::string The name of the counter
     inline auto counterName() const {

test/mobility/Test_dynamics.cpp

@@ -2,6 +2,7 @@
 #include "dsf/mobility/RoadNetwork.hpp"
 #include "dsf/mobility/Itinerary.hpp"
 #include "dsf/mobility/Street.hpp"
+#include "dsf/mobility/Intersection.hpp"
 
 #include <chrono>
 #include <cstdint>
@@ -1282,3 +1283,101 @@
     }
   }
 }
+
+TEST_CASE("Stationary Weights Impact on Random Navigation") {
+  RoadNetwork network;
+
+  network.addNode<Intersection>(0);
+  network.addNode<Intersection>(1);
+  network.addNode<Intersection>(2);
+  network.addNode<Intersection>(3);
+
+  const int numAgents = 3000;
+  const int highCapacity = numAgents + 100;
+
+  // Street 0: 0 -> 1
+  // High length to hold all agents, high transport capacity to move them all at once
+  network.addStreet(Street(0,
+                           {0, 1},
+                           100.0,
+                           10.0,
+                           1,
+                           "Street 0",
+                           {},
+                           highCapacity,
+                           static_cast<double>(highCapacity)));
+  // Street 1: 1 -> 2
+  network.addStreet(Street(1,
+                           {1, 2},
+                           100.0,
+                           10.0,
+                           1,
+                           "Street 1",
+                           {},
+                           highCapacity,
+                           static_cast<double>(highCapacity)));
+  // Street 2: 1 -> 3
+  network.addStreet(Street(2,
+                           {1, 3},
+                           100.0,
+                           10.0,
+                           1,
+                           "Street 2",
+                           {},
+                           highCapacity,
+                           static_cast<double>(highCapacity)));
+
+  // Set stationary weights
+  // Street 0: weight 1.0
+  network.edge(0)->setStationaryWeight(1.0);
+
+  // Street 1: weight 1.0
+  network.edge(1)->setStationaryWeight(1.0);
+
+  // Street 2: weight 4.0
+  network.edge(2)->setStationaryWeight(4.0);
+
+  // Adjust node capacities to match street capacities
+  network.adjustNodeCapacities();
+
+  // Initialize dynamics
+  FirstOrderDynamics dynamics(network, false, 42, 0.0);
+
+  // Add many random agents to Street 0
+  for (int i = 0; i < numAgents; ++i) {
+    auto agent = std::make_unique<Agent>(0, std::nullopt, 0);
+    agent->setStreetId(0);
+    agent->setSpeed(10.0);
+    agent->setFreeTime(0);
+    dynamics.graph().edge(0)->addAgent(std::move(agent));
+  }
+
+  // Evolve simulation
+  // Step 1: Agents move from Street 0 to Node 1
+  dynamics.evolve();
+
+  // Step 2: Agents move from Node 1 to Street 1 or 2
+  dynamics.evolve();
+
+  // Count agents on Street 1 and Street 2
+  size_t countStreet1 = dynamics.graph().edge(1)->nAgents();
+  size_t countStreet2 = dynamics.graph().edge(2)->nAgents();
+
+  // Expected probabilities:
+  // P(1) ~ speed * speed * sqrt(1/1) = 100
+  // P(2) ~ speed * speed * sqrt(4/1) = 200
+  // Ratio 1:2 -> P(1) = 1/3, P(2) = 2/3
+
+  double ratio =
+      (countStreet1 > 0) ? static_cast<double>(countStreet2) / countStreet1 : 0.0;
+
+  std::cout << "Agents on Street 1: " << countStreet1 << std::endl;
+  std::cout << "Agents on Street 2: " << countStreet2 << std::endl;
+  std::cout << "Ratio (Street 2 / Street 1): " << ratio << std::endl;
+
+  // Check if ratio is close to 2.0
+  CHECK_EQ(ratio, doctest::Approx(2.0).epsilon(0.1));  // Allow 10% error margin
+
+  // Check total agents preserved (some might be in transit or node if something went wrong)
+  CHECK_EQ(countStreet1 + countStreet2, numAgents);
+}