From 340b37508d2ee20defc0e3597eefc7b4c0050cfc Mon Sep 17 00:00:00 2001 From: Boon Boonsiri Date: Thu, 14 Aug 2025 15:57:58 -0400 Subject: [PATCH 1/4] SPIKE bidirectional a star with time dependent routing --- src/thor/bidirectional_astar.cc | 61 ++++++++++++++++++++++++- valhalla/midgard/distanceapproximator.h | 2 + valhalla/thor/astarheuristic.h | 2 + valhalla/thor/bidirectional_astar.h | 5 ++ 4 files changed, 68 insertions(+), 2 deletions(-) diff --git a/src/thor/bidirectional_astar.cc b/src/thor/bidirectional_astar.cc index 72c9964057..e9ee8f758b 100644 --- a/src/thor/bidirectional_astar.cc +++ b/src/thor/bidirectional_astar.cc @@ -64,6 +64,7 @@ BidirectionalAStar::BidirectionalAStar(const boost::property_tree::ptree& config pruning_disabled_at_origin_ = false; pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; + reverse_total_time_ = 0.0f; } // Destructor @@ -97,6 +98,7 @@ void BidirectionalAStar::Clear() { pruning_disabled_at_origin_ = false; pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; + reverse_total_time_ = 0.0f; } // Initialize the A* heuristic and adjacency lists for both the forward @@ -396,8 +398,44 @@ void BidirectionalAStar::Expand(baldr::GraphReader& graphreader, // Keep track of superseded edges uint32_t shortcuts = 0; - // Update the time information even if time is invariant to account for timezones - auto seconds_offset = invariant ? 0.f : pred.cost().secs; + // MODIFIED: Calculate time-aware seconds_from_now for bidirectional A* + constexpr bool FORWARD = expansion_direction == ExpansionType::forward; + float seconds_offset = 0.f; + + if (!invariant) { + if (FORWARD) { + // Forward search: Use actual cumulative travel time + seconds_offset = pred.cost().secs; + } else { + // Reverse search: Estimate total route time and work backwards + // This is similar to arrive_by logic - estimate total time and subtract elapsed time + + // Use the pre-calculated total estimated time for reverse search + // This was calculated once in GetBestPath and stored in reverse_total_time_ + float reverse_total_estimated_time = reverse_total_time_; + + // For reverse search: work backwards from total estimated time + // Subtract the cumulative travel time so far from the total estimated time + seconds_offset = reverse_total_estimated_time - pred.cost().secs; + + // Ensure non-negative time + seconds_offset = std::max(0.0f, seconds_offset); + } + } else { + // When invariant = true, we have a specific datetime provided + // We still want time-aware routing, but we need to respect the user's datetime + if (FORWARD) { + // Forward search: Use actual cumulative travel time from the provided datetime + seconds_offset = pred.cost().secs; + } else { + // Reverse search: Estimate total route time and work backwards from the provided datetime + float reverse_total_estimated_time = reverse_total_time_; + seconds_offset = reverse_total_estimated_time - pred.cost().secs; + seconds_offset = std::max(0.0f, seconds_offset); + } + } + + // Create time-aware TimeInfo auto offset_time = FORWARD ? time_info.forward(seconds_offset, static_cast(nodeinfo->timezone())) : time_info.reverse(seconds_offset, static_cast(nodeinfo->timezone())); @@ -515,8 +553,27 @@ BidirectionalAStar::GetBestPath(valhalla::Location& origin, origin.correlation().edges(0).ll().lat()); PointLL destination_new(destination.correlation().edges(0).ll().lng(), destination.correlation().edges(0).ll().lat()); + + + Init(origin_new, destination_new); + // Calculate total estimated time for reverse search (once per route request) + // This is needed for both invariant and non-invariant routes for time-aware routing + // Get origin and destination coordinates + PointLL origin_ll = origin_new; + PointLL dest_ll = destination_new; + + // Calculate total haversine distance + float total_distance = origin_ll.Distance(dest_ll); + + // Estimate total route time: 100 km = 1 hour (3600 seconds) + const float AVERAGE_SPEED_KMH = 100.0f; + const float AVERAGE_SPEED_MS = AVERAGE_SPEED_KMH * 1000.0f / 3600.0f; // m/s + + reverse_total_time_ = total_distance / AVERAGE_SPEED_MS; + reverse_total_time_ = std::max(0.0f, reverse_total_time_); + // we use a non varying time for all time dependent routes until we can figure out how to vary the // time during the path computation in the bidirectional algorithm bool invariant = options.date_time_type() != Options::no_time; diff --git a/valhalla/midgard/distanceapproximator.h b/valhalla/midgard/distanceapproximator.h index 7cfc9a271b..7b2cbc6659 100644 --- a/valhalla/midgard/distanceapproximator.h +++ b/valhalla/midgard/distanceapproximator.h @@ -63,6 +63,8 @@ template class DistanceApproximator { return m_lng_scale_; } + + /** * Approximates the arc distance between the supplied position and the * current test point. It uses the pythagorean theorem with meters diff --git a/valhalla/thor/astarheuristic.h b/valhalla/thor/astarheuristic.h index 0919450567..46ada6923f 100644 --- a/valhalla/thor/astarheuristic.h +++ b/valhalla/thor/astarheuristic.h @@ -77,6 +77,8 @@ class AStarHeuristic { return dist * costfactor_; } + + private: midgard::DistanceApproximator distapprox_; // Distance approximation float costfactor_; // Cost factor - ensures the cost estimate diff --git a/valhalla/thor/bidirectional_astar.h b/valhalla/thor/bidirectional_astar.h index 2da3cf91ad..faed20b0f0 100644 --- a/valhalla/thor/bidirectional_astar.h +++ b/valhalla/thor/bidirectional_astar.h @@ -130,6 +130,11 @@ class BidirectionalAStar : public PathAlgorithm { // edge) bool pruning_disabled_at_origin_, pruning_disabled_at_destination_; + // Total estimated time for reverse search (calculated once per route request) + float reverse_total_time_; + + + /** * Initialize the A* heuristic and adjacency lists for both the forward * and reverse search. From 00abf2adddcdc5a53b9186a2e82f57d3ff379a9f Mon Sep 17 00:00:00 2001 From: Boon Boonsiri Date: Thu, 14 Aug 2025 16:03:28 -0400 Subject: [PATCH 2/4] update --- src/thor/bidirectional_astar.cc | 45 +++++++++---------------- valhalla/midgard/distanceapproximator.h | 2 -- valhalla/thor/astarheuristic.h | 2 -- 3 files changed, 16 insertions(+), 33 deletions(-) diff --git a/src/thor/bidirectional_astar.cc b/src/thor/bidirectional_astar.cc index e9ee8f758b..a5a6c424bb 100644 --- a/src/thor/bidirectional_astar.cc +++ b/src/thor/bidirectional_astar.cc @@ -398,41 +398,28 @@ void BidirectionalAStar::Expand(baldr::GraphReader& graphreader, // Keep track of superseded edges uint32_t shortcuts = 0; - // MODIFIED: Calculate time-aware seconds_from_now for bidirectional A* + // MODIFIED: Calculate time-aware seconds_from_now for bidirectional A* constexpr bool FORWARD = expansion_direction == ExpansionType::forward; float seconds_offset = 0.f; - if (!invariant) { - if (FORWARD) { - // Forward search: Use actual cumulative travel time - seconds_offset = pred.cost().secs; - } else { - // Reverse search: Estimate total route time and work backwards - // This is similar to arrive_by logic - estimate total time and subtract elapsed time + if (FORWARD) { + // Forward search: Always use cumulative travel time + // The TimeInfo object handles the base time (current time or user's datetime) + seconds_offset = pred.cost().secs; + } else { + // Reverse search: Estimate total route time and work backwards + // This is similar to arrive_by logic - estimate total time and subtract elapsed time - // Use the pre-calculated total estimated time for reverse search - // This was calculated once in GetBestPath and stored in reverse_total_time_ - float reverse_total_estimated_time = reverse_total_time_; + // Use the pre-calculated total estimated time for reverse search + // This was calculated once in GetBestPath and stored in reverse_total_time_ + float reverse_total_estimated_time = reverse_total_time_; - // For reverse search: work backwards from total estimated time - // Subtract the cumulative travel time so far from the total estimated time - seconds_offset = reverse_total_estimated_time - pred.cost().secs; + // For reverse search: work backwards from total estimated time + // Subtract the cumulative travel time so far from the total estimated time + seconds_offset = reverse_total_estimated_time - pred.cost().secs; - // Ensure non-negative time - seconds_offset = std::max(0.0f, seconds_offset); - } - } else { - // When invariant = true, we have a specific datetime provided - // We still want time-aware routing, but we need to respect the user's datetime - if (FORWARD) { - // Forward search: Use actual cumulative travel time from the provided datetime - seconds_offset = pred.cost().secs; - } else { - // Reverse search: Estimate total route time and work backwards from the provided datetime - float reverse_total_estimated_time = reverse_total_time_; - seconds_offset = reverse_total_estimated_time - pred.cost().secs; - seconds_offset = std::max(0.0f, seconds_offset); - } + // Ensure non-negative time + seconds_offset = std::max(0.0f, seconds_offset); } // Create time-aware TimeInfo diff --git a/valhalla/midgard/distanceapproximator.h b/valhalla/midgard/distanceapproximator.h index 7b2cbc6659..7cfc9a271b 100644 --- a/valhalla/midgard/distanceapproximator.h +++ b/valhalla/midgard/distanceapproximator.h @@ -63,8 +63,6 @@ template class DistanceApproximator { return m_lng_scale_; } - - /** * Approximates the arc distance between the supplied position and the * current test point. It uses the pythagorean theorem with meters diff --git a/valhalla/thor/astarheuristic.h b/valhalla/thor/astarheuristic.h index 46ada6923f..0919450567 100644 --- a/valhalla/thor/astarheuristic.h +++ b/valhalla/thor/astarheuristic.h @@ -77,8 +77,6 @@ class AStarHeuristic { return dist * costfactor_; } - - private: midgard::DistanceApproximator distapprox_; // Distance approximation float costfactor_; // Cost factor - ensures the cost estimate From b20186f440192c2a61aadb3fd2f7725c0644081a Mon Sep 17 00:00:00 2001 From: Boon Boonsiri Date: Thu, 14 Aug 2025 16:15:03 -0400 Subject: [PATCH 3/4] arrive by --- src/thor/bidirectional_astar.cc | 53 ++++++++++++++++++++--------- valhalla/thor/bidirectional_astar.h | 3 ++ 2 files changed, 40 insertions(+), 16 deletions(-) diff --git a/src/thor/bidirectional_astar.cc b/src/thor/bidirectional_astar.cc index a5a6c424bb..e5bf8b6ba7 100644 --- a/src/thor/bidirectional_astar.cc +++ b/src/thor/bidirectional_astar.cc @@ -65,6 +65,7 @@ BidirectionalAStar::BidirectionalAStar(const boost::property_tree::ptree& config pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; reverse_total_time_ = 0.0f; + date_time_type_ = Options::no_time; } // Destructor @@ -99,6 +100,7 @@ void BidirectionalAStar::Clear() { pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; reverse_total_time_ = 0.0f; + date_time_type_ = Options::no_time; } // Initialize the A* heuristic and adjacency lists for both the forward @@ -398,28 +400,44 @@ void BidirectionalAStar::Expand(baldr::GraphReader& graphreader, // Keep track of superseded edges uint32_t shortcuts = 0; - // MODIFIED: Calculate time-aware seconds_from_now for bidirectional A* + // MODIFIED: Calculate time-aware seconds_from_now for bidirectional A* constexpr bool FORWARD = expansion_direction == ExpansionType::forward; float seconds_offset = 0.f; - if (FORWARD) { - // Forward search: Always use cumulative travel time - // The TimeInfo object handles the base time (current time or user's datetime) - seconds_offset = pred.cost().secs; + // Handle all time scenarios: no time, current time, depart_at, arrive_by + if (date_time_type_ == Options::arrive_by) { + // ARRIVE_BY: User provided arrival time, we need to work backwards + if (FORWARD) { + // Forward search: Estimate departure time and add cumulative travel time + // departure_time = arrival_time - estimated_total_time + float estimated_departure_time = reverse_total_time_; + seconds_offset = estimated_departure_time + pred.cost().secs; + } else { + // Reverse search: Work backwards from arrival time + // The TimeInfo object has the arrival time, subtract cumulative travel time + seconds_offset = pred.cost().secs; + } } else { - // Reverse search: Estimate total route time and work backwards - // This is similar to arrive_by logic - estimate total time and subtract elapsed time + // All other cases: no time, current time, depart_at + if (FORWARD) { + // Forward search: Always use cumulative travel time + // The TimeInfo object handles the base time (current time or user's datetime) + seconds_offset = pred.cost().secs; + } else { + // Reverse search: Estimate total route time and work backwards + // This is similar to arrive_by logic - estimate total time and subtract elapsed time - // Use the pre-calculated total estimated time for reverse search - // This was calculated once in GetBestPath and stored in reverse_total_time_ - float reverse_total_estimated_time = reverse_total_time_; + // Use the pre-calculated total estimated time for reverse search + // This was calculated once in GetBestPath and stored in reverse_total_time_ + float reverse_total_estimated_time = reverse_total_time_; - // For reverse search: work backwards from total estimated time - // Subtract the cumulative travel time so far from the total estimated time - seconds_offset = reverse_total_estimated_time - pred.cost().secs; + // For reverse search: work backwards from total estimated time + // Subtract the cumulative travel time so far from the total estimated time + seconds_offset = reverse_total_estimated_time - pred.cost().secs; - // Ensure non-negative time - seconds_offset = std::max(0.0f, seconds_offset); + // Ensure non-negative time + seconds_offset = std::max(0.0f, seconds_offset); + } } // Create time-aware TimeInfo @@ -561,9 +579,12 @@ BidirectionalAStar::GetBestPath(valhalla::Location& origin, reverse_total_time_ = total_distance / AVERAGE_SPEED_MS; reverse_total_time_ = std::max(0.0f, reverse_total_time_); + // Store date time type for handling all time scenarios + date_time_type_ = options.date_time_type(); + // we use a non varying time for all time dependent routes until we can figure out how to vary the // time during the path computation in the bidirectional algorithm - bool invariant = options.date_time_type() != Options::no_time; + bool invariant = date_time_type_ != Options::no_time; // Get time information for forward and backward searches auto forward_time_info = TimeInfo::make(origin, graphreader, &tz_cache_); auto reverse_time_info = TimeInfo::make(destination, graphreader, &tz_cache_); diff --git a/valhalla/thor/bidirectional_astar.h b/valhalla/thor/bidirectional_astar.h index faed20b0f0..62709b6649 100644 --- a/valhalla/thor/bidirectional_astar.h +++ b/valhalla/thor/bidirectional_astar.h @@ -133,6 +133,9 @@ class BidirectionalAStar : public PathAlgorithm { // Total estimated time for reverse search (calculated once per route request) float reverse_total_time_; + // Date time type for handling all time scenarios (no time, current, depart_at, arrive_by) + Options::DateTimeType date_time_type_; + /** From d8c164c9c1e51c189f65c48b216b2b6566a8ae8e Mon Sep 17 00:00:00 2001 From: Boon Boonsiri Date: Thu, 14 Aug 2025 16:19:06 -0400 Subject: [PATCH 4/4] rename var --- src/thor/bidirectional_astar.cc | 18 +++++++++--------- valhalla/thor/bidirectional_astar.h | 5 +++-- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/src/thor/bidirectional_astar.cc b/src/thor/bidirectional_astar.cc index e5bf8b6ba7..793d3b1b30 100644 --- a/src/thor/bidirectional_astar.cc +++ b/src/thor/bidirectional_astar.cc @@ -64,7 +64,7 @@ BidirectionalAStar::BidirectionalAStar(const boost::property_tree::ptree& config pruning_disabled_at_origin_ = false; pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; - reverse_total_time_ = 0.0f; + total_estimated_time_ = 0.0f; date_time_type_ = Options::no_time; } @@ -99,7 +99,7 @@ void BidirectionalAStar::Clear() { pruning_disabled_at_origin_ = false; pruning_disabled_at_destination_ = false; ignore_hierarchy_limits_ = false; - reverse_total_time_ = 0.0f; + total_estimated_time_ = 0.0f; date_time_type_ = Options::no_time; } @@ -410,7 +410,7 @@ void BidirectionalAStar::Expand(baldr::GraphReader& graphreader, if (FORWARD) { // Forward search: Estimate departure time and add cumulative travel time // departure_time = arrival_time - estimated_total_time - float estimated_departure_time = reverse_total_time_; + float estimated_departure_time = total_estimated_time_; seconds_offset = estimated_departure_time + pred.cost().secs; } else { // Reverse search: Work backwards from arrival time @@ -428,8 +428,8 @@ void BidirectionalAStar::Expand(baldr::GraphReader& graphreader, // This is similar to arrive_by logic - estimate total time and subtract elapsed time // Use the pre-calculated total estimated time for reverse search - // This was calculated once in GetBestPath and stored in reverse_total_time_ - float reverse_total_estimated_time = reverse_total_time_; + // This was calculated once in GetBestPath and stored in total_estimated_time_ + float reverse_total_estimated_time = total_estimated_time_; // For reverse search: work backwards from total estimated time // Subtract the cumulative travel time so far from the total estimated time @@ -563,8 +563,8 @@ BidirectionalAStar::GetBestPath(valhalla::Location& origin, Init(origin_new, destination_new); - // Calculate total estimated time for reverse search (once per route request) - // This is needed for both invariant and non-invariant routes for time-aware routing + // Calculate total estimated time for the entire route (once per route request) + // This is needed for both forward and reverse searches in time-aware routing // Get origin and destination coordinates PointLL origin_ll = origin_new; PointLL dest_ll = destination_new; @@ -576,8 +576,8 @@ BidirectionalAStar::GetBestPath(valhalla::Location& origin, const float AVERAGE_SPEED_KMH = 100.0f; const float AVERAGE_SPEED_MS = AVERAGE_SPEED_KMH * 1000.0f / 3600.0f; // m/s - reverse_total_time_ = total_distance / AVERAGE_SPEED_MS; - reverse_total_time_ = std::max(0.0f, reverse_total_time_); + total_estimated_time_ = total_distance / AVERAGE_SPEED_MS; + total_estimated_time_ = std::max(0.0f, total_estimated_time_); // Store date time type for handling all time scenarios date_time_type_ = options.date_time_type(); diff --git a/valhalla/thor/bidirectional_astar.h b/valhalla/thor/bidirectional_astar.h index 62709b6649..637a673fdc 100644 --- a/valhalla/thor/bidirectional_astar.h +++ b/valhalla/thor/bidirectional_astar.h @@ -130,8 +130,9 @@ class BidirectionalAStar : public PathAlgorithm { // edge) bool pruning_disabled_at_origin_, pruning_disabled_at_destination_; - // Total estimated time for reverse search (calculated once per route request) - float reverse_total_time_; + // Total estimated time for the entire route (calculated once per route request) + // Used for both forward and reverse searches in time-aware routing + float total_estimated_time_; // Date time type for handling all time scenarios (no time, current, depart_at, arrive_by) Options::DateTimeType date_time_type_;