From 1dd671c29893b4f5407af90243199da117b4a61c Mon Sep 17 00:00:00 2001 From: David Schmitz Date: Wed, 10 Sep 2025 20:02:30 +0200 Subject: [PATCH 01/13] implementation contraction hierarchies --- README.md | 4 +- exe/backend/src/http_server.cc | 42 +- exe/extract.cc | 5 +- include/osr/extract/extract.h | 3 +- include/osr/geojson.h | 2 + include/osr/routing/algorithms.h | 2 +- include/osr/routing/contraction_hierarchies.h | 427 ++++++++++++++++++ .../routing/contraction_hierarchies_m_to_n.h | 177 ++++++++ include/osr/routing/dial.h | 65 ++- include/osr/routing/dijkstra.h | 1 + include/osr/routing/profiles/bike_sharing.h | 6 +- include/osr/routing/profiles/car.h | 15 + include/osr/routing/profiles/car_sharing.h | 6 +- include/osr/routing/route.h | 6 + include/osr/ways.h | 44 +- src/extract.cc | 52 ++- src/route.cc | 236 ++++++++++ src/ways.cc | 286 +++++++++++- test/dijkstra_astarbidir_test.cc | 14 +- test/dijkstra_ch_test.cc | 137 ++++++ web/index.html | 43 +- 21 files changed, 1518 insertions(+), 55 deletions(-) create mode 100644 include/osr/routing/contraction_hierarchies.h create mode 100644 include/osr/routing/contraction_hierarchies_m_to_n.h create mode 100644 test/dijkstra_ch_test.cc diff --git a/README.md b/README.md index ef2f6684..c602e98e 100644 --- a/README.md +++ b/README.md @@ -119,8 +119,8 @@ The current set of routing profiles can be found in the [`include/osr/routing/pr - `node` struct: a node defines what is considered a node for this routing profile. The most basic definition of a node would basically be just what's considered a node in the data model (`node_idx_t`, see above). However, for many profiles, this is not succifient. Let's take the foot routing profile as an example: for indoor routing, it should be possible to distinguish the same OSM node on different levels. Therefore, the level has to be part of the foot profile's node definition. Another example is the car profile: to be able to detect u-turns (just changing the direction but staying on the same way), the node has to define the current direction. In order to properly handle turn restrictions, also the last used way has to be part of the node definition. The member function `node::get_key()` returns the key (see `key`). - `label` struct: basically the same as the `node` struct but it should additionally carry the routing costs (currently tracked in seconds). The label has to provide `get_node()` and `cost()` member functions. -- `key`: The shortest path algorithm (e.g. Dijkstra) tracks minimal costs to each node in a hash map. In theory, it would be sufficient to use `node` as hash map key as we need to only track one shortest path to each profile `node`. To allow for a more efficient storage, multiple nodes can share the same hash map key, therefore reducing the hash map size (which can speedup the routing significantly). Therefore, a profile can define a `key` which can be the same as `node` but doesn't have to be. The key doesn't need any member functions and can therefore just be a typedef to `node_idx_t` (in the most simple case). -- `entry`: The entry is the hash map entry and is stored for each `key`. It has to store the costs and precessor. If `key` maps several `node`s to the same `entry`, then the `entry` has to store costs and predecessory for each of the `node`s. It has to provide the following member functions: +- `key`: The shortest path algorithm (e.g. Dijkstra) tracks minimal costs to each node in a hash map. In theory, it would be sufficient to use `node` as hash map key as we need to only track one shortest path to each profile `node`. To allow for a more efficient storage, multiple nodes can share the same hash map key, therefore reducing the hash map size (which can speed up the routing significantly). Therefore, a profile can define a `key` which can be the same as `node` but doesn't have to be. The key doesn't need any member functions and can therefore just be a typedef to `node_idx_t` (in the most simple case). +- `entry`: The entry is the hash map entry and is stored for each `key`. It has to store the costs and predecessor. If `key` maps several `node`s to the same `entry`, then the `entry` has to store costs and predecessors for each of the `node`s. It has to provide the following member functions: - `std::optional pred(node)`: returns the predecessor for the node, or `std::nullopt` if this `node` has never been visited. - `cost_t cost(node)`: returns the shortest path costs to this `node` - `bool update(label, node, cost_t, node pred)`: updates the costs for this node if they are better than the previously stored costs and returns `true` if the costs where updated and `false` otherwise. diff --git a/exe/backend/src/http_server.cc b/exe/backend/src/http_server.cc index 72bc213f..244ed803 100644 --- a/exe/backend/src/http_server.cc +++ b/exe/backend/src/http_server.cc @@ -130,28 +130,34 @@ struct http_server::impl { auto const max = static_cast( max_it == q.end() ? 3600 : max_it->value().as_int64()); - auto const p = route(w_, l_, profile, from, to, max, dir, 100, nullptr, - nullptr, elevations_, routing_algo); + try { + auto const p = route(w_, l_, profile, from, to, max, dir, 100, nullptr, + nullptr, elevations_, routing_algo); - auto const p1 = route(w_, l_, profile, from, std::vector{to}, max, dir, 100, - nullptr, nullptr, elevations_); + auto const p1 = route(w_, l_, profile, from, std::vector{to}, max, dir, 100, + nullptr, nullptr, elevations_); - auto const print = [](char const* name, std::optional const& p) { - if (p.has_value()) { - std::cout << name << " cost: " << p->cost_ << "\n"; - } else { - std::cout << name << ": not found\n"; + auto const print = [](char const* name, std::optional const& p) { + if (p.has_value()) { + std::cout << name << " cost: " << p->cost_ << "\n"; + } else { + std::cout << name << ": not found\n"; + } + }; + print("p", p); + print("p1", p1.at(0)); + + if (!p.has_value()) { + cb(json_response(req, "could not find a valid path", + http::status::not_found)); + return; } - }; - print("p", p); - print("p1", p1.at(0)); - - if (!p.has_value()) { - cb(json_response(req, "could not find a valid path", - http::status::not_found)); - return; + cb(json_response(req, to_featurecollection(w_, p))); + } catch (std::runtime_error const& e) { + cb(json_response(req, boost::json::serialize(json::object{ + {"message", e.what()} + }), http::status::not_found)); } - cb(json_response(req, to_featurecollection(w_, p))); } void handle_levels(web_server::http_req_t const& req, diff --git a/exe/extract.cc b/exe/extract.cc index 86085269..ebe38bc2 100644 --- a/exe/extract.cc +++ b/exe/extract.cc @@ -20,10 +20,11 @@ struct config : public conf::configuration { param(out_, "out,o", "output directory"); param(elevation_data_, "elevation_data,e", "directory with elevation data"); param(with_platforms_, "with_platforms,p", "extract platform info"); + param(with_ch_, "with_contraction_hierarchies,ch", "build CH during extraction"); } std::filesystem::path in_, out_, elevation_data_; - bool with_platforms_{false}; + bool with_platforms_{false}, with_ch_{false}; }; int main(int ac, char const** av) { @@ -45,5 +46,5 @@ int main(int ac, char const** av) { utl::activate_progress_tracker("osr"); auto const silencer = utl::global_progress_bars{false}; - extract(c.with_platforms_, c.in_, c.out_, c.elevation_data_); + extract(c.with_platforms_, c.in_, c.out_, c.elevation_data_, c.with_ch_); } \ No newline at end of file diff --git a/include/osr/extract/extract.h b/include/osr/extract/extract.h index 402f8c91..2cb37272 100644 --- a/include/osr/extract/extract.h +++ b/include/osr/extract/extract.h @@ -5,6 +5,7 @@ namespace osr { void extract(bool with_platforms, std::filesystem::path const& in, std::filesystem::path const& out, - std::filesystem::path const& elevation_dir); + std::filesystem::path const& elevation_dir, + bool with_contraction_hierarchies=false); } // namespace osr \ No newline at end of file diff --git a/include/osr/geojson.h b/include/osr/geojson.h index 5435621b..3cfaf1f7 100644 --- a/include/osr/geojson.h +++ b/include/osr/geojson.h @@ -38,6 +38,7 @@ inline std::string to_featurecollection(ways const& w, bool const with_properties = true) { return boost::json::serialize(boost::json::object{ {"type", "FeatureCollection"}, + {"message", ""}, {"metadata", with_properties ? boost::json::value{{"duration", p->cost_}, {"distance", p->dist_}} : boost::json::value{{}}}, @@ -179,6 +180,7 @@ struct geojson_writer { {"car", p.is_car_accessible()}, {"bike", p.is_bike_accessible()}, {"foot", p.is_walk_accessible()}, + {"importance", p.importance()}, {"is_restricted", w_.r_->node_is_restricted_[n]}, {"is_entrance", p.is_entrance()}, {"is_elevator", p.is_elevator()}, diff --git a/include/osr/routing/algorithms.h b/include/osr/routing/algorithms.h index acc9a211..ee1ce8e7 100644 --- a/include/osr/routing/algorithms.h +++ b/include/osr/routing/algorithms.h @@ -5,7 +5,7 @@ namespace osr { -enum class routing_algorithm : std::uint8_t { kDijkstra, kAStarBi }; +enum class routing_algorithm : std::uint8_t { kDijkstra, kAStarBi, kContractionHierarchies }; routing_algorithm to_algorithm(std::string_view); diff --git a/include/osr/routing/contraction_hierarchies.h b/include/osr/routing/contraction_hierarchies.h new file mode 100644 index 00000000..023bc320 --- /dev/null +++ b/include/osr/routing/contraction_hierarchies.h @@ -0,0 +1,427 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "osr/elevation_storage.h" +#include "osr/routing/additional_edge.h" +#include "osr/routing/dial.h" +#include "osr/types.h" +#include "osr/ways.h" +#include "profiles/car.h" + +namespace osr { + +struct sharing_data; +struct ch_config { + static bool constexpr kUseHeuristic = false; + static float constexpr kHeuristicThresholdFactor = 0.8; + static bool constexpr kUseNodeImportance = true; + static bool constexpr kRemoveCycles = false; +}; + +template +struct contraction_hierarchies { + using profile_t = Profile; + using key = typename Profile::key; + using label = typename Profile::label; + using node = typename Profile::node; + using entry = typename Profile::entry; + using hash = typename Profile::hash; + using cost_map = typename ankerl::unordered_dense::map; + bool static constexpr kDebug = false; + + struct get_bucket { + cost_t operator()(label const& l) { return l.cost(); } + }; + + void clear_mp() { + meet_point_ = meet_point_.invalid(); + best_cost_ = kInfeasible; + } + + void reset(cost_t const max) { + pq_.clear(); + pq_.n_buckets(max + 1U); + assert(max + 1U != 0); // catch overflows + cost1_.clear(); + cost2_.clear(); + pred_edges1_.clear(); + pred_edges2_.clear(); + clear_mp(); + max_reached_1_ = false; + max_reached_2_ = false; + } + + void add_start(label const l) { + if (cost1_[l.get_node().get_key()].update(l, l.get_node(), l.cost(), + node::invalid())) { + pq_.push(l); + } + } + + void add_end(label const l) { + if (cost2_[l.get_node().get_key()].update(l, l.get_node(), l.cost(), + node::invalid())) { + pq_.push(l); + } + } + + template + ankerl::unordered_dense::map& get_cost_map() { + return SearchDir == direction::kForward ? cost1_ : cost2_; + } + + template + cost_t get_cost(node const n) const { + if (SearchDir == direction::kForward) { + auto const it = cost1_.find(n.get_key()); + return it != end(cost1_) ? it->second.cost(n) : kInfeasible; + } else { + auto const it = cost2_.find(n.get_key()); + return it != end(cost2_) ? it->second.cost(n) : kInfeasible; + } + } + + cost_t get_cost_to_mp() const { + assert(meet_point_ != node::invalid()); + auto const f_cost = get_cost(meet_point_); + auto const b_cost = get_cost(meet_point_); + if (f_cost == kInfeasible || b_cost == kInfeasible) { + return kInfeasible; + } + return f_cost + b_cost; + } + + template + bool run([[maybe_unused]] ways const& w, + [[maybe_unused]] ways::routing const& r, + cost_t const max, + [[maybe_unused]] bitvec const* blocked, + [[maybe_unused]] sharing_data const* sharing, + [[maybe_unused]] elevation_storage const* elevations) { + while (!pq_.empty()) { + auto const l = pq_.pop(); + auto const curr = l.get_node(); + auto to_id = [&](node const& n) -> std::int64_t { + auto start = r.node_idx_to_identifier_[n.get_node()]; + for (auto candidate = start; candidate < start + entry::kN; + ++candidate) { + if (n == r.identifier_to_node_[candidate].template conv()) + return candidate; + } + assert(false); + return 0; + }; + auto const curr_id = to_id(curr); + auto const f_cost = get_cost(curr); + auto const b_cost = get_cost(curr); + if (f_cost != kInfeasible && b_cost != kInfeasible) { + auto candidate_cost = f_cost + b_cost; + if (candidate_cost < best_cost_) { + best_cost_ = candidate_cost; + meet_point_ = curr; + } + } + if (f_cost < best_cost_ && f_cost == pq_.current_bucket_) { + for (auto edge_idx : r.upwards_edges_outgoing_[curr_id]) { + auto edge = r.contracted_edges_[edge_idx]; + auto const total = f_cost + edge.cost_; + if (total > max) { + max_reached_1_ = true; + continue; + } + auto neighbor = + r.identifier_to_node_[edge.to_].template conv(); + if (get_cost_map()[neighbor.n_].update(l, neighbor, + static_cast(total), curr)) { + auto next = label{neighbor, static_cast(total)}; + pq_.push(std::move(next)); + pred_edges1_[edge.to_] = edge_idx; + } + } + } + if (b_cost < best_cost_ && b_cost == pq_.current_bucket_) { + for (auto edge_idx : r.downwards_edges_incoming_[curr_id]) { + auto edge = r.contracted_edges_[edge_idx]; + auto const total = b_cost + edge.cost_; + if (total > max) { + max_reached_2_ = true; + continue; + } + auto neighbor = + r.identifier_to_node_[edge.from_].template conv(); + if (get_cost_map()[neighbor.n_].update(l, neighbor, + static_cast(total), curr)) { + auto next = label{neighbor, static_cast(total)}; + pq_.push(std::move(next)); + pred_edges2_[edge.from_] = edge_idx; + } + } + } + } + if (best_cost_ != kInfeasible && best_cost_ > max) { + clear_mp(); + return false; + } + + return !max_reached_1_ || !max_reached_2_; + } + + bool run(ways const& w, + ways::routing const& r, + cost_t const max, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations, + direction const dir) { + if (blocked == nullptr) { + return dir == direction::kForward + ? run(w, r, max, blocked, sharing, + elevations) + : run(w, r, max, blocked, sharing, + elevations); + } else { + return dir == direction::kForward + ? run(w, r, max, blocked, sharing, + elevations) + : run(w, r, max, blocked, sharing, + elevations); + } + } + [[nodiscard]] std::list uncontract_edges( + ways const& w) const { + std::list result; + assert(best_cost_ != kInfeasible); + bool found = false; + ways::routing::node_identifier meet_point_ID = + w.r_->node_idx_to_identifier_[meet_point_.get_node()]; + while (!found && w.r_->identifier_to_node_[meet_point_ID].n_ == + meet_point_.get_node()) { + if (meet_point_ == + w.r_->identifier_to_node_[meet_point_ID].conv()) { + found = true; + } else { + ++meet_point_ID; + } + }; + assert(found); + auto current_ID = meet_point_ID; + while (true) { + auto pred = pred_edges1_.find(current_ID); + if (pred == pred_edges1_.end()) { + break; + } + auto edge_idx = pred->second; + result.push_front(pred->second); + current_ID = w.r_->contracted_edges_[edge_idx].from_; + } + current_ID = meet_point_ID; + while (true) { + auto pred = pred_edges2_.find(current_ID); + if (pred == pred_edges2_.end()) { + break; + } + auto const edge_idx = pred->second; + result.push_back(pred->second); + current_ID = w.r_->contracted_edges_[edge_idx].to_; + } + + // uncontract all edges + for (auto iterator = result.begin(); iterator != result.end();) { + auto const edge_idx = *iterator; + auto edge = w.r_->contracted_edges_[edge_idx]; + if (!edge.is_contracted()) { + ++iterator; + continue; + } + *iterator = edge.contracted_child2_; + result.insert(iterator, edge.contracted_child1_); + --iterator; + } + return result; + } + + dial_ch pq_{get_bucket{}}; + node meet_point_; + cost_t best_cost_{kInfeasible}; + ankerl::unordered_dense::map cost1_; + ankerl::unordered_dense::map cost2_; + std::map + pred_edges1_; + std::map + pred_edges2_; + bool max_reached_1_{false}; + bool max_reached_2_{false}; +}; + +using dial_element_t = std::pair; +struct dial_element_to_bucket { + cost_t operator()(dial_element_t const& el) const { return el.second; } +}; +inline dial ch_dial_{dial_element_to_bucket{}}; + +inline void dijkstra_ch( + ways::routing::node_identifier start, + std::vector const& destinations, + ways::routing& r_, + std::vector> const& outgoing_edges, + [[maybe_unused]] std::vector const& node_to_level, + cost_t const cutoff, + std::uint64_t search_ID, + std::vector>& index_handler, + [[maybe_unused]] ways::routing::node_identifier min_level) { + + ch_dial_.clear(); + ch_dial_.n_buckets(cutoff + 1); + ch_dial_.push(std::make_pair(start, 0U)); + index_handler[start] = std::make_pair(0U, search_ID); + + std::size_t destinations_finished = 0; + + while (!ch_dial_.empty() && destinations_finished < destinations.size()) { + auto [fst, snd] = ch_dial_.pop(); + ways::routing::node_identifier const node_ID = fst; + cost_t const current_cost = snd; + if (auto [cost, seen_ID] = index_handler[node_ID]; + current_cost > cost && search_ID == seen_ID) { // check if the element was already seen + continue; + } + + if (std::ranges::find(destinations, node_ID) != destinations.end()) { + ++destinations_finished; + } + + for (auto const edge_idx : outgoing_edges[node_ID]) { + auto edge = r_.contracted_edges_[edge_idx]; + assert(node_to_level[edge.to_] > min_level); + auto new_cost = current_cost + edge.cost_; + auto [prev_cost, prev_search_ID] = index_handler[edge.to_]; + if (prev_search_ID == search_ID && prev_cost <= new_cost) continue; + if (new_cost > cutoff) continue; + ch_dial_.push(std::make_pair(edge.to_, new_cost)); + index_handler[edge.to_] = std::make_pair(new_cost, search_ID); + } + } + +} + +inline void local_1_hop_ch( + ways::routing::node_identifier start, + std::vector>& outgoing_edges, + std::vector>& incoming_edges, + ways::routing& r_){ + + auto const& out_edges = outgoing_edges[start]; + std::map> neighbor_to_edge_idx_cost; + std::unordered_set out_neighbors; + + for (auto const edge_idx : out_edges) { + auto const& edge = r_.contracted_edges_[edge_idx]; + out_neighbors.insert(edge.to_); + neighbor_to_edge_idx_cost[edge.to_] = std::make_pair(edge_idx, edge.cost_); + } + + auto const& in_edges = incoming_edges[start]; + for (auto const in_edge_idx : in_edges) { + auto remaining_neighbors = out_neighbors; + + auto& in_edge = r_.contracted_edges_[in_edge_idx]; + auto const in_cost = in_edge.cost_; + auto const in_neighbor_ID = in_edge.from_; + auto const& edges = outgoing_edges[in_neighbor_ID]; + + for (auto const edge_idx : edges) { + auto& edge = r_.contracted_edges_[edge_idx]; + if (remaining_neighbors.erase(edge.to_) == 1) { + auto [seen_through, seen_with_cost] = neighbor_to_edge_idx_cost[edge.to_]; + if (seen_with_cost < kInfeasible - in_cost && edge.cost_ > seen_with_cost + in_cost) { + r_.contracted_edges_[edge_idx].cost_ = seen_with_cost + in_cost; + r_.contracted_edges_[edge_idx].contracted_child1_ = in_edge_idx; + r_.contracted_edges_[edge_idx].contracted_child2_ = seen_through; + } + } + } + // introduce shortcuts for all neighbors not seen + for (auto neighbor : remaining_neighbors) { + auto [seen_through, seen_with_cost] = neighbor_to_edge_idx_cost[neighbor]; + if (seen_with_cost >= kInfeasible - in_cost) continue; + if (neighbor == in_neighbor_ID) continue; + auto new_edge = ways::routing::contracted_edge{ + in_edge_idx, + seen_through, + in_neighbor_ID, + neighbor, + static_cast(seen_with_cost + in_cost) + }; + r_.contracted_edges_.emplace_back(new_edge); + auto new_edge_idx = r_.contracted_edges_.size() - 1; + outgoing_edges[in_neighbor_ID].push_back(new_edge_idx); + incoming_edges[neighbor].push_back(new_edge_idx); + } + } +} + +template +[[nodiscard]] +std::pair, std::vector> eliminate_cycles( + ways const& w, std::list const& list_of_edges) { + + std::vector costs; + std::vector profile_nodes; + + auto const start_ID = w.r_->contracted_edges_[list_of_edges.front()].from_; + auto start_node = w.r_->identifier_to_node_[start_ID]; + + profile_nodes.push_back(start_node.conv()); + for (auto const edge_idx : list_of_edges) { + auto edge = w.r_->contracted_edges_[edge_idx]; + profile_nodes.push_back(w.r_->identifier_to_node_[edge.to_].conv()); + costs.push_back(edge.cost_); + } + + if constexpr (!ch_config::kRemoveCycles) { + return std::make_pair(profile_nodes, costs); + } + + for (size_t index = 0; index < profile_nodes.size(); ++index) { + auto it = std::find_if(profile_nodes.rbegin(), profile_nodes.rend(), + [&](auto const& node) { return node.n_ == profile_nodes[index].n_; }); + if (*it == profile_nodes[index]) continue; + + if (it == profile_nodes.rbegin()) { + profile_nodes.resize(index + 1); + costs.resize(index); + return std::make_pair(profile_nodes, costs); + }; + auto dist = std::distance(profile_nodes.begin(), it.base()) - 1; + cost_t sum_of_costs = std::accumulate(costs.begin() + static_cast(index), costs.begin() + dist, 0); + cost_t sum_of_costs_2 = costs[dist] + sum_of_costs; + + Profile::template adjacent(*w.r_, + profile_nodes[index], nullptr, nullptr, nullptr, [&](car::node const tail, + std::uint32_t const cost, distance_t, way_idx_t, std::uint16_t, + std::uint16_t, elevation_storage::elevation, bool) { + if (tail == *it.base() && cost <= sum_of_costs_2) { + assert(cost == sum_of_costs_2); + profile_nodes.erase(profile_nodes.begin() + index + 1, profile_nodes.begin() + dist + 1); + costs.erase(costs.begin() + static_cast(index), costs.begin() + dist); + costs[index] = sum_of_costs_2; + } + if (tail == *it && cost <= sum_of_costs) { + assert(cost == sum_of_costs); + profile_nodes.erase(profile_nodes.begin() + index + 1, profile_nodes.begin() + dist); + costs.erase(costs.begin() + static_cast(index), costs.begin() + (dist - 1)); + costs[index] = sum_of_costs; + } + }); + } + return std::make_pair(profile_nodes, costs); +} + +} // namespace osr + diff --git a/include/osr/routing/contraction_hierarchies_m_to_n.h b/include/osr/routing/contraction_hierarchies_m_to_n.h new file mode 100644 index 00000000..bdaf5793 --- /dev/null +++ b/include/osr/routing/contraction_hierarchies_m_to_n.h @@ -0,0 +1,177 @@ +#pragma once + +#include +#include + +#include "osr/routing/dial.h" +#include "osr/types.h" +#include "osr/ways.h" +#include "profiles/car.h" + +namespace osr { + +template +struct contraction_hierarchies_m_to_n { + using profile_t = Profile; + using key = typename Profile::key; + using label = typename Profile::label; + using node = typename Profile::node; + using entry = typename Profile::entry; + using hash = typename Profile::hash; + using cost_map = typename ankerl::unordered_dense::map; + + struct get_bucket { + cost_t operator()(label const& l) { return l.cost(); } + }; + + + void clear_mp() { + meet_points_.clear(); + best_cost_.clear(); + } + + void reset(cost_t const max) { + pq_.clear(); + pq_.n_buckets(max + 1U); + assert(max + 1U != 0); // catch overflows + cost1_.clear(); + cost2_.clear(); + pred_edges1_.clear(); + pred_edges2_.clear(); + clear_mp(); + max_reached_1_.clear(); + max_reached_2_.clear(); + starts_.clear(); + dests_.clear(); + } + + void add_starts_and_destinations(std::vector