Add random fluctuations on speed (#169)

--------- Co-authored-by: sbaldu <[email protected]> Co-authored-by: Simone Balducci <[email protected]>
physycom · Apr 10, 2024 · a65137c · a65137c
1 parent 48af81c
commit a65137c
Show file tree

Hide file tree

Showing 7 changed files with 122 additions and 40 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.16.0)
 
-project(dms VERSION 1.3.2 LANGUAGES CXX)
+project(dms VERSION 1.3.3 LANGUAGES CXX)
 
 # set the C++ standard
 set(CMAKE_CXX_STANDARD 20)

diff --git a/src/dsm/dsm.hpp b/src/dsm/dsm.hpp
@@ -5,8 +5,8 @@
 
 static constexpr uint8_t DSM_VERSION_MAJOR = 1;
 static constexpr uint8_t DSM_VERSION_MINOR = 3;
-static constexpr uint8_t DSM_VERSION_PATCH = 2;
-static constexpr uint8_t DSM_VERSION_BUILD = 2;
+static constexpr uint8_t DSM_VERSION_PATCH = 3;
+static constexpr uint8_t DSM_VERSION_BUILD = 8;
 
 #include <string>
 
@@ -16,8 +16,8 @@ namespace dsm {
 
 std::string dsm::version() {
   return std::to_string(DSM_VERSION_MAJOR) + "." + std::to_string(DSM_VERSION_MINOR) +
-             "." + std::to_string(DSM_VERSION_PATCH),
-         "." + std::to_string(DSM_VERSION_BUILD);
+         "." + std::to_string(DSM_VERSION_PATCH) + "." +
+         std::to_string(DSM_VERSION_BUILD);
 }
 
 #include "headers/Agent.hpp"

diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp
@@ -339,7 +339,7 @@ namespace dsm {
       }
       const auto& nextStreet{
           m_graph.streetSet()[m_nextStreetId(agentId, destinationNode->id())]};
-      if (nextStreet->density() == 1) {
+      if (!(nextStreet->density() < 1)) {
         continue;
       }
       street->dequeue();
@@ -349,11 +349,11 @@ namespace dsm {
       if (destinationNode->isIntersection()) {
         auto& intersection = dynamic_cast<Node<Id, Size>&>(*destinationNode);
         intersection.addAgent(delta, agentId);
+        m_agentNextStreetId.emplace(agentId, nextStreet->id());
       } else if (destinationNode->isRoundabout()) {
         auto& roundabout = dynamic_cast<Roundabout<Id, Size>&>(*destinationNode);
         roundabout.enqueue(agentId);
       }
-      m_agentNextStreetId.emplace(agentId, nextStreet->id());
     }
   }
 
@@ -372,7 +372,7 @@ namespace dsm {
             this->setAgentSpeed(agentId);
             m_agents[agentId]->incrementDelay(
                 std::ceil(nextStreet->length() / m_agents[agentId]->speed()));
-            nextStreet->enqueue(agentId);
+            nextStreet->addAgent(agentId);
             m_agentNextStreetId.erase(agentId);
           } else if (m_forcePriorities) {
             break;
@@ -387,15 +387,15 @@ namespace dsm {
         const auto nAgents{roundabout.agents().size()};
         for (size_t i{0}; i < nAgents; ++i) {
           const auto agentId{roundabout.agents().front()};
-          const auto& nextStreet{m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+          const auto& nextStreet{
+              this->m_graph.streetSet()[m_nextStreetId(agentId, node->id())]};
           if (nextStreet->density() < 1) {
             roundabout.dequeue();
             m_agents[agentId]->setStreetId(nextStreet->id());
             this->setAgentSpeed(agentId);
             m_agents[agentId]->incrementDelay(
                 std::ceil(nextStreet->length() / m_agents[agentId]->speed()));
-            nextStreet->enqueue(agentId);
-            m_agentNextStreetId.erase(agentId);
+            nextStreet->addAgent(agentId);
           } else {
             break;
           }
@@ -410,19 +410,21 @@ namespace dsm {
   void Dynamics<Id, Size, Delay>::m_evolveAgents() {
     for (const auto& [agentId, agent] : this->m_agents) {
       if (agent->delay() > 0) {
+        const auto& street{m_graph.streetSet()[agent->streetId().value()]};
         if (agent->delay() > 1) {
           agent->incrementDistance();
-        } else if (agent->streetId().has_value()) {
-          double distance{
-              std::fmod(this->m_graph.streetSet()[agent->streetId().value()]->length(),
-                        agent->speed())};
+        } else {
+          double distance{std::fmod(street->length(), agent->speed())};
           if (distance < std::numeric_limits<double>::epsilon()) {
             agent->incrementDistance();
           } else {
             agent->incrementDistance(distance);
           }
         }
         agent->decrementDelay();
+        if (agent->delay() == 0) {
+          street->enqueue(agentId);
+        }
       } else if (!agent->streetId().has_value()) {
         assert(agent->srcNodeId().has_value());
         const auto& srcNode{this->m_graph.nodeSet()[agent->srcNodeId().value()]};
@@ -438,11 +440,13 @@ namespace dsm {
         if (srcNode->isIntersection()) {
           auto& intersection = dynamic_cast<Node<Id, Size>&>(*srcNode);
           intersection.addAgent(0., agentId);
+          m_agentNextStreetId.emplace(agentId, nextStreet->id());
         } else if (srcNode->isRoundabout()) {
           auto& roundabout = dynamic_cast<Roundabout<Id, Size>&>(*srcNode);
           roundabout.enqueue(agentId);
         }
-        m_agentNextStreetId.emplace(agentId, nextStreet->id());
+      } else if (agent->delay() == 0) {
+        agent->setSpeed(0.);
       }
       agent->incrementTime();
     }
@@ -655,7 +659,7 @@ namespace dsm {
       this->setAgentSpeed(agentId);
       this->m_agents[agentId]->incrementDelay(
           std::ceil(street->length() / this->m_agents[agentId]->speed()));
-      street->enqueue(agentId);
+      street->addAgent(agentId);
       ++agentId;
     }
   }
@@ -853,15 +857,22 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              std::unsigned_integral<Delay>)
   class FirstOrderDynamics : public Dynamics<Id, Size, Delay> {
+    double m_speedFluctuationSTD;
+
   public:
     FirstOrderDynamics() = delete;
     /// @brief Construct a new First Order Dynamics object
     /// @param graph, The graph representing the network
-    FirstOrderDynamics(Graph<Id, Size>& graph) : Dynamics<Id, Size, Delay>(graph){};
+    FirstOrderDynamics(Graph<Id, Size>& graph)
+        : Dynamics<Id, Size, Delay>(graph), m_speedFluctuationSTD{0.} {};
     /// @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(Size agentId) 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);
     /// @brief Get the mean speed of a street
     /// @details The mean speed of a street is given by the formula:
     /// \f$ v_{\text{mean}} = v_{\text{max}} \left(1 - \frac{\alpha}{2} \left( n - 1\right)  \right) \f$
@@ -882,10 +893,28 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
              std::unsigned_integral<Delay>)
   void FirstOrderDynamics<Id, Size, Delay>::setAgentSpeed(Size agentId) {
-    const auto& street{
-        this->m_graph.streetSet()[this->m_agents[agentId]->streetId().value()]};
+    const auto& agent{this->m_agents[agentId]};
+    const auto& street{this->m_graph.streetSet()[agent->streetId().value()]};
     double speed{street->maxSpeed() * (1. - this->m_minSpeedRateo * street->density())};
-    this->m_agents[agentId]->setSpeed(speed);
+    if (this->m_speedFluctuationSTD > 0.) {
+      std::normal_distribution<double> speedDist{speed,
+                                                 speed * this->m_speedFluctuationSTD};
+      speed = speedDist(this->m_generator);
+    }
+    speed < 0. ? agent->setSpeed(street->maxSpeed() * (1. - this->m_minSpeedRateo))
+               : agent->setSpeed(speed);
+  }
+
+  template <typename Id, typename Size, typename Delay>
+    requires(std::unsigned_integral<Id> && std::unsigned_integral<Size> &&
+             std::unsigned_integral<Delay>)
+  void FirstOrderDynamics<Id, Size, Delay>::setSpeedFluctuationSTD(
+      double speedFluctuationSTD) {
+    if (speedFluctuationSTD < 0.) {
+      throw std::invalid_argument(
+          buildLog("The speed fluctuation standard deviation must be positive."));
+    }
+    m_speedFluctuationSTD = speedFluctuationSTD;
   }
 
   template <typename Id, typename Size, typename Delay>
@@ -899,11 +928,15 @@ namespace dsm {
     }
     double meanSpeed{0.};
     Size n{0};
-    if (this->m_agents.at(street->queue().front())->delay() > 0) {
+    if (street->queue().size() == 0) {
       n = static_cast<Size>(street->queue().size());
       double alpha{this->m_minSpeedRateo / street->capacity()};
       meanSpeed = street->maxSpeed() * n * (1. - 0.5 * alpha * (n - 1.));
     } else {
+      for (const auto& agentId : street->waitingAgents()) {
+        meanSpeed += this->m_agents.at(agentId)->speed();
+        ++n;
+      }
       for (const auto& agentId : street->queue()) {
         meanSpeed += this->m_agents.at(agentId)->speed();
         ++n;

diff --git a/src/dsm/headers/Street.hpp b/src/dsm/headers/Street.hpp
@@ -16,6 +16,7 @@
 #include <stdexcept>
 #include <cmath>
 #include <numbers>
+#include <set>
 
 #include "Agent.hpp"
 #include "Node.hpp"
@@ -31,7 +32,8 @@ namespace dsm {
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   class Street {
   private:
-    dsm::queue<Size> m_queue;
+    dsm::queue<Size> m_exitQueue;
+    std::set<Id> m_waitingAgents;
     std::pair<Id, Id> m_nodePair;
     double m_len;
     double m_maxSpeed;
@@ -88,7 +90,7 @@ namespace dsm {
     void setLength(double len);
     /// @brief Set the street's queue
     /// @param queue The street's queue
-    void setQueue(dsm::queue<Size> queue) { m_queue = std::move(queue); }
+    void setQueue(dsm::queue<Size> queue) { m_exitQueue = std::move(queue); }
     /// @brief Set the street's node pair
     /// @param node1 The source node of the street
     /// @param node2 The destination node of the street
@@ -129,25 +131,33 @@ namespace dsm {
     /// @brief Get the street's length
     /// @return double, The street's length
     double length() const { return m_len; }
+    /// @brief Get the street's waiting agents
+    /// @return std::set<Id>, The street's waiting agents
+    const std::set<Id>& waitingAgents() const { return m_waitingAgents; }
     /// @brief Get the street's queue
     /// @return dsm::queue<Size>, The street's queue
-    const dsm::queue<Size>& queue() const { return m_queue; }
+    const dsm::queue<Size>& queue() const { return m_exitQueue; }
     /// @brief Get the street's node pair
     /// @return std::pair<Id, Id>, The street's node pair
     const std::pair<Id, Id>& nodePair() const { return m_nodePair; }
     /// @brief Get the street's density
     /// @return double, The street's density
-    double density() const { return static_cast<double>(m_queue.size()) / m_capacity; }
+    double density() const {
+      return static_cast<double>(m_exitQueue.size() + m_waitingAgents.size()) /
+             m_capacity;
+    }
     /// @brief Get the street's speed limit
     /// @return double, The street's speed limit
     double maxSpeed() const { return m_maxSpeed; }
     /// @brief Get the street's angle
     /// @return double The street's angle
     double angle() const { return m_angle; }
+
+    virtual void addAgent(Id agentId);
     /// @brief Add an agent to the street's queue
     /// @param agentId The id of the agent to add to the street's queue
     /// @throw std::runtime_error If the street's queue is full
-    virtual void enqueue(Id agentId);
+    void enqueue(Id agentId);
     /// @brief Remove an agent from the street's queue
     virtual std::optional<Id> dequeue();
     /// @brief Check if the street is a spire
@@ -238,27 +248,41 @@ namespace dsm {
     m_angle = angle;
   }
 
+  template <typename Id, typename Size>
+    requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
+  void Street<Id, Size>::addAgent(Id agentId) {
+    if (m_waitingAgents.contains(agentId)) {
+      throw std::runtime_error(buildLog("The agent is already on the street."));
+    }
+    for (auto const& id : m_exitQueue) {
+      if (id == agentId) {
+        throw std::runtime_error(buildLog("The agent is already on the street."));
+      }
+    }
+    m_waitingAgents.insert(agentId);
+  }
   template <typename Id, typename Size>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   void Street<Id, Size>::enqueue(Id agentId) {
-    if (m_queue.size() == m_capacity) {
-      throw std::runtime_error(buildLog("The street's queue is full."));
+    if (!m_waitingAgents.contains(agentId)) {
+      throw std::runtime_error(buildLog("The agent is not on the street."));
     }
-    for (auto const& id : m_queue) {
+    for (auto const& id : m_exitQueue) {
       if (id == agentId) {
         throw std::runtime_error(buildLog("The agent is already on the street."));
       }
     }
-    m_queue.push(agentId);
+    m_waitingAgents.erase(agentId);
+    m_exitQueue.push(agentId);
   }
   template <typename Id, typename Size>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
   std::optional<Id> Street<Id, Size>::dequeue() {
-    if (m_queue.empty()) {
+    if (m_exitQueue.empty()) {
       return std::nullopt;
     }
-    Id id = m_queue.front();
-    m_queue.pop();
+    Id id = m_exitQueue.front();
+    m_exitQueue.pop();
     return id;
   }
 
@@ -297,7 +321,7 @@ namespace dsm {
     /// @brief Add an agent to the street's queue
     /// @param agentId The id of the agent to add to the street's queue
     /// @throw std::runtime_error If the street's queue is full
-    void enqueue(Id agentId) override;
+    void addAgent(Id agentId) override;
 
     /// @brief Get the input flow of the street
     /// @return Size The input flow of the street
@@ -345,8 +369,8 @@ namespace dsm {
 
   template <typename Id, typename Size>
     requires(std::unsigned_integral<Id> && std::unsigned_integral<Size>)
-  void SpireStreet<Id, Size>::enqueue(Id agentId) {
-    Street<Id, Size>::enqueue(agentId);
+  void SpireStreet<Id, Size>::addAgent(Id agentId) {
+    Street<Id, Size>::addAgent(agentId);
     ++m_agentCounterIn;
   }
 

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.16.0)
 
-project(dsm_tests VERSION 1.3.2 LANGUAGES CXX)
+project(dsm_tests VERSION 1.3.3 LANGUAGES CXX)
 
 # Set the C++ standard
 set(CMAKE_CXX_STANDARD 23)

diff --git a/test/Test_dynamics.cpp b/test/Test_dynamics.cpp
@@ -484,7 +484,8 @@ TEST_CASE("Dynamics") {
     for (const auto& [agentId, agent] : dynamics.agents()) {
       meanSpeed += agent->speed();
     }
-    meanSpeed /= dynamics.graph().streetSet().at(1)->queue().size();
+    meanSpeed /= (dynamics.graph().streetSet().at(1)->queue().size() +
+                  dynamics.graph().streetSet().at(1)->waitingAgents().size());
     CHECK(dynamics.streetMeanSpeed(1).has_value());
     CHECK_EQ(dynamics.streetMeanSpeed(1).value(), meanSpeed);
     CHECK_EQ(dynamics.streetMeanSpeed().mean, dynamics.meanSpeed().mean);