diff --git a/src/dsm/dsm.hpp b/src/dsm/dsm.hpp index cfc288f57..500c1404d 100644 --- a/src/dsm/dsm.hpp +++ b/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) diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp index 4bdcbdcfc..aeed632ad 100644 --- a/src/dsm/headers/Dynamics.hpp +++ b/src/dsm/headers/Dynamics.hpp @@ -475,7 +475,20 @@ namespace dsm { } if (destinationNode->isTrafficLight()) { auto& tl = dynamic_cast&>(*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,7 +511,7 @@ 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; } @@ -506,12 +519,7 @@ namespace dsm { assert(destinationNode->id() == nextStreet->nodePair().first); if (destinationNode->isIntersection()) { auto& intersection = dynamic_cast(*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 diff --git a/src/dsm/headers/Node.hpp b/src/dsm/headers/Node.hpp index 816e108a7..b6973d46c 100644 --- a/src/dsm/headers/Node.hpp +++ b/src/dsm/headers/Node.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "../utility/Logger.hpp" #include "../utility/queue.hpp" @@ -164,6 +165,7 @@ namespace dsm { requires(std::unsigned_integral) class TrafficLight : public Intersection { private: + std::optional> m_leftTurnRatio; std::optional> 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 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 The node's delay std::optional> delay() const { return m_delay; } Delay counter() const { return m_counter; } + /// @brief Get the node's left turn ratio + /// @return std::optional> The node's left turn ratio + inline std::optional> 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 + requires(std::unsigned_integral) + void TrafficLight::setLeftTurnRatio(std::pair 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 requires(std::unsigned_integral) void TrafficLight::increaseCounter() { @@ -321,6 +354,33 @@ namespace dsm { return !hasPriority; } + template + requires(std::unsigned_integral) + bool TrafficLight::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 diff --git a/src/dsm/headers/Street.cpp b/src/dsm/headers/Street.cpp index c5031e391..9d16b59b9 100644 --- a/src/dsm/headers/Street.cpp +++ b/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} {} diff --git a/src/dsm/headers/Street.hpp b/src/dsm/headers/Street.hpp index 580a89b5c..ed2058ce9 100644 --- a/src/dsm/headers/Street.hpp +++ b/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 diff --git a/test/Test_dynamics.cpp b/test/Test_dynamics.cpp index 1e1d1c5a7..2d63eb314 100644 --- a/test/Test_dynamics.cpp +++ b/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(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(*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 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(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(*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 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") {