Manage multi-lane Traffic Lights (#211)

* Init multi-lane Traffic Lights

* Fix

* Moved new functions in Traffic Light class

* Add first test for left turn probability

* Add second test for left turn probability

* Update version

Author: Grufoony

src/dsm/dsm.hpp

@@ -6,7 +6,7 @@
 
 static constexpr uint8_t DSM_VERSION_MAJOR = 2;
 static constexpr uint8_t DSM_VERSION_MINOR = 1;
-static constexpr uint8_t DSM_VERSION_PATCH = 0;
+static constexpr uint8_t DSM_VERSION_PATCH = 1;
 
 #define DSM_VERSION \
   std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH)

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.hpp

@@ -18,6 +18,7 @@
 #include <set>
 #include <map>
 #include <format>
+#include <cassert>
 
 #include "../utility/Logger.hpp"
 #include "../utility/queue.hpp"
@@ -164,6 +165,7 @@ namespace dsm {
     requires(std::unsigned_integral<Delay>)
   class TrafficLight : public Intersection {
   private:
+    std::optional<std::pair<double, double>> m_leftTurnRatio;
     std::optional<std::pair<Delay, Delay>> m_delay;
     Delay m_counter;
     Delay m_phase;
@@ -198,6 +200,22 @@ namespace dsm {
     /// @param phase The node's phase
     /// @throw std::runtime_error if the delay is not set
     void setPhase(Delay phase);
+    /// @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));
+    }
+    /// @brief Set the node's left turn ratio as std::pair(ratio, ratio)
+    /// @param ratio The left turn ratio
+    inline void setLeftTurnRatio(double const ratio) {
+      setLeftTurnRatio(std::make_pair(ratio, ratio));
+    }
     /// @brief Increase the node's counter
     /// @details This function is used to increase the node's counter
     ///          when the simulation is running. It automatically resets the counter
@@ -213,10 +231,16 @@ namespace dsm {
     /// @return std::optional<Delay> The node's delay
     std::optional<std::pair<Delay, Delay>> delay() const { return m_delay; }
     Delay counter() const { return m_counter; }
+    /// @brief Get the node's left turn ratio
+    /// @return std::optional<std::pair<double, double>> The node's left turn ratio
+    inline std::optional<std::pair<double, double>> leftTurnRatio() const {
+      return m_leftTurnRatio;
+    }
     /// @brief Returns true if the traffic light is green
     /// @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; }
   };
 
@@ -282,6 +306,15 @@ namespace dsm {
     m_phase = phase;
   }
 
+  template <typename Delay>
+    requires(std::unsigned_integral<Delay>)
+  void TrafficLight<Delay>::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);
+  }
+
   template <typename Delay>
     requires(std::unsigned_integral<Delay>)
   void TrafficLight<Delay>::increaseCounter() {
@@ -321,6 +354,33 @@ 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 * (1. - m_leftTurnRatio.value().first);
+      } else {
+        return m_counter > pair.first &&
+               m_counter <
+                   pair.first + pair.second * (1. - 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

test/Test_dynamics.cpp

@@ -568,6 +568,138 @@ TEST_CASE("Dynamics") {
         }
       }
     }
