Manage multi-lane Traffic Lights

src/dsm/headers/Dynamics.hpp

@@ -475,7 +475,20 @@ namespace dsm {
       }
       if (destinationNode->isTrafficLight()) {
         auto& tl = dynamic_cast<TrafficLight<Delay>&>(*destinationNode);
-        if (!tl.isGreen(streetId)) {
+        if (tl.leftTurnRatio().has_value()) {
+          auto it = m_agentNextStreetId.find(agentId);
+          if (it == m_agentNextStreetId.end()) {
+            if (!tl.isGreen(streetId, 0.)) {
+              continue;
+            }
+          } else {
+            auto const& nextStreet{m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+            auto const delta{nextStreet->deltaAngle(pStreet->angle())};
+            if (!tl.isGreen(streetId, delta)) {
+              continue;
+            }
+          }
+        } else if (!tl.isGreen(streetId)) {
           continue;
         }
       }
@@ -498,20 +511,15 @@ namespace dsm {
         }
         return;
       }
-      const auto& nextStreet{m_graph.streetSet()[m_agentNextStreetId[agentId]]};
+      auto const& nextStreet{m_graph.streetSet()[m_agentNextStreetId[agentId]]};
       if (nextStreet->isFull()) {
         continue;
       }
       pStreet->dequeue(queueIndex);
       assert(destinationNode->id() == nextStreet->nodePair().first);
       if (destinationNode->isIntersection()) {
         auto& intersection = dynamic_cast<Intersection&>(*destinationNode);
-        auto delta = nextStreet->angle() - pStreet->angle();
-        if (delta > std::numbers::pi) {
-          delta -= 2 * std::numbers::pi;
-        } else if (delta < -std::numbers::pi) {
-          delta += 2 * std::numbers::pi;
-        }
+        auto const delta{nextStreet->deltaAngle(pStreet->angle())};
         m_increaseTurnCounts(streetId, delta);
         intersection.addAgent(delta, agentId);
       } else if (destinationNode->isRoundabout()) {
@@ -606,12 +614,7 @@ namespace dsm {
             if (nLanes == 1) {
               street->enqueue(agentId, 0);
             } else {
-              double deltaAngle{pNextStreet->angle() - street->angle()};
-              if (deltaAngle > std::numbers::pi) {
-                deltaAngle -= 2 * std::numbers::pi;
-              } else if (deltaAngle < -std::numbers::pi) {
-                deltaAngle += 2 * std::numbers::pi;
-              }
+              auto const deltaAngle{pNextStreet->deltaAngle(street->angle())};
               if (std::abs(deltaAngle) < std::numbers::pi) {
                 // Lanes are counted as 0 is the far right lane
                 if (deltaAngle < 0.) {                   // Right

src/dsm/headers/Node.cpp

@@ -48,6 +48,13 @@ namespace dsm {
     ++m_agentCounter;
   }
 
+  void Intersection::setLeftTurnRatio(std::pair<double, double> ratio) {
+    assert((void("Left turn ratio components must be between 0 and 1."),
+            ratio.first >= 0. && ratio.first <= 1. && ratio.second >= 0. &&
+                ratio.second <= 1.));
+    m_leftTurnRatio = std::move(ratio);
+  }
+
   void Intersection::removeAgent(Id agentId) {
     assert((void("Trying to remove an agent not on the node"),
             std::erase_if(m_agents, [agentId](const auto& p) {

src/dsm/headers/Node.hpp

@@ -18,6 +18,7 @@
 #include <set>
 #include <map>
 #include <format>
+#include <cassert>
 
 #include "../utility/Logger.hpp"
 #include "../utility/queue.hpp"
@@ -92,6 +93,7 @@ namespace dsm {
     std::set<Id>
         m_streetPriorities;  // A set containing the street ids that have priority - like main roads
     Size m_agentCounter;
+    std::optional<std::pair<double, double>> m_leftTurnRatio;
 
   public:
     Intersection() = default;
@@ -135,6 +137,20 @@ namespace dsm {
     /// @brief Add a street to the node street priorities
     /// @param streetId The street's id
     inline void addStreetPriority(Id streetId) { m_streetPriorities.emplace(streetId); }
+    /// @brief Set the node's left turn ratio
+    /// @param ratio A std::pair containing the left turn ratio
+    /// @details ratio.first * greentime is the green time for left turns while ratio.second * redtime is the red time for left turns
+    /// This is useful for traffic lights when the input street has many lanes and, for example, one resevred for left turns.
+    void setLeftTurnRatio(std::pair<double, double> ratio);
+    /// @brief Set the node's left turn ratio
+    /// @param first The first component of the left turn ratio
+    /// @param second The second component of the left turn ratio
+    inline void setLeftTurnRatio(double const first, double const second) {
+      setLeftTurnRatio(std::make_pair(first, second));
+    }
+    inline void setLeftTurnRatio(double const ratio) {
+      setLeftTurnRatio(std::make_pair(ratio, ratio));
+    }
 
     /// @brief Returns true if the node is full
     /// @return bool True if the node is full
@@ -157,6 +173,10 @@ namespace dsm {
     ///          since the last time this function was called. It also resets the counter.
     Size agentCounter();
 
+    inline std::optional<std::pair<double, double>> leftTurnRatio() const {
+      return m_leftTurnRatio;
+    }
+
     inline virtual bool isIntersection() const noexcept override final { return true; }
   };
 
@@ -217,6 +237,7 @@ namespace dsm {
     /// @return bool True if the traffic light is green
     bool isGreen() const;
     bool isGreen(Id streetId) const;
+    bool isGreen(Id streetId, double angle) const;
     bool isTrafficLight() const noexcept override { return true; }
   };
 
@@ -321,6 +342,32 @@ namespace dsm {
     return !hasPriority;
   }
 
+  template <typename Delay>
+    requires(std::unsigned_integral<Delay>)
+  bool TrafficLight<Delay>::isGreen(Id streetId, double angle) const {
+    assert((void("TrafficLight's delay has not been set."), m_delay.has_value()));
+    assert((void("TrafficLight's left turn ratio has not been set."),
+            m_leftTurnRatio.has_value()));
+    bool const hasPriority{this->streetPriorities().contains(streetId)};
+    auto const pair{m_delay.value()};
+    if (angle > 0.) {
+      if (hasPriority) {
+        return m_counter > pair.first * (1. - m_leftTurnRatio.value().first) &&
+               m_counter < pair.first;
+      } else {
+        return m_counter >
+               pair.first + pair.second * (1. - m_leftTurnRatio.value().second);
+      }
+    } else {
+      if (hasPriority) {
+        return m_counter < pair.first * m_leftTurnRatio.value().first;
+      } else {
+        return m_counter > pair.first &&
+               m_counter < pair.first + pair.second * m_leftTurnRatio.value().second;
+      }
+    }
+  }
+
   /// @brief The Roundabout class represents a roundabout node in the network.
   /// @tparam Id The type of the node's id
   /// @tparam Size The type of the node's capacity

src/dsm/headers/Street.cpp

@@ -161,6 +161,16 @@ namespace dsm {
     return nAgents;
   }
 
+  double Street::deltaAngle(double const previousStreetAngle) const {
+    double deltaAngle{m_angle - previousStreetAngle};
+    if (deltaAngle > std::numbers::pi) {
+      deltaAngle -= 2 * std::numbers::pi;
+    } else if (deltaAngle < -std::numbers::pi) {
+      deltaAngle += 2 * std::numbers::pi;
+    }
+    return deltaAngle;
+  }
+
   SpireStreet::SpireStreet(Id id, const Street& street)
       : Street(id, street), m_agentCounterIn{0}, m_agentCounterOut{0} {}
 

src/dsm/headers/Street.hpp

@@ -195,6 +195,10 @@ namespace dsm {
     /// @brief Get the number of agents on all queues
     /// @return Size The number of agents on all queues
     Size nExitingAgents() const;
+    /// @brief Get the delta angle between the street and the previous street, normalized between -pi and pi
+    /// @param previousStreetAngle The angle of the previous street
+    /// @return double The delta angle between the street and the previous street
+    double deltaAngle(double const previousStreetAngle) const;
 
     virtual void addAgent(Id agentId);
     /// @brief Add an agent to the street's queue