Add RoadNetwork::shortestPath function (#365)

* Add node-to-node shortestPath function

* Add PathCollection class

* Bump vesrion adn copilot fixes

Author: Grufoony

benchmark/Bench_Network.cpp

@@ -57,7 +57,7 @@ static void BM_RoadNetwork_EdgesLooping(benchmark::State& state) {
     }
   }
 }
-static void BM_RoadNetwork_ShortestPath(benchmark::State& state) {
+static void BM_RoadNetwork_AllPathsTo(benchmark::State& state) {
   dsf::mobility::RoadNetwork network;
   network.importEdges((DATA_FOLDER / "forlì_edges.csv").string());
   network.importNodeProperties((DATA_FOLDER / "forlì_nodes.csv").string());
@@ -68,12 +68,31 @@ static void BM_RoadNetwork_ShortestPath(benchmark::State& state) {
     ++itNode;
   }
 }
+static void BM_RoadNetwork_ShortestPath(benchmark::State& state) {
+  dsf::mobility::RoadNetwork network;
+  network.importEdges((DATA_FOLDER / "forlì_edges.csv").string());
+  network.importNodeProperties((DATA_FOLDER / "forlì_nodes.csv").string());
+  auto itSource = network.nodes().cbegin();
+  auto itTarget = std::next(network.nodes().cbegin(), network.nodes().size() / 2);
+  for (auto _ : state) {
+    auto path = network.shortestPath(itSource->first,
+                                     itTarget->first,
+                                     [](auto const& pEdge) { return pEdge->length(); });
+    benchmark::DoNotOptimize(path);
+    ++itSource;
+    ++itTarget;
+    if (itTarget == network.nodes().cend()) {
+      itTarget = network.nodes().cbegin();
+    }
+  }
+}
 BENCHMARK(BM_RoadNetwork_AddNode);
 BENCHMARK(BM_RoadNetwork_AddEdge);
 BENCHMARK(BM_RoadNetwork_CSVImport);
 BENCHMARK(BM_RoadNetwork_GeoJSONImport);
 BENCHMARK(BM_RoadNetwork_NodesLooping);
 BENCHMARK(BM_RoadNetwork_EdgesLooping);
 BENCHMARK(BM_RoadNetwork_ShortestPath);
+BENCHMARK(BM_RoadNetwork_AllPathsTo);
 
 BENCHMARK_MAIN();

src/dsf/bindings.cpp

@@ -231,7 +231,104 @@ PYBIND11_MODULE(dsf_cpp, m) {
            &dsf::mobility::RoadNetwork::addCoil,
            pybind11::arg("streetId"),
            pybind11::arg("name") = std::string(),
-           dsf::g_docstrings.at("dsf::mobility::RoadNetwork::addCoil").c_str());
+           dsf::g_docstrings.at("dsf::mobility::RoadNetwork::addCoil").c_str())
+      .def(
+          "shortestPath",
+          [](const dsf::mobility::RoadNetwork& self,
+             dsf::Id sourceId,
+             dsf::Id targetId,
+             dsf::PathWeight weightFunction,
+             double threshold) {
+            return self.shortestPath(
+                sourceId,
+                targetId,
+                [weightFunction](const std::unique_ptr<dsf::mobility::Street>& street) {
+                  switch (weightFunction) {
+                    case dsf::PathWeight::LENGTH:
+                      return street->length();
+                    case dsf::PathWeight::TRAVELTIME:
+                      return street->length() / street->maxSpeed();
+                    case dsf::PathWeight::WEIGHT:
+                      return street->weight();
+                    default:
+                      return street->length() / street->maxSpeed();
+                  }
+                },
+                threshold);
+          },
+          pybind11::arg("sourceId"),
+          pybind11::arg("targetId"),
+          pybind11::arg("weightFunction") = dsf::PathWeight::TRAVELTIME,
+          pybind11::arg("threshold") = 1e-9,
+          "Find the shortest path between two nodes using Dijkstra's algorithm.\n\n"
+          "Args:\n"
+          "    sourceId (int): The id of the source node\n"
+          "    targetId (int): The id of the target node\n"
+          "    weightFunction (PathWeight): The weight function to use (LENGTH, "
+          "TRAVELTIME, or WEIGHT)\n"
+          "    threshold (float): A threshold value to consider alternative paths\n\n"
+          "Returns:\n"
+          "    PathCollection: A map where each key is a node id and the value is a "
+          "vector of next hop node ids toward the target");
+
+  pybind11::class_<dsf::mobility::PathCollection>(mobility, "PathCollection")
+      .def(pybind11::init<>(), "Create an empty PathCollection")
+      .def(
+          "__getitem__",
+          [](const dsf::mobility::PathCollection& self, dsf::Id key) {
+            auto it = self.find(key);
+            if (it == self.end()) {
+              throw pybind11::key_error("Key not found");
+            }
+            return it->second;
+          },
+          pybind11::arg("key"),
+          "Get the next hops for a given node id")
+      .def(
+          "__setitem__",
+          [](dsf::mobility::PathCollection& self,
+             dsf::Id key,
+             std::vector<dsf::Id> value) { self[key] = value; },
+          pybind11::arg("key"),
+          pybind11::arg("value"),
+          "Set the next hops for a given node id")
+      .def(
+          "__contains__",
+          [](const dsf::mobility::PathCollection& self, dsf::Id key) {
+            return self.find(key) != self.end();
+          },
+          pybind11::arg("key"),
+          "Check if a node id exists in the collection")
+      .def(
+          "__len__",
+          [](const dsf::mobility::PathCollection& self) { return self.size(); },
+          "Get the number of nodes in the collection")
+      .def(
+          "keys",
+          [](const dsf::mobility::PathCollection& self) {
+            std::vector<dsf::Id> keys;
+            keys.reserve(self.size());
+            for (const auto& [key, _] : self) {
+              keys.push_back(key);
+            }
+            return keys;
+          },
+          "Get all node ids in the collection")
+      .def(
+          "items",
+          [](const dsf::mobility::PathCollection& self) {
+            pybind11::dict items;
+            for (const auto& [key, value] : self) {
+              items[pybind11::int_(key)] = pybind11::cast(value);
+            }
+            return items;
+          },
+          "Get all items (node id, next hops) in the collection")
+      .def("explode",
+           &dsf::mobility::PathCollection::explode,
+           pybind11::arg("sourceId"),
+           pybind11::arg("targetId"),
+           dsf::g_docstrings.at("dsf::mobility::PathCollection::explode").c_str());
 
   pybind11::class_<dsf::mobility::Itinerary>(mobility, "Itinerary")
       .def(pybind11::init<dsf::Id, dsf::Id>(),

src/dsf/dsf.hpp

@@ -6,7 +6,7 @@
 
 static constexpr uint8_t DSF_VERSION_MAJOR = 4;
 static constexpr uint8_t DSF_VERSION_MINOR = 4;
-static constexpr uint8_t DSF_VERSION_PATCH = 0;
+static constexpr uint8_t DSF_VERSION_PATCH = 1;
 
 static auto const DSF_VERSION =
     std::format("{}.{}.{}", DSF_VERSION_MAJOR, DSF_VERSION_MINOR, DSF_VERSION_PATCH);

src/dsf/mobility/Itinerary.cpp

@@ -32,15 +32,13 @@ namespace dsf::mobility {
     inFile.close();
   }
 
-  void Itinerary::setPath(std::unordered_map<Id, std::vector<Id>> path) {
-    m_path = std::move(path);
+  void Itinerary::setPath(PathCollection pathCollection) {
+    m_path = std::move(pathCollection);
   }
 
   Id Itinerary::id() const { return m_id; }
   Id Itinerary::destination() const { return m_destination; }
-  std::unordered_map<Id, std::vector<Id>> const& Itinerary::path() const {
-    return m_path;
-  }
+  PathCollection const& Itinerary::path() const { return m_path; }
   void Itinerary::save(const std::string& fileName) const {
     // Open binary file
     std::ofstream outFile{fileName, std::ios::binary};

src/dsf/mobility/Itinerary.hpp

@@ -8,6 +8,7 @@
 
 #pragma once
 
+#include "PathCollection.hpp"
 #include "../utility/Typedef.hpp"
 
 #include <concepts>
@@ -25,7 +26,7 @@ namespace dsf::mobility {
   private:
     Id m_id;
     Id m_destination;
-    std::unordered_map<Id, std::vector<Id>> m_path;
+    PathCollection m_path;
 
   public:
     /// @brief Construct a new Itinerary object
@@ -42,8 +43,8 @@ namespace dsf::mobility {
     void load(const std::string& fileName);
 
     /// @brief Set the itinerary's path
-    /// @param path An adjacency matrix made by a SparseMatrix representing the itinerary's path
-    void setPath(std::unordered_map<Id, std::vector<Id>> path);
+    /// @param pathCollection A dsf::mobility::PathCollection representing all equivalent paths to the destination
+    void setPath(PathCollection pathCollection);
 
     /// @brief Get the itinerary's id
     /// @return Id, The itinerary's id
@@ -52,9 +53,10 @@ namespace dsf::mobility {
     /// @return Id, The itinerary's destination
     Id destination() const;
     /// @brief Get the itinerary's path
-    /// @return std::unordered_map<Id, std::vector<Id>> const&, The itinerary's path
-    std::unordered_map<Id, std::vector<Id>> const& path() const;
-
+    /// @return PathCollection const&, The itinerary's path
+    PathCollection const& path() const;
+    /// @brief Save the itinerary to a binary file
+    /// @param fileName The name of the file to save the itinerary to
     void save(const std::string& fileName) const;
   };
 };  // namespace dsf::mobility

src/dsf/mobility/PathCollection.cpp

@@ -0,0 +1,39 @@
+#include "PathCollection.hpp"
+
+std::list<std::vector<dsf::Id>> dsf::mobility::PathCollection::explode(
+    Id const sourceId, Id const targetId) const {
+  std::list<std::vector<Id>> paths;
+
+  // Base case: if source equals target, return a path with just the source
+  if (sourceId == targetId) {
+    paths.push_back({sourceId});
+    return paths;
+  }
+
+  // Check if sourceId exists in the map
+  auto it = this->find(sourceId);
+  if (it == this->end()) {
+    return paths;  // No paths available from this source
+  }
+
+  auto const& nextHops = it->second;
+
+  // For each possible next hop from sourceId
+  for (auto const& hop : nextHops) {
+    if (hop == targetId) {
+      // Direct path found
+      paths.push_back({sourceId, targetId});
+    } else {
+      // Recursively find paths from hop to target
+      auto subPaths = explode(hop, targetId);
+
+      // Prepend sourceId to each sub-path
+      for (auto& subPath : subPaths) {
+        subPath.insert(subPath.begin(), sourceId);
+        paths.push_back(std::move(subPath));
+      }
+    }
+  }
+
+  return paths;
+}

src/dsf/mobility/PathCollection.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "../utility/Typedef.hpp"
+
+#include <list>
+#include <unordered_map>
+#include <vector>
+
+namespace dsf::mobility {
+  class PathCollection : public std::unordered_map<Id, std::vector<Id>> {
+  public:
+    using std::unordered_map<Id, std::vector<Id>>::unordered_map;  // Inherit constructors
+
+    /// @brief Explode all possible paths from sourceId to targetId
+    /// @param sourceId The starting point of the paths
+    /// @param targetId The end point of the paths
+    /// @return A list of vectors, each vector representing a path from sourceId to targetId
+    std::list<std::vector<Id>> explode(Id const sourceId, Id const targetId) const;
+  };
+}  // namespace dsf::mobility