From 5adaeba5db4438949330ab9adac8f54f49dcaeb3 Mon Sep 17 00:00:00 2001 From: grufoony Date: Tue, 14 Apr 2026 11:38:04 +0200 Subject: [PATCH] Now threshold in shortest paths is applied on the whole path, not step-by-step --- src/dsf/bindings.cpp | 3 +- src/dsf/mobility/RoadNetwork.hpp | 307 ++++++++++++++----------------- test/mobility/Test_dynamics.cpp | 2 +- test/mobility/Test_graph.cpp | 65 +++++++ 4 files changed, 205 insertions(+), 172 deletions(-) diff --git a/src/dsf/bindings.cpp b/src/dsf/bindings.cpp index 08065e0e..30abf558 100644 --- a/src/dsf/bindings.cpp +++ b/src/dsf/bindings.cpp @@ -282,7 +282,8 @@ PYBIND11_MODULE(dsf_cpp, m) { " 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" + " threshold (float): Relative tolerance applied to the full " + "source-to-target path cost\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") diff --git a/src/dsf/mobility/RoadNetwork.hpp b/src/dsf/mobility/RoadNetwork.hpp index 5dd5ad9e..36d7fae0 100644 --- a/src/dsf/mobility/RoadNetwork.hpp +++ b/src/dsf/mobility/RoadNetwork.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -244,7 +245,8 @@ namespace dsf::mobility { /// @tparam DynamicsFunc A callable type that takes a const reference to a Street and returns a double representing the edge weight /// @param targetId The id of the target node /// @param getEdgeWeight A callable that takes a const reference to a Street and returns a double representing the edge weight - /// @param threshold A threshold value to consider alternative paths + /// @param threshold Relative tolerance on full path cost from each node to the + /// target /// @return A map where each key is a node id and the value is a vector of next hop node ids toward the target /// @throws std::out_of_range if the target node does not exist template @@ -258,11 +260,12 @@ namespace dsf::mobility { /// @param sourceId The id of the source node /// @param targetId The id of the target node /// @param getEdgeWeight A callable that takes a const reference to a Street and returns a double representing the edge weight - /// @param threshold A threshold value to consider alternative paths + /// @param threshold Relative tolerance on the full source-to-target path cost /// @return A map where each key is a node id and the value is a vector of next hop node ids toward the target. Returns an empty map if no path exists /// @throws std::out_of_range if the source or target node does not exist - /// @details Uses Dijkstra's algorithm to find shortest paths from source to target. - /// Like allPathsTo, this method tracks all equivalent paths within the threshold, allowing for multiple next hops per node. + /// @details Uses Dijkstra's algorithm to compute strict distances to target, then + /// includes only next hops that belong to full source-to-target paths + /// within the threshold budget. template requires(std::is_invocable_r_v) PathCollection shortestPath(Id const sourceId, @@ -353,88 +356,106 @@ namespace dsf::mobility { addStreets(std::forward(streets)...); } + namespace detail { + constexpr double kPathBudgetEpsilon = 1e-9; + + template + requires(std::is_invocable_r_v) + std::unordered_map computeDistancesToTarget( + RoadNetwork const& graph, Id const targetId, DynamicsFunc const& getEdgeWeight) { + if (!graph.nodes().contains(targetId)) { + throw std::out_of_range( + std::format("Target node with id {} does not exist in the graph", targetId)); + } + + std::unordered_map distToTarget; + distToTarget.reserve(graph.nNodes()); + for (auto const& [nodeId, pNode] : graph.nodes()) { + (void)pNode; + distToTarget[nodeId] = std::numeric_limits::infinity(); + } + + std::priority_queue, + std::vector>, + std::greater<>> + pq; + + distToTarget[targetId] = 0.0; + pq.push({0.0, targetId}); + + while (!pq.empty()) { + auto const [currentDist, currentNode] = pq.top(); + pq.pop(); + + if (currentDist > distToTarget[currentNode]) { + continue; + } + + for (auto const& inEdgeId : graph.node(currentNode).ingoingEdges()) { + auto const& inEdge = graph.edge(inEdgeId); + if (inEdge.roadStatus() == RoadStatus::CLOSED) { + continue; + } + + auto const neighborId = inEdge.source(); + auto const candidateDistance = currentDist + getEdgeWeight(inEdge); + if (candidateDistance < distToTarget[neighborId]) { + distToTarget[neighborId] = candidateDistance; + pq.push({candidateDistance, neighborId}); + } + } + } + + return distToTarget; + } + } // namespace detail + template requires(std::is_invocable_r_v) PathCollection RoadNetwork::allPathsTo(Id const targetId, DynamicsFunc f, double const threshold) const { - auto const& nodes = this->nodes(); - - // Distance from each node to the source (going backward) - std::unordered_map distToTarget; - distToTarget.reserve(nNodes()); - // Next hop from each node toward the source - PathCollection nextHopsToTarget; - - // Priority queue: pair (min-heap) - std::priority_queue, - std::vector>, - std::greater<>> - pq; - - // Initialize all nodes with infinite distance - std::for_each(nodes.cbegin(), nodes.cend(), [&](auto const& pair) { - distToTarget[pair.first] = std::numeric_limits::infinity(); - nextHopsToTarget[pair.first] = std::vector(); - }); - - // Target has distance 0 to itself - distToTarget[targetId] = 0.0; - pq.push({0.0, targetId}); - - while (!pq.empty()) { - auto [currentDist, currentNode] = pq.top(); - pq.pop(); - - // Skip if we've already found a better path to this node - if (currentDist > distToTarget[currentNode]) { + auto const distToTarget = detail::computeDistancesToTarget(*this, targetId, f); + PathCollection result; + for (auto const& [nodeId, pNode] : this->nodes()) { + if (nodeId == targetId) { + continue; + } + + auto const nodeDistToTarget = distToTarget.at(nodeId); + if (nodeDistToTarget == std::numeric_limits::infinity()) { continue; } - // Explore all incoming edges (nodes that can reach currentNode) - auto const& inEdges = node(currentNode).ingoingEdges(); - for (auto const& inEdgeId : inEdges) { - // Skip closed roads - if (edge(inEdgeId).roadStatus() == RoadStatus::CLOSED) { + auto const nodeBudget = (1. + threshold) * nodeDistToTarget; + std::vector hops; + hops.reserve(pNode->outgoingEdges().size()); + + for (auto const& outEdgeId : pNode->outgoingEdges()) { + auto const& outEdge = this->edge(outEdgeId); + if (outEdge.roadStatus() == RoadStatus::CLOSED) { continue; } - Id neighborId = edge(inEdgeId).source(); - - // Calculate the weight of the edge from neighbor to currentNode using the dynamics function - double edgeWeight = f(this->edge(inEdgeId)); - double newDistToTarget = distToTarget[currentNode] + edgeWeight; - - // If we found a shorter path from neighborId to source - if (newDistToTarget < distToTarget[neighborId]) { - distToTarget[neighborId] = newDistToTarget; - nextHopsToTarget[neighborId].clear(); - nextHopsToTarget[neighborId].push_back(currentNode); - pq.push({newDistToTarget, neighborId}); + + auto const nextNodeId = outEdge.target(); + auto const nextDistToTarget = distToTarget.at(nextNodeId); + if (nextDistToTarget == std::numeric_limits::infinity()) { + continue; } - // If we found an equally good path, add it as alternative - else if (newDistToTarget < (1. + threshold) * distToTarget[neighborId]) { - spdlog::debug( - "Found alternative path to node {} with distance {:.6f} (existing: {:.6f}) " - "for threshold {:.6f}", - neighborId, - newDistToTarget, - distToTarget[neighborId], - threshold); - // Check if currentNode is not already in the nextHops - auto& hops = nextHopsToTarget[neighborId]; - if (std::find(hops.begin(), hops.end(), currentNode) == hops.end()) { - hops.push_back(currentNode); - } + + // Keep hop transitions acyclic so path expansion remains finite. + if (nextDistToTarget + detail::kPathBudgetEpsilon >= nodeDistToTarget) { + continue; + } + + auto const fullPathCost = f(outEdge) + nextDistToTarget; + if (fullPathCost <= nodeBudget + detail::kPathBudgetEpsilon && + std::find(hops.begin(), hops.end(), nextNodeId) == hops.end()) { + hops.push_back(nextNodeId); } } - } - // Build result: only include reachable nodes (excluding source) - PathCollection result; - for (auto const& [nodeId, hops] : nextHopsToTarget) { - if (nodeId != targetId && - distToTarget[nodeId] != std::numeric_limits::infinity() && - !hops.empty()) { + if (!hops.empty()) { result[nodeId] = hops; } } @@ -462,117 +483,63 @@ namespace dsf::mobility { throw std::out_of_range( std::format("Target node with id {} does not exist in the graph", targetId)); } - auto const& nodes = this->nodes(); - - // Distance from each node to the target (going backward) - std::unordered_map distToTarget; - distToTarget.reserve(nNodes()); - // Next hop from each node toward the target - PathCollection nextHopsToTarget; - - // Priority queue: pair (min-heap) - std::priority_queue, - std::vector>, - std::greater<>> - pq; - - // Initialize all nodes with infinite distance - std::for_each(nodes.cbegin(), nodes.cend(), [&](auto const& pair) { - distToTarget[pair.first] = std::numeric_limits::infinity(); - nextHopsToTarget[pair.first] = std::vector(); - }); - - // Target has distance 0 to itself - distToTarget[targetId] = 0.0; - pq.push({0.0, targetId}); - - while (!pq.empty()) { - auto [currentDist, currentNode] = pq.top(); - pq.pop(); - - // Skip if we've already found a better path to this node - if (currentDist > distToTarget[currentNode]) { - continue; - } - - // If we've reached the source, we can stop early - if (currentNode == sourceId) { - break; - } - - // Explore all incoming edges (nodes that can reach currentNode) - auto const& inEdges = node(currentNode).ingoingEdges(); - for (auto const& inEdgeId : inEdges) { - // Skip closed roads - if (edge(inEdgeId).roadStatus() == RoadStatus::CLOSED) { - continue; - } - Id neighborId = edge(inEdgeId).source(); - - // Calculate the weight of the edge from neighbor to currentNode using the dynamics function - double edgeWeight = f(this->edge(inEdgeId)); - double newDistToTarget = distToTarget[currentNode] + edgeWeight; - - // If we found a shorter path from neighborId to target - if (newDistToTarget < distToTarget[neighborId]) { - distToTarget[neighborId] = newDistToTarget; - nextHopsToTarget[neighborId].clear(); - nextHopsToTarget[neighborId].push_back(currentNode); - pq.push({newDistToTarget, neighborId}); - } - // If we found an equally good path, add it as alternative - else if (newDistToTarget < (1. + threshold) * distToTarget[neighborId]) { - spdlog::debug( - "Found alternative path to node {} with distance {:.6f} (existing: {:.6f}) " - "for threshold {:.6f}", - neighborId, - newDistToTarget, - distToTarget[neighborId], - threshold); - // Check if currentNode is not already in the nextHops - auto& hops = nextHopsToTarget[neighborId]; - if (std::find(hops.begin(), hops.end(), currentNode) == hops.end()) { - hops.push_back(currentNode); - } - } - } - } + auto const distToTarget = detail::computeDistancesToTarget(*this, targetId, f); - // Check if target is reachable from source - if (distToTarget[sourceId] == std::numeric_limits::infinity()) { + auto const sourceBestDistance = distToTarget.at(sourceId); + if (sourceBestDistance == std::numeric_limits::infinity()) { return PathCollection{}; } - // Build result: only include nodes on the path from source to target + auto const sourceBudget = (1. + threshold) * sourceBestDistance; + PathCollection result; - std::unordered_set nodesOnPath; - // Start from source and traverse to target using BFS to find all nodes on valid paths - std::queue queue; - queue.push(sourceId); - nodesOnPath.insert(sourceId); + std::function buildPathsWithinBudget; + buildPathsWithinBudget = [&](Id const currentNode, double const prefixCost) -> bool { + if (currentNode == targetId) { + return true; + } - while (!queue.empty()) { - Id current = queue.front(); - queue.pop(); + auto const currentDistToTarget = distToTarget.at(currentNode); + bool hasValidPathToTarget{false}; - if (current == targetId) { - continue; - } + for (auto const& outEdgeId : this->node(currentNode).outgoingEdges()) { + auto const& outEdge = this->edge(outEdgeId); + if (outEdge.roadStatus() == RoadStatus::CLOSED) { + continue; + } + + auto const nextNodeId = outEdge.target(); + auto const nextDistToTarget = distToTarget.at(nextNodeId); + if (nextDistToTarget == std::numeric_limits::infinity()) { + continue; + } - // Add this node's next hops to the result if they exist - if (nextHopsToTarget.contains(current) && !nextHopsToTarget[current].empty()) { - result[current] = nextHopsToTarget[current]; + // Keep recursion acyclic and guaranteed to converge toward target. + if (nextDistToTarget + detail::kPathBudgetEpsilon >= currentDistToTarget) { + continue; + } + + auto const edgeWeight = f(outEdge); + auto const newPrefixCost = prefixCost + edgeWeight; + auto const optimisticCost = newPrefixCost + nextDistToTarget; + if (optimisticCost > sourceBudget + detail::kPathBudgetEpsilon) { + continue; + } - // Add next hops to the queue if not already visited - for (Id nextHop : nextHopsToTarget[current]) { - if (!nodesOnPath.contains(nextHop)) { - nodesOnPath.insert(nextHop); - queue.push(nextHop); + if (buildPathsWithinBudget(nextNodeId, newPrefixCost)) { + auto& hops = result[currentNode]; + if (std::find(hops.begin(), hops.end(), nextNodeId) == hops.end()) { + hops.push_back(nextNodeId); } + hasValidPathToTarget = true; } } - } + + return hasValidPathToTarget; + }; + + (void)buildPathsWithinBudget(sourceId, 0.0); return result; } diff --git a/test/mobility/Test_dynamics.cpp b/test/mobility/Test_dynamics.cpp index 6f29e645..80571613 100644 --- a/test/mobility/Test_dynamics.cpp +++ b/test/mobility/Test_dynamics.cpp @@ -396,7 +396,7 @@ TEST_CASE("FirstOrderDynamics") { RoadNetwork graph2; graph2.addStreets(s1, s2, s3); FirstOrderDynamics dynamics{graph2, false, 69}; - dynamics.setWeightFunction(dsf::PathWeight::LENGTH); + dynamics.setWeightFunction(dsf::PathWeight::LENGTH, 0.0); WHEN("We add an itinerary and update the paths") { dynamics.addItinerary(std::make_shared(0, 2)); dynamics.updatePaths(); diff --git a/test/mobility/Test_graph.cpp b/test/mobility/Test_graph.cpp index b36be8e8..7fae7ee2 100644 --- a/test/mobility/Test_graph.cpp +++ b/test/mobility/Test_graph.cpp @@ -1299,6 +1299,71 @@ TEST_CASE("ShortestPath") { CHECK(foundPath2); } + SUBCASE("Threshold - deep alternative paths") { + Street s1(0, std::make_pair(0, 1), 3.); + Street s2(1, std::make_pair(1, 2), 3.); + Street s3(2, std::make_pair(2, 4), 3.); + Street s4(3, std::make_pair(0, 3), 5.); + Street s5(4, std::make_pair(3, 4), 5.); + RoadNetwork graph{}; + graph.addStreets(s1, s2, s3, s4, s5); + + auto const& pathMap = + graph.allPathsTo(4, [](auto const& pEdge) { return pEdge.length(); }, 0.7); + CHECK_EQ(pathMap.at(0).size(), 2); + } + + SUBCASE("Threshold - full path budget without accumulation") { + // Best path: 0 -> 1 -> 2 -> 5 = 10.0 + // Alternative: 0 -> 1 -> 3 -> 5 = 10.89 (within 10%) + // Over-budget path: 0 -> 1 -> 3 -> 4 -> 5 = 11.39 (must be excluded) + Street s01(0, std::make_pair(0, 1), 1.0); + Street s12(1, std::make_pair(1, 2), 4.0); + Street s25(2, std::make_pair(2, 5), 5.0); + Street s13(3, std::make_pair(1, 3), 4.89); + Street s35(4, std::make_pair(3, 5), 5.0); + Street s34(5, std::make_pair(3, 4), 0.5); + Street s45(6, std::make_pair(4, 5), 5.0); + + RoadNetwork graph{}; + graph.addStreets(s01, s12, s25, s13, s35, s34, s45); + + auto const pathMap = + graph.shortestPath(0, 5, [](auto const& pEdge) { return pEdge.length(); }, 0.1); + + auto const allPaths = pathMap.explode(0, 5); + CHECK_EQ(allPaths.size(), 2); + + bool foundBestPath{false}; + bool foundValidAlternative{false}; + bool foundOverBudgetPath{false}; + + for (auto const& path : allPaths) { + double totalWeight{0.0}; + for (std::size_t i = 0; i + 1 < path.size(); ++i) { + totalWeight += graph.edge(path[i], path[i + 1]).length(); + } + CHECK_LE(totalWeight, 11.0 + 1e-9); + + if (path.size() == 4 && path[0] == 0 && path[1] == 1 && path[2] == 2 && + path[3] == 5) { + foundBestPath = true; + } + if (path.size() == 4 && path[0] == 0 && path[1] == 1 && path[2] == 3 && + path[3] == 5) { + foundValidAlternative = true; + } + if (path.size() == 5 && path[0] == 0 && path[1] == 1 && path[2] == 3 && + path[3] == 4 && path[4] == 5) { + foundOverBudgetPath = true; + } + } + + CHECK(foundBestPath); + CHECK(foundValidAlternative); + CHECK_FALSE(foundOverBudgetPath); + } + SUBCASE("PathCollection::explode - Many Equivalent Paths") { // Create a grid-like network with many equivalent paths // 0 -> 1 -> 2