Enhance right of way management

src/dsm/dsm.hpp

@@ -6,7 +6,7 @@
 
 static constexpr uint8_t DSM_VERSION_MAJOR = 2;
 static constexpr uint8_t DSM_VERSION_MINOR = 6;
-static constexpr uint8_t DSM_VERSION_PATCH = 21;
+static constexpr uint8_t DSM_VERSION_PATCH = 22;
 
 static auto const DSM_VERSION =
     std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH);

src/dsm/headers/Road.hpp

@@ -79,6 +79,16 @@ namespace dsm {
     /// @details The forbidden turns are the road ids that are not allowed to be used by the agents
     ///          when they are on the road.
     std::set<Id> const& forbiddenTurns() const;
+    /// @brief Get the road's turn direction given the previous road angle
+    /// @param previousStreetAngle The angle of the previous road
+    /// @return Direction The turn direction
+    /// @details The turn direction is the direction that the agent must take when it is on the road.
+    ///          The possible values are:
+    ///          - UTURN (abs of delta is greater than pi)
+    ///          - STRAIGHT (abs of delta is less than pi /8)
+    ///          - RIGHT (delta is negative and not covered by the above conditions)
+    ///          - LEFT (delta is positive and not covered by the above conditions)
+    Direction turnDirection(double const& previousStreetAngle) const;
 
     virtual int nAgents() const = 0;
     virtual int nMovingAgents() const = 0;

src/dsm/headers/RoadDynamics.hpp

@@ -638,6 +638,98 @@
           Logger::debug(std::format("Green light on street {} and direction {}",
                                     pStreet->id(),
                                     directionToString.at(direction)));
+        } else if (destinationNode->isIntersection()) {
+          auto& intersection = dynamic_cast<Intersection&>(*destinationNode);
+          bool bCanPass{true};
+          auto const& thisDirection{this->graph()
+                                        .edge(pAgentTemp->nextStreetId().value())
+                                        ->turnDirection(pStreet->angle())};
+          if (!intersection.streetPriorities().contains(pStreet->id())) {
+            // I have to check if the agent has right of way
+            auto const& inNeighbours{
+                this->graph().adjacencyMatrix().getCol(destinationNode->id())};
+            for (auto const& sourceId : inNeighbours) {
+              auto const& streetId{sourceId * this->graph().nNodes() +
+                                   destinationNode->id()};
+              if (streetId == pStreet->id()) {
+                continue;
+              }
+              auto const& pStreetTemp{this->graph().edge(streetId)};
+              if (pStreetTemp->nExitingAgents() == 0) {
+                continue;
+              }
+              if (intersection.streetPriorities().contains(streetId)) {
+                Logger::debug(std::format(
+                    "Skipping agent emission from street {} -> {} due to right of way.",
+                    pStreet->source(),
+                    pStreet->target()));
+                bCanPass = false;
+                break;
+              } else if (thisDirection >= Direction::LEFT) {
+                // Check if the agent has right of way using direction
+                // The problem arises only when you have to turn left
+                for (auto i{0}; i < pStreetTemp->nLanes(); ++i) {
+                  // check queue is not empty and take the top agent
+                  if (pStreetTemp->queue(i).empty()) {
+                    continue;
+                  }
+                  auto const& pAgentTemp2{pStreetTemp->queue(i).front()};
+                  if (!pAgentTemp2->streetId().has_value()) {
+                    continue;
+                  }
+                  auto const& otherDirection{
+                      this->graph()
+                          .edge(pAgentTemp2->nextStreetId().value())
+                          ->turnDirection(this->graph()
+                                              .edge(pAgentTemp2->streetId().value())
+                                              ->angle())};
+                  if (otherDirection < Direction::LEFT) {
+                    Logger::debug(std::format(
+                        "Skipping agent emission from street {} -> {} due to right of "
+                        "way with other agents.",
+                        pStreet->source(),
+                        pStreet->target()));
+                    bCanPass = false;
+                    break;
+                  }
+                }
+              }
+            }
+          } else if (thisDirection >= Direction::LEFT) {
+            for (auto const& streetId : intersection.streetPriorities()) {
+              if (streetId == pStreet->id()) {
+                continue;
+              }
+              auto const& pStreetTemp{this->graph().edge(streetId)};
+              for (auto i{0}; i < pStreetTemp->nLanes(); ++i) {
+                // check queue is not empty and take the top agent
+                if (pStreetTemp->queue(i).empty()) {
+                  continue;
+                }
+                auto const& pAgentTemp2{pStreetTemp->queue(i).front()};
+                if (!pAgentTemp2->streetId().has_value()) {
+                  continue;
+                }
+                auto const& otherDirection{
+                    this->graph()
+                        .edge(pAgentTemp2->nextStreetId().value())
+                        ->turnDirection(
+                            this->graph().edge(pAgentTemp2->streetId().value())->angle())};
+                if (otherDirection < thisDirection) {
+                  Logger::debug(std::format(
+                      "Skipping agent emission from street {} -> {} due to right of "
+                      "way with other agents.",
+                      pStreet->source(),
+                      pStreet->target()));
+                  bCanPass = false;
+                  break;
+                }
+              }
+            }
+          }
+          if (!bCanPass) {
+            continue;
+          }
         }
         bool bArrived{false};
         if (!(uniformDist(this->m_generator) < m_passageProbability.value_or(1.1))) {
@@ -1055,10 +1147,10 @@
           tbb::blocked_range<size_t>(0, numNodes, grainSize),
           [&](const tbb::blocked_range<size_t>& range) {
             for (size_t i = range.begin(); i != range.end(); ++i) {
-              const auto& pNode = nodes.at(i);
+              auto const& pNode = nodes.at(i);
               for (auto const& sourceId :
                    this->graph().adjacencyMatrix().getCol(pNode->id())) {
-                auto const streetId = sourceId * N + pNode->id();
+                auto const streetId{sourceId * N + pNode->id()};
                 auto const& pStreet{this->graph().edge(streetId)};
                 if (bUpdateData && pNode->isTrafficLight()) {
                   if (!m_queuesAtTrafficLights.contains(streetId)) {
@@ -1107,10 +1199,16 @@
                     pStreet->enqueue(0);
                     continue;
                   }
-                  auto const deltaAngle{pNextStreet->deltaAngle(pStreet->angle())};
-                  if (std::abs(deltaAngle) < std::numbers::pi) {
-                    // Lanes are counted as 0 is the far right lane
-                    if (std::abs(deltaAngle) < std::numbers::pi / 8) {
+                  auto const direction{pNextStreet->turnDirection(pStreet->angle())};
+                  switch (direction) {
+                    case Direction::UTURN:
+                    case Direction::LEFT:
+                      pStreet->enqueue(nLanes - 1);
+                      break;
+                    case Direction::RIGHT:
+                      pStreet->enqueue(0);
+                      break;
+                    default:
                       std::vector<double> weights;
                       for (auto const& queue : pStreet->exitQueues()) {
                         weights.push_back(1. / (queue.size() + 1));
@@ -1134,13 +1232,6 @@
                       std::discrete_distribution<size_t> laneDist{weights.begin(),
                                                                   weights.end()};
                       pStreet->enqueue(laneDist(this->m_generator));
-                    } else if (deltaAngle < 0.) {    // Right
-                      pStreet->enqueue(0);           // Always the first lane
-                    } else {                         // Left (deltaAngle > 0.)
-                      pStreet->enqueue(nLanes - 1);  // Always the last lane
-                    }
-                  } else {                         // U turn
-                    pStreet->enqueue(nLanes - 1);  // Always the last lane
                   }
                 }
               }

src/dsm/sources/Road.cpp

@@ -4,6 +4,7 @@
 #include <cassert>
 #include <cmath>
 #include <format>
+#include <numbers>
 #include <stdexcept>
 
 namespace dsm {
@@ -65,6 +66,19 @@
     assert(priority >= 0);
     m_priority = priority;
   }
+  Direction Road::turnDirection(double const& previousStreetAngle) const {
+    auto const deltaAngle{this->deltaAngle(previousStreetAngle)};
+    if (std::abs(deltaAngle) >= std::numbers::pi) {
+      return Direction::UTURN;
+    }
+    if (std::abs(deltaAngle) < std::numbers::pi / 8) {
+      return Direction::STRAIGHT;
+    }
+    if (deltaAngle < 0.) {
+      return Direction::RIGHT;
+    }
+    return Direction::LEFT;
+  }
 
   double Road::length() const { return m_length; }
   double Road::maxSpeed() const { return m_maxSpeed; }

test/Test_dynamics.cpp

@@ -1025,7 +1025,7 @@ TEST_CASE("FirstOrderDynamics") {
         dynamics.evolve(false);
         THEN("The agent in A passes first") {
           CHECK_EQ(dynamics.graph().edge(2)->nAgents(), 1);
-          CHECK_EQ(nodeO.agents().size(), 2);
+          CHECK_EQ(nodeO.agents().size(), 1);
           CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 20);
         }
         dynamics.evolve(false);
@@ -1050,7 +1050,7 @@ TEST_CASE("FirstOrderDynamics") {
         dynamics.evolve(false);
         THEN("The agent in D passes first") {
           CHECK_EQ(dynamics.graph().edge(1)->nAgents(), 1);
-          CHECK_EQ(nodeO.agents().size(), 2);
+          CHECK_EQ(nodeO.agents().size(), 1);
           CHECK_EQ(nodeO.agents().begin()->second->streetId().value(), 15);
         }
         dynamics.evolve(false);