+    GIVEN(
+        "A traffic light managing an intersection with 4 3-lanes streets and 4 1-lane "
+        "streets") {
+      // Streets
+      Street s0_1{1, 1, 30., 15., std::make_pair(0, 1), 3};
+      Street s1_0{5, 1, 30., 15., std::make_pair(1, 0), 3};
+      Street s1_2{7, 1, 30., 15., std::make_pair(1, 2), 3};
+      Street s2_1{11, 1, 30., 15., std::make_pair(2, 1), 3};
+
+      Street s3_1{8, 1, 30., 15., std::make_pair(3, 1)};
+      Street s1_3{16, 1, 30., 15., std::make_pair(1, 3)};
+      Street s4_1{21, 1, 30., 15., std::make_pair(4, 1)};
+      Street s1_4{9, 1, 30., 15., std::make_pair(1, 4)};
+
+      Graph graph2;
+      graph2.addNode(std::make_unique<TrafficLight>(1));
+      graph2.addStreets(s0_1, s1_0, s1_2, s2_1, s3_1, s1_3, s4_1, s1_4);
+      graph2.buildAdj();
+      graph2.adjustNodeCapacities();
+      graph2.normalizeStreetCapacities();
+      auto const& nodes = graph2.nodeSet();
+      auto& tl = dynamic_cast<TrafficLight&>(*nodes.at(1));
+      tl.setDelay(3);
+      tl.setLeftTurnRatio(0.3);
+      tl.setPhase(2);
+      tl.addStreetPriority(1);
+      tl.setCoords({0., 0.});
+      nodes.at(0)->setCoords({-1., 0.});
+      nodes.at(2)->setCoords({1., 0.});
+      nodes.at(3)->setCoords({0., -1.});
+      nodes.at(4)->setCoords({0., 1.});
+      graph2.buildStreetAngles();
+
+      Dynamics dynamics{graph2};
+      dynamics.setSeed(69);
+
+      std::vector<uint32_t> destinationNodes{0, 2, 3, 4};
+      dynamics.setDestinationNodes(destinationNodes);
+
+      CHECK(tl.leftTurnRatio().has_value());
+
+      WHEN("We add agents and make the system evolve") {
+        Agent agent1{0, 2, 0};
+        Agent agent2{1, 4, 0};
+        dynamics.addAgents(agent1, agent2);
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        THEN("The agents are correctly placed") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+        }
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        THEN("The agent 0 passes and agent 1 waits") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+        }
+        dynamics.evolve(false);
+        THEN("The agent 1 passes") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 9);
+        }
+      }
+    }
+    GIVEN(
+        "A traffic light managing an intersection with 4 3-lanes streets and 4 1-lane "
+        "streets") {
+      // Streets
+      Street s0_1{1, 1, 30., 15., std::make_pair(0, 1), 3};
+      Street s1_0{5, 1, 30., 15., std::make_pair(1, 0), 3};
+      Street s1_2{7, 1, 30., 15., std::make_pair(1, 2), 3};
+      Street s2_1{11, 1, 30., 15., std::make_pair(2, 1), 3};
+
+      Street s3_1{8, 1, 30., 15., std::make_pair(3, 1)};
+      Street s1_3{16, 1, 30., 15., std::make_pair(1, 3)};
+      Street s4_1{21, 1, 30., 15., std::make_pair(4, 1)};
+      Street s1_4{9, 1, 30., 15., std::make_pair(1, 4)};
+
+      Graph graph2;
+      graph2.addNode(std::make_unique<TrafficLight>(1));
+      graph2.addStreets(s0_1, s1_0, s1_2, s2_1, s3_1, s1_3, s4_1, s1_4);
+      graph2.buildAdj();
+      graph2.adjustNodeCapacities();
+      graph2.normalizeStreetCapacities();
+      auto const& nodes = graph2.nodeSet();
+      auto& tl = dynamic_cast<TrafficLight&>(*nodes.at(1));
+      tl.setDelay(3);
+      tl.setLeftTurnRatio(0.3);
+      // NO! Now testing red light
+      // tl.setPhase(2);
+      tl.addStreetPriority(21);
+      tl.addStreetPriority(8);
+      tl.setCoords({0., 0.});
+      nodes.at(0)->setCoords({-1., 0.});
+      nodes.at(2)->setCoords({1., 0.});
+      nodes.at(3)->setCoords({0., -1.});
+      nodes.at(4)->setCoords({0., 1.});
+      graph2.buildStreetAngles();
+
+      Dynamics dynamics{graph2};
+      dynamics.setSeed(69);
+
+      std::vector<uint32_t> destinationNodes{0, 2, 3, 4};
+      dynamics.setDestinationNodes(destinationNodes);
+
+      CHECK(tl.leftTurnRatio().has_value());
+
+      WHEN("We add agents and make the system evolve") {
+        Agent agent1{0, 2, 0};
+        Agent agent2{1, 4, 0};
+        dynamics.addAgents(agent1, agent2);
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        THEN("The agents are correctly placed") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 1);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+        }
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        dynamics.evolve(false);
+        THEN("The agent 0 passes and agent 1 waits") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 1);
+        }
+        dynamics.evolve(false);
+        THEN("The agent 1 passes") {
+          CHECK_EQ(dynamics.agents().at(0)->streetId().value(), 7);
+          CHECK_EQ(dynamics.agents().at(1)->streetId().value(), 9);
+        }
+      }
+    }
   }
   SUBCASE("Traffic Lights optimization algorithm") {
     GIVEN("A dynamics object with a traffic light intersection") {