Handle u-turns in updatePaths function (#279)

* Try to fix

* Last fix

* Update version

* Revert

* Less lines

Author: Grufoony

README.md

@@ -36,8 +36,7 @@ pip install -r ./requirements.txt
 ## Installation
 The library can be installed using CMake. To build and install the project in the default folder run:
 ```shell
-cmake -B build -DCMAKE_BUILD_TYPE=Release
-cmake --build build
+cmake -B build -DCMAKE_BUILD_TYPE=Release && cmake --build build
 sudo cmake --install build
 ```
 Otherwise, it is possible to customize the installation path:

src/dsm/dsm.hpp

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

src/dsm/headers/RoadDynamics.hpp

@@ -400,25 +400,25 @@ namespace dsm {
         return;
       }
     }
-
+    Logger::info(std::format("Threshold {}", m_weightTreshold));
     auto const destinationID = pItinerary->destination();
-    std::vector<double> shortestDistances(this->graph().nNodes());
+    std::vector<DijkstraResult> shortestPaths(this->graph().nNodes());
     tbb::parallel_for_each(
         this->graph().nodes().cbegin(),
         this->graph().nodes().cend(),
-        [this, &shortestDistances, &destinationID](auto const& it) -> void {
+        [this, &shortestPaths, &destinationID](auto const& it) -> void {
           auto const nodeId{it.first};
           if (nodeId == destinationID) {
-            shortestDistances[nodeId] = -1.;
+            shortestPaths[nodeId] = DijkstraResult{};
           } else {
             auto result =
                 this->graph().shortestPath(nodeId, destinationID, m_weightFunction);
             if (result.has_value()) {
-              shortestDistances[nodeId] = result.value().distance();
+              shortestPaths[nodeId] = *result;
             } else {
               Logger::warning(std::format(
                   "No path found from node {} to node {}", nodeId, destinationID));
-              shortestDistances[nodeId] = -1.;
+              shortestPaths[nodeId] = DijkstraResult{};
             }
           }
         });
@@ -429,7 +429,7 @@ namespace dsm {
         continue;
       }
       // save the minimum distance between i and the destination
-      const auto minDistance{shortestDistances[nodeId]};
+      const auto minDistance{shortestPaths[nodeId].distance()};
       if (minDistance < 0.) {
         continue;
       }
@@ -451,10 +451,13 @@ namespace dsm {
           }
           continue;
         }
-        auto const distance{shortestDistances[nextNodeId]};
+        auto const distance{shortestPaths[nextNodeId].distance()};
         if (distance < 0.) {
           continue;
         }
+        if (shortestPaths[nextNodeId].path()[1] == nodeId && !node->isRoundabout()) {
+          continue;
+        }
         bool const bIsMinDistance{
             std::abs(m_weightFunction(&this->graph(), nodeId, nextNodeId) + distance -
                      minDistance) <

src/dsm/sources/DijkstraWeights.cpp

@@ -15,7 +15,7 @@ namespace dsm {
       const auto length{(*street)->length()};
       const auto speed{(*street)->maxSpeed() *
                        (1. - (*street)->nAgents() / (*street)->capacity())};
-      return length / speed;
+      return std::ceil(length / speed);
     }
   }  // namespace weight_functions
 

src/dsm/utility/DijkstraResult.hpp

@@ -18,6 +18,7 @@ namespace dsm {
     double m_distance;
 
   public:
+    DijkstraResult() : m_path{}, m_distance{-1.} {}
     /// @brief Construct a new DijkstraResult object
     /// @param path, A vector of the ids of the nodes in the path
     /// @param distance, The distance of the path