diff --git a/src/dsm/dsm.hpp b/src/dsm/dsm.hpp index cba2b9066..da0c24095 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 = 2; -static constexpr uint8_t DSM_VERSION_PATCH = 0; +static constexpr uint8_t DSM_VERSION_PATCH = 1; static auto const DSM_VERSION = std::format("{}.{}.{}", DSM_VERSION_MAJOR, DSM_VERSION_MINOR, DSM_VERSION_PATCH); diff --git a/src/dsm/headers/DijkstraWeights.cpp b/src/dsm/headers/DijkstraWeights.cpp new file mode 100644 index 000000000..e4af8f820 --- /dev/null +++ b/src/dsm/headers/DijkstraWeights.cpp @@ -0,0 +1,21 @@ + +#include "DijkstraWeights.hpp" +#include "Graph.hpp" + +namespace dsm { + + double streetLength(const Graph* graph, Id node1, Id node2) { + const auto street{graph->street(node1, node2)}; + return (*street)->length(); + } + + double streetTime(const Graph* graph, Id node1, Id node2) { + const auto street{graph->street(node1, node2)}; + const auto length{(*street)->length()}; + const auto speed{(*street)->maxSpeed() * + (1. - (*street)->nAgents() / (*street)->capacity())}; + + return length / speed; + } + +}; // namespace dsm diff --git a/src/dsm/headers/DijkstraWeights.hpp b/src/dsm/headers/DijkstraWeights.hpp new file mode 100644 index 000000000..89639ec4f --- /dev/null +++ b/src/dsm/headers/DijkstraWeights.hpp @@ -0,0 +1,13 @@ + +#pragma once + +#include "../utility/Typedef.hpp" + +namespace dsm { + + class Graph; + + double streetLength(const Graph* graph, Id node1, Id node2); + double streetTime(const Graph* graph, Id node1, Id node2); + +}; // namespace dsm diff --git a/src/dsm/headers/Dynamics.hpp b/src/dsm/headers/Dynamics.hpp index 8ed230c02..e134286d6 100644 --- a/src/dsm/headers/Dynamics.hpp +++ b/src/dsm/headers/Dynamics.hpp @@ -23,6 +23,7 @@ #include #include "Agent.hpp" +#include "DijkstraWeights.hpp" #include "Itinerary.hpp" #include "Graph.hpp" #include "SparseMatrix.hpp" diff --git a/src/dsm/headers/Graph.cpp b/src/dsm/headers/Graph.cpp index 2ff9f208f..75a49e198 100644 --- a/src/dsm/headers/Graph.cpp +++ b/src/dsm/headers/Graph.cpp @@ -525,94 +525,4 @@ namespace dsm { return this->street(nodePair.second, nodePair.first); } - std::optional Graph::shortestPath( - const Intersection& source, const Intersection& destination) const { - return this->shortestPath(source.id(), destination.id()); - } - - std::optional Graph::shortestPath(Id source, Id destination) const { - const Id sourceId{source}; - - std::unordered_set unvisitedNodes; - bool source_found{false}; - bool dest_found{false}; - std::for_each(m_nodes.begin(), - m_nodes.end(), - [&unvisitedNodes, &source_found, &dest_found, source, destination]( - const auto& node) -> void { - if (!source_found && node.first == source) { - source_found = true; - } - if (!dest_found && node.first == destination) { - dest_found = true; - } - unvisitedNodes.emplace(node.first); - }); - if (!source_found || !dest_found) { - return std::nullopt; - } - - const size_t n_nodes{m_nodes.size()}; - auto adj{m_adjacency}; - - std::unordered_set visitedNodes; - std::vector> dist(n_nodes); - std::for_each(dist.begin(), dist.end(), [count = 0](auto& element) mutable -> void { - element.first = count; - element.second = std::numeric_limits::max(); - ++count; - }); - dist[source] = std::make_pair(source, 0.); - - std::vector> prev(n_nodes); - std::for_each(prev.begin(), prev.end(), [](auto& pair) -> void { - pair.first = std::numeric_limits::max(); - pair.second = std::numeric_limits::max(); - }); - prev[source].second = 0.; - - while (unvisitedNodes.size() != 0) { - source = *std::min_element(unvisitedNodes.begin(), - unvisitedNodes.end(), - [&dist](const auto& a, const auto& b) -> bool { - return dist[a].second < dist[b].second; - }); - - unvisitedNodes.erase(source); - visitedNodes.emplace(source); - - const auto& neighbors{adj.getRow(source)}; - for (const auto& neighbour : neighbors) { - // if the node has already been visited, skip it - if (visitedNodes.find(neighbour.first) != visitedNodes.end()) { - continue; - } - double streetLength = (*(this->street(source, neighbour.first)))->length(); - // if current path is shorter than the previous one, update the distance - if (streetLength + dist[source].second < dist[neighbour.first].second) { - dist[neighbour.first].second = streetLength + dist[source].second; - prev[neighbour.first] = std::make_pair(source, dist[neighbour.first].second); - } - } - - adj.emptyColumn(source); - } - - std::vector path{destination}; - Id previous{destination}; - while (true) { - previous = prev[previous].first; - if (previous == std::numeric_limits::max()) { - return std::nullopt; - } - path.push_back(previous); - if (previous == sourceId) { - break; - } - } - - std::reverse(path.begin(), path.end()); - return DijkstraResult(path, prev[destination].second); - } - }; // namespace dsm diff --git a/src/dsm/headers/Graph.hpp b/src/dsm/headers/Graph.hpp index e551444cc..650f7899f 100644 --- a/src/dsm/headers/Graph.hpp +++ b/src/dsm/headers/Graph.hpp @@ -26,6 +26,7 @@ #include #include +#include "DijkstraWeights.hpp" #include "Node.hpp" #include "SparseMatrix.hpp" #include "Street.hpp" @@ -234,13 +235,21 @@ namespace dsm { /// @param source The source node /// @param destination The destination node /// @return A DijkstraResult object containing the path and the distance - std::optional shortestPath(const Intersection& source, - const Intersection& destination) const; + template > + requires(std::is_same_v, double>) + std::optional shortestPath(const Node& source, + const Node& destination, + Func f = streetLength) const; + /// @brief Get the shortest path between two nodes using dijkstra algorithm /// @param source The source node id /// @param destination The destination node id /// @return A DijkstraResult object containing the path and the distance - std::optional shortestPath(Id source, Id destination) const; + template > + requires(std::is_same_v, double>) + std::optional shortestPath(Id source, + Id destination, + Func f = streetLength) const; }; template @@ -283,4 +292,100 @@ namespace dsm { addStreets(std::forward(streets)...); } + template + requires(std::is_same_v, double>) + std::optional Graph::shortestPath(const Node& source, + const Node& destination, + Func f) const { + return this->shortestPath(source.id(), destination.id()); + } + + template + requires(std::is_same_v, double>) + std::optional Graph::shortestPath(Id source, + Id destination, + Func getStreetWeight) const { + const Id sourceId{source}; + + std::unordered_set unvisitedNodes; + bool source_found{false}; + bool dest_found{false}; + std::for_each(m_nodes.begin(), + m_nodes.end(), + [&unvisitedNodes, &source_found, &dest_found, source, destination]( + const auto& node) -> void { + if (!source_found && node.first == source) { + source_found = true; + } + if (!dest_found && node.first == destination) { + dest_found = true; + } + unvisitedNodes.emplace(node.first); + }); + if (!source_found || !dest_found) { + return std::nullopt; + } + + const size_t n_nodes{m_nodes.size()}; + auto adj{m_adjacency}; + + std::unordered_set visitedNodes; + std::vector> dist(n_nodes); + std::for_each(dist.begin(), dist.end(), [count = 0](auto& element) mutable -> void { + element.first = count; + element.second = std::numeric_limits::max(); + ++count; + }); + dist[source] = std::make_pair(source, 0.); + + std::vector> prev(n_nodes); + std::for_each(prev.begin(), prev.end(), [](auto& pair) -> void { + pair.first = std::numeric_limits::max(); + pair.second = std::numeric_limits::max(); + }); + prev[source].second = 0.; + + while (unvisitedNodes.size() != 0) { + source = *std::min_element(unvisitedNodes.begin(), + unvisitedNodes.end(), + [&dist](const auto& a, const auto& b) -> bool { + return dist[a].second < dist[b].second; + }); + + unvisitedNodes.erase(source); + visitedNodes.emplace(source); + + const auto& neighbors{adj.getRow(source)}; + for (const auto& neighbour : neighbors) { + // if the node has already been visited, skip it + if (visitedNodes.find(neighbour.first) != visitedNodes.end()) { + continue; + } + double streetWeight = getStreetWeight(this, source, neighbour.first); + // if current path is shorter than the previous one, update the distance + if (streetWeight + dist[source].second < dist[neighbour.first].second) { + dist[neighbour.first].second = streetWeight + dist[source].second; + prev[neighbour.first] = std::make_pair(source, dist[neighbour.first].second); + } + } + + adj.emptyColumn(source); + } + + std::vector path{destination}; + Id previous{destination}; + while (true) { + previous = prev[previous].first; + if (previous == std::numeric_limits::max()) { + return std::nullopt; + } + path.push_back(previous); + if (previous == sourceId) { + break; + } + } + + std::reverse(path.begin(), path.end()); + return DijkstraResult(path, prev[destination].second); + } }; // namespace dsm