diff --git a/exe/backend/src/http_server.cc b/exe/backend/src/http_server.cc index fcf18eb2..ce27e573 100644 --- a/exe/backend/src/http_server.cc +++ b/exe/backend/src/http_server.cc @@ -116,12 +116,36 @@ struct http_server::impl { : to_algorithm(routing_it->value().as_string()); } + static bool get_use_ch_from_request(boost::json::object const& q) { + auto const it = q.find("useCH"); + if (it != q.end()) { + if (it->value().is_bool()) { + return it->value().as_bool(); + } + if (it->value().is_string()) { + auto s = it->value().as_string(); + return s == "1" || s == "true" || s == "True" || s == "yes"; + } + if (it->value().is_int64()) { + return it->value().as_int64() != 0; + } + if (it->value().is_uint64()) { + return it->value().as_uint64() != 0; + } + if (it->value().is_double()) { + return it->value().as_double() != 0.0; + } + } + return false; + } + void handle_route(web_server::http_req_t const& req, web_server::http_res_cb_t const& cb) { auto const q = boost::json::parse(req.body()).as_object(); auto const profile = get_search_profile_from_request(q); auto const direction_it = q.find("direction"); auto const routing_algo = get_routing_algorithm_from_request(q); + auto const use_ch = get_use_ch_from_request(q); auto const dir = to_direction(direction_it == q.end() || !direction_it->value().is_string() ? to_str(direction::kForward) @@ -140,6 +164,7 @@ struct http_server::impl { foot_speed_result.value()} : get_parameters(profile); + // Call overload with CH toggle auto const p = route(params, w_, l_, profile, from, to, max, dir, 100, nullptr, nullptr, elevations_, routing_algo); diff --git a/include/osr/geojson.h b/include/osr/geojson.h index 5435621b..d0ac16eb 100644 --- a/include/osr/geojson.h +++ b/include/osr/geojson.h @@ -179,6 +179,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/preprocessing/ch_preprocessing.h b/include/osr/preprocessing/ch_preprocessing.h new file mode 100644 index 00000000..21c3ddd6 --- /dev/null +++ b/include/osr/preprocessing/ch_preprocessing.h @@ -0,0 +1,447 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "osr/elevation_storage.h" +#include "osr/routing/dijkstra.h" +#include "osr/routing/profile.h" +#include "osr/routing/sharing_data.h" +#include "osr/types.h" +#include "osr/ways.h" + +namespace osr { + +template +struct ch_preprocessor { + using node = typename Profile::node; + using label = typename Profile::label; + using dijkstra_t = dijkstra; + using parameters_t = typename Profile::parameters; + + // Hasher for Profile::node that delegates to Profile::hash using + // node.get_key(). + struct node_hash { + std::size_t operator()(node const& n) const noexcept { + return static_cast(typename Profile::hash{}(n.get_key())); + } + }; + + struct shortcut { + node from_; + node to_; + cost_t cost_; + node via_; + }; + + void run(parameters_t const& params, + ways const& w, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + calculate_node_priorities(w, r, blocked, sharing, elevations); + contract_all_nodes(params, w, r, blocked, sharing, elevations); + } + + void calculate_node_priorities(ways const& w, + ways::routing const& r, + bitvec const* /*blocked*/, + sharing_data const* /*sharing*/, + elevation_storage const* /*elevations*/) { + // Use precomputed node importance directly as priority (lower = contract + // earlier). + node_priorities_.clear(); + for (node_idx_t::value_t i = 0U; i < w.n_nodes(); ++i) { + auto const idx = node_idx_t{i}; + node n{}; + Profile::resolve_all(r, idx, level_t{}, [&](node const& nr) { n = nr; }); + node_priorities_[n] = r.node_properties_[idx].importance(); + } + } + + int calculate_priority(node const& n, + ways const& /*w*/, + ways::routing const& r, + bitvec const* /*blocked*/, + sharing_data const* /*sharing*/, + elevation_storage const* /*elevations*/) { + // Priority is fixed to the node's importance (lower first). + return r.node_properties_[n.get_node()].importance(); + } + + void contract_all_nodes(parameters_t const& params, + ways const& w, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + using priority_pair = std::pair; + struct priority_cmp { + bool operator()(priority_pair const& a, + priority_pair const& b) const noexcept { + return a.first > b.first; // min-heap by priority only + } + }; + std::priority_queue, priority_cmp> + pq; + + for (auto const& [n, priority] : node_priorities_) { + pq.emplace(priority, n); + } + + int level = 0; + std::unordered_set contracted; + while (!pq.empty()) { + auto [priority, n] = pq.top(); + pq.pop(); + + if (contracted.contains(n)) { + continue; + } + + // Re-evaluate priority based on importance (fixed); if it differs, + // requeue. + auto const current_priority = + calculate_priority(n, w, r, blocked, sharing, elevations); + if (current_priority != priority) { + node_priorities_[n] = current_priority; + pq.emplace(current_priority, n); + continue; + } + + auto affected_nodes = + contract_node(params, n, r, w, blocked, sharing, elevations); + contracted.emplace(n); + node_levels_[n] = level++; + + // Update priorities of affected nodes (still based on their importance, + // stable). + for (auto const& affected : affected_nodes) { + if (!contracted.contains(affected)) { + auto new_priority = + calculate_priority(affected, w, r, blocked, sharing, elevations); + node_priorities_[affected] = new_priority; + pq.emplace(new_priority, affected); + } + } + } + } + + bool is_shortcut(parameters_t const& params, + node const& v, + node const& n, + node const& u, + ways const& w, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + auto const cost_vn = + get_cost(params, v, n, r, blocked, sharing, elevations); + auto const cost_nu = + get_cost(params, n, u, r, blocked, sharing, elevations); + if (cost_vn >= kInfeasible || cost_nu >= kInfeasible) { + return false; + } + + auto const shortcut_cost = static_cast(cost_vn + cost_nu); + auto const limit = shortcut_cost >= kInfeasible - 1 + ? kInfeasible + : static_cast(shortcut_cost + 1U); + auto const witness_cost = + local_dijkstra(v, u, w, r, blocked, sharing, elevations, n, limit); + return witness_cost > shortcut_cost; + } + + std::unordered_set contract_node( + parameters_t const& params, + node const& u, + ways::routing const& r, + ways const& w, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + auto in_neighbors = get_neighbors(params, r, u, blocked, sharing, + elevations, direction::kBackward); + auto out_neighbors = get_neighbors(params, r, u, blocked, sharing, + elevations, direction::kForward); + + std::unordered_set affected_nodes; + + for (auto const& v_i : in_neighbors) { + auto const cost_vu = + get_cost(params, v_i, u, r, blocked, sharing, elevations); + if (cost_vu >= kInfeasible) { + continue; + } + affected_nodes.insert(v_i); + + for (auto const& w_j : out_neighbors) { + if (v_i == w_j) { + continue; + } + auto const cost_uw = + get_cost(params, u, w_j, r, blocked, sharing, elevations); + if (cost_uw >= kInfeasible) { + continue; + } + affected_nodes.insert(w_j); + auto const shortcut_cost = static_cast(cost_vu + cost_uw); + auto const limit = shortcut_cost >= kInfeasible - 1 + ? kInfeasible + : static_cast(shortcut_cost + 1U); + if (cost_t const witness = local_dijkstra( + v_i, w_j, w, r, blocked, sharing, elevations, u, limit); + witness > shortcut_cost) { + add_shortcut(u, v_i, w_j, shortcut_cost); + } + } + } + return affected_nodes; + } + + void add_shortcut(node const& via, + node const& from, + node const& to, + cost_t const cost) { + // Store (from -> to). If directed asymmetry is needed, remove reverse + // insertion below. + auto& out = shortcuts_[from]; + bool updated = false; + for (auto& s : out) { + if (s.to_ == to) { + if (cost < s.cost_) { + s.cost_ = cost; + s.via_ = via; + } + updated = true; + break; + } + } + if (!updated) { + out.push_back( + shortcut{.from_ = from, .to_ = to, .cost_ = cost, .via_ = via}); + } + + // Also insert reverse for now (simplifies neighbor queries) + auto& out_rev = shortcuts_[to]; + updated = false; + for (auto& s : out_rev) { + if (s.to_ == from) { + if (cost < s.cost_) { + s.cost_ = cost; + s.via_ = via; + } + updated = true; + break; + } + } + if (!updated) { + out_rev.push_back( + shortcut{.from_ = to, .to_ = from, .cost_ = cost, .via_ = via}); + } + } + + std::vector get_neighbors(parameters_t const& params, + ways::routing const& r, + node const& n, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations, + direction const dir) const { + std::vector neighbors; + + auto collect = [&](auto dir_tag, auto blocked_tag) { + if constexpr (std::is_same_v>) { + if constexpr (blocked_tag.value) { + Profile::template adjacent( + params, r, n, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (cost < kInfeasible) { + neighbors.push_back(neighbor); + } + }); + } else { + Profile::template adjacent( + params, r, n, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (cost < kInfeasible) { + neighbors.push_back(neighbor); + } + }); + } + } else { // backward + if constexpr (blocked_tag.value) { + Profile::template adjacent( + params, r, n, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (cost < kInfeasible) { + neighbors.push_back(neighbor); + } + }); + } else { + Profile::template adjacent( + params, r, n, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (cost < kInfeasible) { + neighbors.push_back(neighbor); + } + }); + } + } + }; + + if (dir == direction::kForward) { + if (blocked == nullptr) { + collect(std::integral_constant{}, + std::false_type{}); + } else { + collect(std::integral_constant{}, + std::true_type{}); + } + } else { + if (blocked == nullptr) { + collect(std::integral_constant{}, + std::false_type{}); + } else { + collect(std::integral_constant{}, + std::true_type{}); + } + } + + // Shortcuts (outgoing). For backward neighbors we rely on reverse insertion + // done in add_shortcut. + if (auto it = shortcuts_.find(n); it != end(shortcuts_)) { + for (auto const& sc : it->second) { + if (blocked != nullptr && blocked->test(sc.to_.get_node())) { + continue; + } + neighbors.push_back(sc.to_); + } + } + return neighbors; + } + + cost_t get_cost(parameters_t const& params, + node const& from, + node const& to, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) const { + cost_t final_cost = kInfeasible; + // forward adjacency only (caller ensures correct direction semantics) + if (blocked == nullptr) { + Profile::template adjacent( + params, r, from, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (neighbor == to && cost < final_cost) { + final_cost = cost; + } + }); + } else { + Profile::template adjacent( + params, r, from, blocked, sharing, elevations, + [&](node const neighbor, cost_t const cost, distance_t, way_idx_t, + std::uint16_t, std::uint16_t, elevation_storage::elevation, + bool) { + if (neighbor == to && cost < final_cost) { + final_cost = cost; + } + }); + } + if (auto it = shortcuts_.find(from); it != end(shortcuts_)) { + for (auto const& sc : it->second) { + if (sc.to_ == to) { + final_cost = std::min(final_cost, sc.cost_); + } + } + } + return final_cost; + } + + cost_t local_dijkstra(node const& from, + node const& to, + ways const& w, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) const { + parameters_t params{}; + dijkstra_t d{}; + d.reset(kInfeasible); + d.add_start(w, label{from, 0U}); + (void)d.run(params, w, r, kInfeasible, blocked, sharing, elevations, + direction::kForward); + return d.get_cost(to); + } + + cost_t local_dijkstra(node const& from, + node const& to, + ways const& w, + ways::routing const& r, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations, + node const& exclude, + cost_t const cost_limit) const { + // Copy blocked bitmap (could be optimized by reusing a shared buffer per + // contraction) + bitvec tmp{w.n_nodes()}; + if (blocked != nullptr) { + for (node_idx_t::value_t i = 0U; i < w.n_nodes(); ++i) { + if (blocked->test(node_idx_t{i})) { + tmp.set(node_idx_t{i}, true); + } + } + } + tmp.set(exclude.get_node(), true); + + parameters_t params{}; + dijkstra_t d{}; + d.reset(cost_limit); + d.add_start(w, label{from, 0U}); + (void)d.run(params, w, r, cost_limit, &tmp, sharing, elevations, + direction::kForward); + return d.get_cost(to); + } + + node get_node(ways::routing const& r, node_idx_t const n) { + node result{}; + Profile::resolve_all(r, n, level_t{}, + [&](node const& node_ref) { result = node_ref; }); + return result; + } + + std::unordered_map node_levels_; + std::unordered_map, node_hash> shortcuts_; + +private: + std::unordered_map node_priorities_; +}; + +// Provide a thread-local CH preprocessor per profile in header so templates can +// use it. +template +inline ch_preprocessor& get_ch_preprocessor() { + static thread_local ch_preprocessor s; + return s; +} + +} // namespace osr diff --git a/include/osr/routing/algorithms.h b/include/osr/routing/algorithms.h index acc9a211..88cf9ecc 100644 --- a/include/osr/routing/algorithms.h +++ b/include/osr/routing/algorithms.h @@ -5,7 +5,11 @@ namespace osr { -enum class routing_algorithm : std::uint8_t { kDijkstra, kAStarBi }; +enum class routing_algorithm : std::uint8_t { + kDijkstra, + kAStarBi, + kBiDijkstra +}; routing_algorithm to_algorithm(std::string_view); diff --git a/include/osr/routing/bidirectional_dijkstra.h b/include/osr/routing/bidirectional_dijkstra.h new file mode 100644 index 00000000..c021154e --- /dev/null +++ b/include/osr/routing/bidirectional_dijkstra.h @@ -0,0 +1,345 @@ +#pragma once +#include "utl/verify.h" + +#include "osr/elevation_storage.h" +#include "osr/preprocessing/ch_preprocessing.h" +#include "osr/routing/dial.h" +#include "osr/routing/profile.h" +#include "osr/routing/sharing_data.h" +#include "osr/types.h" +#include "osr/ways.h" + +namespace osr { + +struct sharing_data; + +template +struct bidirectional_dijkstra { + using profile_t = P; + using key = typename P::key; + using label = typename P::label; + using node = typename P::node; + using entry = typename P::entry; + using hash = typename P::hash; + using cost_map = typename ankerl::unordered_dense::map; + using ch_preprocessor_t = ch_preprocessor

; + + constexpr static auto const kDebug = false; + + bool use_ch_preprocessing_ = false; + ch_preprocessor_t const* ch_pre_ = nullptr; + + struct get_bucket { + cost_t operator()(label const& l) { return l.cost(); } + }; + + void clear_mp() { + meet_point_1_ = meet_point_1_.invalid(); + meet_point_2_ = meet_point_2_.invalid(); + best_cost_ = kInfeasible; + } + + void reset(cost_t const max) { + pq1_.clear(); + pq2_.clear(); + pq1_.n_buckets(max + 1U); + pq2_.n_buckets(max + 1U); + cost1_.clear(); + cost2_.clear(); + clear_mp(); + radius_ = max; + max_reached_1_ = false; + max_reached_2_ = false; + } + + void add_start(ways const& w, label const l, sharing_data const*) { + if (kDebug) { + l.get_node().print(std::cout, w); + std::cout << " starting " << l.get_node().n_ << std::endl; + } + if (l.cost() < pq1_.n_buckets() - 1 && + cost1_[l.get_node().get_key()].update(l, l.get_node(), l.cost(), + node::invalid())) { + pq1_.push(l); + } + } + + void add_end(ways const& w, label const l, sharing_data const*) { + if (kDebug) { + l.get_node().print(std::cout, w); + std::cout << " ending " << l.get_node().n_ << std::endl; + } + if (l.cost() < pq2_.n_buckets() - 1 && + cost2_[l.get_node().get_key()].update(l, l.get_node(), l.cost(), + node::invalid())) { + pq2_.push(l); + } + } + + 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(node const n1, node const n2) const { + auto const f_cost = get_cost(n1); + auto const b_cost = get_cost(n2); + if (f_cost == kInfeasible || b_cost == kInfeasible) { + return kInfeasible; + } + return f_cost + b_cost; + } + + template + bool run_single(typename P::parameters const& params, + ways const& w, + ways::routing const& r, + cost_t const max, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations, + dial& pq, + cost_map& costs) { + if (pq.empty()) { + return false; + } + + auto const is_fwd = PathDir == direction::kForward; + auto const l = pq.pop(); + auto const curr = l.get_node(); + auto const curr_cost = get_cost(curr); + + if (curr_cost < l.cost()) { + return true; + } + + if constexpr (kDebug) { + std::cout << "EXTRACT "; + curr.print(std::cout, w); + std::cout << " cost: " << l.cost() << "\n"; + } + + if (l.cost() >= max) { + if (is_fwd) { + max_reached_1_ = true; + } else { + max_reached_2_ = true; + } + return false; + } + + auto const opposite_cost_map = is_fwd ? &cost2_ : &cost1_; + auto const opposite_candidate = opposite_cost_map->find(curr.get_key()); + if (opposite_candidate != end(*opposite_cost_map)) { + cost_t best_other = kInfeasible; + node best_other_state = node::invalid(); + P::resolve_all(r, curr.get_node(), level_t{0.0F}, [&](node const alt) { + auto const oc = opposite_candidate->second.cost(alt); + if (oc != kInfeasible && oc < best_other) { + best_other = oc; + best_other_state = alt; + } + }); + if (best_other != kInfeasible) { + auto const tentative_cost = curr_cost + best_other; + if (tentative_cost < best_cost_) { + if (is_fwd) { + meet_point_1_ = curr; + meet_point_2_ = best_other_state; + } else { + meet_point_1_ = best_other_state; + meet_point_2_ = curr; + } + best_cost_ = static_cast(tentative_cost); + + if constexpr (kDebug) { + std::cout << " MEETING POINT FOUND! Cost: " << best_cost_ << "\n"; + } + } + } + } + + if (use_ch_preprocessing_ && ch_pre_ != nullptr) { + auto neighbors = ch_pre_->get_neighbors( + params, r, curr, blocked, sharing, elevations, + SearchDir == direction::kForward ? direction::kForward + : direction::kBackward); + for (auto const& neighbor : neighbors) { + cost_t edge_cost = ch_pre_->get_cost(params, curr, neighbor, r, blocked, + sharing, elevations); + if (edge_cost >= kInfeasible) continue; + + auto const total = curr_cost + edge_cost; + if (total >= max) { + if (is_fwd) { + max_reached_1_ = true; + } else { + max_reached_2_ = true; + } + continue; + } + + if (total < max && costs[neighbor.get_key()].update( + l, neighbor, static_cast(total), curr)) { + auto next = label{neighbor, static_cast(total)}; + next.track(l, r, way_idx_t{}, neighbor.get_node(), false); + pq.push(std::move(next)); + + if constexpr (kDebug) { + std::cout << " -> PUSH (CH) neighbor cost: " << total << "\n"; + } + } + } + } else { + P::template adjacent( + params, r, curr, blocked, sharing, elevations, + [&](node const neighbor, std::uint32_t const edge_cost, distance_t, + way_idx_t const way, std::uint16_t, std::uint16_t, + elevation_storage::elevation const, bool const track) { + if constexpr (kDebug) { + std::cout << " NEIGHBOR "; + neighbor.print(std::cout, w); + } + + auto const total = curr_cost + edge_cost; + if (total >= max) { + if (is_fwd) { + max_reached_1_ = true; + } else { + max_reached_2_ = true; + } + return; + } + + if (total < max && + costs[neighbor.get_key()].update( + l, neighbor, static_cast(total), curr)) { + auto next = label{neighbor, static_cast(total)}; + next.track(l, r, way, neighbor.get_node(), track); + pq.push(std::move(next)); + + if constexpr (kDebug) { + std::cout << " -> PUSH cost: " << total << "\n"; + } + } else { + if constexpr (kDebug) { + std::cout << " -> DOMINATED\n"; + } + } + }); + } + + if (best_cost_ != kInfeasible) { + auto const top_f = + pq1_.empty() ? kInfeasible + : pq1_.buckets_[pq1_.get_next_bucket()].empty() + ? kInfeasible + : pq1_.buckets_[pq1_.get_next_bucket()].back().cost(); + auto const top_r = + pq2_.empty() ? kInfeasible + : pq2_.buckets_[pq2_.get_next_bucket()].empty() + ? kInfeasible + : pq2_.buckets_[pq2_.get_next_bucket()].back().cost(); + + if (top_f + top_r >= best_cost_) { + return false; + } + } + + return true; + } + + template + bool run(typename P::parameters const& params, + ways const& w, + ways::routing const& r, + cost_t const max, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + if (radius_ == max) { /* normal case - radius_==max always */ + } + while (!pq1_.empty() || !pq2_.empty()) { + if (!pq1_.empty() && + !run_single( + params, w, r, max, blocked, sharing, elevations, pq1_, cost1_)) { + break; + } + if (!pq2_.empty() && + !run_single( + params, w, r, max, blocked, sharing, elevations, pq2_, cost2_)) { + break; + } + } + if (best_cost_ != kInfeasible && best_cost_ > max) { + clear_mp(); + return false; + } + return !max_reached_1_ || !max_reached_2_; + } + + bool run(typename P::parameters const& params, + 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 (use_ch_preprocessing_ && ch_pre_ != nullptr) { + if (blocked == nullptr) { + return dir == direction::kForward + ? run(params, w, r, max, blocked, + sharing, elevations) + : run( + params, w, r, max, blocked, sharing, elevations); + } else { + return dir == direction::kForward + ? run(params, w, r, max, blocked, + sharing, elevations) + : run(params, w, r, max, blocked, + sharing, elevations); + } + } + if (blocked == nullptr) { + return dir == direction::kForward + ? run(params, w, r, max, blocked, + sharing, elevations) + : run(params, w, r, max, blocked, + sharing, elevations); + } else { + return dir == direction::kForward + ? run(params, w, r, max, blocked, + sharing, elevations) + : run(params, w, r, max, blocked, + sharing, elevations); + } + } + + explicit bidirectional_dijkstra(bool use_ch_preprocessing = false, + ch_preprocessor_t const* ch_pre = nullptr) + : use_ch_preprocessing_{use_ch_preprocessing}, ch_pre_{ch_pre} {} + + void set_ch_preprocessor(ch_preprocessor_t const* ch_pre) { + ch_pre_ = ch_pre; + } + + dial pq1_{get_bucket{}}; + dial pq2_{get_bucket{}}; + node meet_point_1_; + node meet_point_2_; + cost_t best_cost_{kInfeasible}; + cost_map cost1_; + cost_map cost2_; + cost_t radius_{}; + bool max_reached_1_{false}; + bool max_reached_2_{false}; +}; + +} // namespace osr diff --git a/include/osr/routing/profiles/bike.h b/include/osr/routing/profiles/bike.h index 7db21d03..e68229cf 100644 --- a/include/osr/routing/profiles/bike.h +++ b/include/osr/routing/profiles/bike.h @@ -206,8 +206,8 @@ struct bike { : 0); auto const cost = way_cost(params, target_way_prop, way_dir, dist) + node_cost(target_node_prop) + elevation_cost; - fn(node{target_node, way_dir}, static_cast(cost), dist, - way, from, to, elevation, false); + fn(node{target_node, way_dir}, static_cast(cost), dist, way, + from, to, elevation, false); }; if (i != 0U) { diff --git a/include/osr/routing/profiles/bike_sharing.h b/include/osr/routing/profiles/bike_sharing.h index 778e3ba2..c6f21f54 100644 --- a/include/osr/routing/profiles/bike_sharing.h +++ b/include/osr/routing/profiles/bike_sharing.h @@ -47,7 +47,8 @@ struct bike_sharing { .is_sidewalk_separate_ = false, .motor_vehicle_no_ = false, .has_toll_ = false, - .is_big_street_ = false}; + .is_big_street_ = false, + .importance_ = 0}; static constexpr auto const kAdditionalNodeProperties = node_properties{.from_level_ = 0, @@ -58,7 +59,8 @@ struct bike_sharing { .is_entrance_ = false, .is_multi_level_ = false, .is_parking_ = false, - .to_level_ = 0}; + .to_level_ = 0, + .importance_ = 0}; enum class node_type : std::uint8_t { kInitialFoot, @@ -299,7 +301,8 @@ struct bike_sharing { std::uint16_t const from, std::uint16_t const to, elevation_storage::elevation const elevation, bool) { if (is_allowed(sharing->through_allowed_, neighbor.n_)) { - fn(to_node(neighbor, nt), cost + switch_penalty, dist, way, from, + fn(to_node(neighbor, nt), + static_cast(cost + switch_penalty), dist, way, from, to, elevation, false); } }); @@ -327,8 +330,9 @@ struct bike_sharing { std::uint16_t const from, std::uint16_t const to, elevation_storage::elevation const elevation, bool) { if (is_allowed(sharing->through_allowed_, neighbor.n_)) { - fn(to_node(neighbor, kNoLevel), cost + switch_penalty, dist, way, - from, to, elevation, false); + fn(to_node(neighbor, kNoLevel), + static_cast(cost + switch_penalty), dist, way, from, + to, elevation, false); } }); if (include_additional_edges) { diff --git a/include/osr/routing/profiles/car.h b/include/osr/routing/profiles/car.h index 05ca4c8c..838d61f8 100644 --- a/include/osr/routing/profiles/car.h +++ b/include/osr/routing/profiles/car.h @@ -206,8 +206,8 @@ struct car { auto const cost = way_cost(params, target_way_prop, way_dir, dist) + node_cost(target_node_prop) + (is_u_turn ? kUturnPenalty : 0U); - fn(target, cost, dist, way, from, to, elevation_storage::elevation{}, - false); + fn(target, static_cast(cost), dist, way, from, to, + elevation_storage::elevation{}, false); }; if (i != 0U) { diff --git a/include/osr/routing/profiles/car_parking.h b/include/osr/routing/profiles/car_parking.h index f7431456..0fd2c341 100644 --- a/include/osr/routing/profiles/car_parking.h +++ b/include/osr/routing/profiles/car_parking.h @@ -268,8 +268,9 @@ struct car_parking { std::uint16_t const from, std::uint16_t const to, elevation_storage::elevation const elevation, bool) { fn(to_node(neighbor), - cost + (n.is_foot_node() ? 0 : kSwitchPenalty), dist, way, from, - to, elevation, false); + static_cast(cost + + (n.is_foot_node() ? 0 : kSwitchPenalty)), + dist, way, from, to, elevation, false); }); } @@ -282,8 +283,9 @@ struct car_parking { elevation_storage::elevation const elevation, bool) { auto const way_prop = w.way_properties_[way]; fn(to_node(neighbor, way_prop.from_level()), - cost + (n.is_car_node() ? 0 : kSwitchPenalty), dist, way, from, - to, elevation, false); + static_cast(cost + + (n.is_car_node() ? 0 : kSwitchPenalty)), + dist, way, from, to, elevation, false); }); } } diff --git a/include/osr/routing/profiles/car_sharing.h b/include/osr/routing/profiles/car_sharing.h index 636a4377..9df85096 100644 --- a/include/osr/routing/profiles/car_sharing.h +++ b/include/osr/routing/profiles/car_sharing.h @@ -47,7 +47,8 @@ struct car_sharing { .is_sidewalk_separate_ = false, .motor_vehicle_no_ = false, .has_toll_ = false, - .is_big_street_ = false}; + .is_big_street_ = false, + .importance_ = 0}; static constexpr auto const kAdditionalNodeProperties = node_properties{.from_level_ = 0, @@ -58,7 +59,8 @@ struct car_sharing { .is_entrance_ = false, .is_multi_level_ = false, .is_parking_ = false, - .to_level_ = 0}; + .to_level_ = 0, + .importance_ = 0}; enum class node_type : std::uint8_t { kInitialFoot, @@ -351,7 +353,8 @@ struct car_sharing { std::uint16_t const from, std::uint16_t const to, elevation_storage::elevation const elevation, bool) { if (is_allowed(sharing->through_allowed_, neighbor.n_)) { - fn(to_node(neighbor, nt), cost + switch_penalty, dist, way, from, + fn(to_node(neighbor, nt), + static_cast(cost + switch_penalty), dist, way, from, to, elevation, switch_penalty != 0); } }); @@ -379,8 +382,9 @@ struct car_sharing { std::uint16_t const from, std::uint16_t const to, elevation_storage::elevation const elevation, bool) { if (is_allowed(sharing->through_allowed_, neighbor.n_)) { - fn(to_node(neighbor, kNoLevel), cost + switch_penalty, dist, way, - from, to, elevation, false); + fn(to_node(neighbor, kNoLevel), + static_cast(cost + switch_penalty), dist, way, from, + to, elevation, false); } }); if (include_additional_edges) { diff --git a/include/osr/routing/profiles/foot.h b/include/osr/routing/profiles/foot.h index 0a51458c..d6de1945 100644 --- a/include/osr/routing/profiles/foot.h +++ b/include/osr/routing/profiles/foot.h @@ -190,9 +190,8 @@ struct foot { auto const cost = way_cost(params, target_way_prop, way_dir, dist) + node_cost(target_node_prop); - fn(node{target_node, target_lvl}, - static_cast(cost), dist, way, from, to, - elevation_storage::elevation{}, false); + fn(node{target_node, target_lvl}, static_cast(cost), + dist, way, from, to, elevation_storage::elevation{}, false); }); } else { auto const target_lvl = get_target_level(w, n.n_, n.lvl_, way); @@ -203,8 +202,8 @@ struct foot { auto const dist = w.way_node_dist_[way][std::min(from, to)]; auto const cost = way_cost(params, target_way_prop, way_dir, dist) + node_cost(target_node_prop); - fn(node{target_node, *target_lvl}, static_cast(cost), - dist, way, from, to, elevation_storage::elevation{}, false); + fn(node{target_node, *target_lvl}, static_cast(cost), dist, + way, from, to, elevation_storage::elevation{}, false); } }; diff --git a/include/osr/ways.h b/include/osr/ways.h index 9a3496ef..e2c940e3 100644 --- a/include/osr/ways.h +++ b/include/osr/ways.h @@ -103,6 +103,7 @@ struct way_properties { bool motor_vehicle_no_ : 1; bool has_toll_ : 1; bool is_big_street_ : 1; + std::uint8_t importance_ : 3; // only used during extract }; static_assert(sizeof(way_properties) == 4); @@ -115,6 +116,7 @@ struct node_properties { constexpr bool is_multi_level() const { return is_multi_level_; } constexpr bool is_entrance() const { return is_entrance_; } constexpr bool is_parking() const { return is_parking_; } + constexpr std::uint8_t importance() const { return importance_; } constexpr level_t from_level() const { return level_t{from_level_}; } constexpr level_t to_level() const { return level_t{to_level_}; } @@ -142,6 +144,7 @@ struct node_properties { bool is_parking_ : 1; std::uint8_t to_level_ : 5; + std::uint8_t importance_ : 3; }; static_assert(sizeof(node_properties) == 3); @@ -152,7 +155,7 @@ struct ways { void add_restriction(std::vector&); void compute_big_street_neighbors(); void connect_ways(); - void build_components(); + void build_components_and_importance(); std::optional find_way(osm_way_idx_t const i) { auto const it = std::lower_bound(begin(way_osm_idx_), end(way_osm_idx_), i); diff --git a/src/extract.cc b/src/extract.cc index 8465d269..e0530d97 100644 --- a/src/extract.cc +++ b/src/extract.cc @@ -60,23 +60,38 @@ bool is_number(std::string_view s) { utl::all_of(s, [](char const c) { return std::isdigit(c); }); } -bool is_big_street(tags const& t) { +std::uint8_t get_importance(tags const& t) { switch (cista::hash(t.highway_)) { - case cista::hash("motorway"): - case cista::hash("motorway_link"): - case cista::hash("trunk"): - case cista::hash("trunk_link"): - case cista::hash("primary"): - case cista::hash("primary_link"): + case cista::hash("pedestrian"): + case cista::hash("busway"): + case cista::hash("footway"): + case cista::hash("cycleway"): + case cista::hash("bridleway"): + case cista::hash("steps"): + case cista::hash("corridor"): + case cista::hash("path"): return 0; + case cista::hash("track"): return 1; + case cista::hash("living_street"): + case cista::hash("service"): return 2; + case cista::hash("residential"): return 3; + case cista::hash("road"): return 4; + case cista::hash("unclassified"): + case cista::hash("tertiary"): + case cista::hash("tertiary_link"): return 5; case cista::hash("secondary"): case cista::hash("secondary_link"): - case cista::hash("tertiary"): - case cista::hash("tertiary_link"): - case cista::hash("unclassified"): return true; - default: return false; + case cista::hash("primary"): + case cista::hash("primary_link"): return 6; + case cista::hash("trunk"): + case cista::hash("trunk_link"): + case cista::hash("motorway"): + case cista::hash("motorway_link"): return 7; + default: return 4; } } +bool is_big_street(std::uint8_t const importance) { return importance > 4; } + speed_limit get_speed_limit(tags const& t) { if (is_number(t.max_speed_) /* TODO: support units (kmh/mph) */) { return get_speed_limit( @@ -140,7 +155,8 @@ way_properties get_way_properties(tags const& t) { p.motor_vehicle_no_ = (t.motor_vehicle_ == "no"sv) || (t.vehicle_ == override::kBlacklist); p.has_toll_ = t.toll_; - p.is_big_street_ = is_big_street(t); + p.importance_ = get_importance(t); + p.is_big_street_ = is_big_street(p.importance_); return p; } @@ -157,6 +173,7 @@ std::pair get_node_properties(tags const& t) { p.is_multi_level_ = is_multi; p.is_parking_ = t.is_parking_; p.to_level_ = to_idx(to); + p.importance_ = 0; return {p, t.level_bits_}; } @@ -581,7 +598,6 @@ void extract(bool const with_platforms, w.sync(); w.connect_ways(); - w.build_components(); auto r = std::vector{}; { @@ -600,6 +616,7 @@ void extract(bool const with_platforms, pt->update(pt->in_high_); } + w.build_components_and_importance(); w.add_restriction(r); utl::sort(w.r_->multi_level_elevators_); diff --git a/src/route.cc b/src/route.cc index 4c33bc05..56aa613c 100644 --- a/src/route.cc +++ b/src/route.cc @@ -1,12 +1,11 @@ #include "osr/routing/route.h" +#include "osr/preprocessing/ch_preprocessing.h" -#include #include #include #include "boost/thread/tss.hpp" -#include "utl/concat.h" #include "utl/enumerate.h" #include "utl/helpers/algorithm.h" #include "utl/to_vec.h" @@ -15,20 +14,14 @@ #include "osr/elevation_storage.h" #include "osr/lookup.h" #include "osr/routing/bidirectional.h" +#include "osr/routing/bidirectional_dijkstra.h" #include "osr/routing/dijkstra.h" -#include "osr/routing/profiles/bike.h" -#include "osr/routing/profiles/bike_sharing.h" -#include "osr/routing/profiles/car.h" -#include "osr/routing/profiles/car_parking.h" -#include "osr/routing/profiles/car_sharing.h" -#include "osr/routing/profiles/foot.h" #include "osr/routing/sharing_data.h" #include "osr/routing/with_profile.h" #include "osr/util/infinite.h" #include "osr/util/reverse.h" namespace osr { - constexpr auto const kMaxMatchingDistanceSquaredRatio = 9.0; constexpr auto const kBottomKDefinitelyConsidered = 5; @@ -50,6 +43,15 @@ dijkstra

& get_dijkstra() { return *s.get(); } +template +bidirectional_dijkstra

& get_bidirectional_dijkstra() { + static auto s = boost::thread_specific_ptr>{}; + if (s.get() == nullptr) { + s.reset(new bidirectional_dijkstra

{true, &get_ch_preprocessor

()}); + } + return *s.get(); +} + struct connecting_way { constexpr bool valid() const { return way_ != way_idx_t::invalid(); } @@ -64,6 +66,8 @@ routing_algorithm to_algorithm(std::string_view s) { switch (cista::hash(s)) { case cista::hash("dijkstra"): return routing_algorithm::kDijkstra; case cista::hash("bidirectional"): return routing_algorithm::kAStarBi; + case cista::hash("bidirectional_dijkstra"): + return routing_algorithm::kBiDijkstra; } throw utl::fail("unknown routing algorithm: {}", s); } @@ -408,6 +412,128 @@ path reconstruct(typename P::parameters const& params, return p; } +template +std::optional reconstruct_ch_preprocessing( + typename P::parameters const& params, + ways const& w, + lookup const& l, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations, + bidirectional_dijkstra

const& bd, + location const& from, + location const& to, + way_candidate const& start, + way_candidate const& dest, + cost_t const cost, + direction const dir) { + + // Use the same reconstruction logic as bidirectional but adapted for + // bidirectional_dijkstra + auto forward_n = bd.meet_point_1_; + auto forward_segments = std::vector{}; + auto forward_dist = 0.0; + + // Reconstruct forward path + while (true) { + auto const it = bd.cost1_.find(forward_n.get_key()); + if (it == bd.cost1_.end()) { + break; + } + auto const& e = it->second; + auto const pred = e.pred(forward_n); + if (pred.has_value()) { + auto const pred_cost = bd.template get_cost(*pred); + auto const curr_cost = + bd.template get_cost(forward_n); + if (pred_cost != kInfeasible && curr_cost != kInfeasible) { + auto const expected_cost = static_cast(curr_cost - pred_cost); + forward_dist += + add_path

(params, w, *w.r_, blocked, sharing, elevations, *pred, + forward_n, expected_cost, forward_segments, dir); + } + } else { + break; + } + forward_n = *pred; + } + + // Add start segment + auto const& start_node_candidate = + forward_n.get_node() == start.left_.node_ ? start.left_ : start.right_; + forward_segments.push_back( + {.polyline_ = + l.get_node_candidate_path(start, start_node_candidate, false, from), + .from_level_ = start_node_candidate.lvl_, + .to_level_ = start_node_candidate.lvl_, + .from_ = dir == direction::kBackward ? forward_n.get_node() + : node_idx_t::invalid(), + .to_ = dir == direction::kForward ? forward_n.get_node() + : node_idx_t::invalid(), + .way_ = way_idx_t::invalid(), + .cost_ = start_node_candidate.cost_, + .dist_ = static_cast(start_node_candidate.dist_to_node_), + .mode_ = forward_n.get_mode()}); + + // Reconstruct backward path + auto backward_segments = std::vector{}; + auto backward_n = bd.meet_point_2_; + auto backward_dist = 0.0; + + while (true) { + auto const it = bd.cost2_.find(backward_n.get_key()); + if (it == bd.cost2_.end()) { + break; + } + auto const& e = it->second; + auto const pred = e.pred(backward_n); + if (pred.has_value()) { + auto const pred_cost = bd.template get_cost(*pred); + auto const curr_cost = + bd.template get_cost(backward_n); + if (pred_cost != kInfeasible && curr_cost != kInfeasible) { + auto const expected_cost = static_cast(curr_cost - pred_cost); + backward_dist += add_path

( + params, w, *w.r_, blocked, sharing, elevations, *pred, backward_n, + expected_cost, backward_segments, opposite(dir)); + } + } else { + break; + } + backward_n = *pred; + } + + // Add destination segment + auto const& dest_node_candidate = + backward_n.get_node() == dest.left_.node_ ? dest.left_ : dest.right_; + backward_segments.push_back( + {.polyline_ = + l.get_node_candidate_path(dest, dest_node_candidate, true, to), + .from_level_ = dest_node_candidate.lvl_, + .to_level_ = dest_node_candidate.lvl_, + .from_ = dir == direction::kForward ? backward_n.get_node() + : node_idx_t::invalid(), + .to_ = dir == direction::kBackward ? backward_n.get_node() + : node_idx_t::invalid(), + .way_ = way_idx_t::invalid(), + .cost_ = dest_node_candidate.cost_, + .dist_ = static_cast(dest_node_candidate.dist_to_node_), + .mode_ = backward_n.get_mode()}); + + // Combine segments + std::reverse(begin(forward_segments), end(forward_segments)); + forward_segments.insert(end(forward_segments), begin(backward_segments), + end(backward_segments)); + + auto total_dist = forward_dist + backward_dist + + static_cast(start_node_candidate.dist_to_node_) + + static_cast(dest_node_candidate.dist_to_node_); + + return path{.cost_ = cost, + .dist_ = static_cast(total_dist), + .segments_ = std::move(forward_segments)}; +} + bool component_seen(ways const& w, match_view_t matches, size_t match_idx, @@ -613,6 +739,100 @@ std::optional route_bidirectional(typename P::parameters const& params, return std::nullopt; } +template +std::optional route_bidirectional_dijkstra( + typename P::parameters const& params, + ways const& w, + lookup const& l, + bidirectional_dijkstra

& bd, + location const& from, + location const& to, + match_view_t from_match, + match_view_t to_match, + cost_t const max, + direction const dir, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + if (auto const direct = try_direct(from, to); direct.has_value()) { + return *direct; + } + + bd.reset(max); + auto const limit_squared_max_matching_distance = + std::pow(geo::distance(from.pos_, to.pos_), 2) / + kMaxMatchingDistanceSquaredRatio; + + for (auto const [i, start] : utl::enumerate(from_match)) { + if (utl::none_of(to_match, [&](way_candidate const& end) { + return w.r_->way_component_[start.way_] == + w.r_->way_component_[end.way_]; + })) { + continue; + } + + auto const start_way = start.way_; + for (auto const* nc : {&start.left_, &start.right_}) { + if (nc->valid() && nc->cost_ < max) { + P::resolve_start_node( + *w.r_, start.way_, nc->node_, from.lvl_, dir, [&](auto const node) { + auto label = typename P::label{node, nc->cost_}; + label.track(label, *w.r_, start_way, node.get_node(), false); + bd.add_start(w, label, sharing); + }); + } + } + + for (auto const [j, end] : utl::enumerate(to_match)) { + if (w.r_->way_component_[start.way_] != w.r_->way_component_[end.way_]) { + continue; + } + if (std::pow(end.dist_to_way_, 2) > limit_squared_max_matching_distance && + j > kBottomKDefinitelyConsidered) { + break; + } + + auto const end_way = end.way_; + for (auto const* nc : {&end.left_, &end.right_}) { + if (nc->valid() && nc->cost_ < max) { + P::resolve_start_node( + *w.r_, end_way, nc->node_, to.lvl_, opposite(dir), + [&](auto const node) { + auto label = typename P::label{node, nc->cost_}; + label.track(label, *w.r_, end_way, node.get_node(), false); + bd.add_end(w, label, sharing); + }); + } + } + + if (bd.pq1_.empty() || bd.pq2_.empty()) { + continue; + } + + auto const should_continue = + bd.run(params, w, *w.r_, max, blocked, sharing, elevations, dir); + + if (bd.meet_point_1_.get_node() == node_idx_t::invalid()) { + if (should_continue) { + continue; + } + return std::nullopt; + } + + auto const cost = bd.get_cost_to_mp(bd.meet_point_1_, bd.meet_point_2_); + + return reconstruct_ch_preprocessing(params, w, l, blocked, sharing, + elevations, bd, from, to, start, end, + cost, dir); + } + bd.pq1_.clear(); + bd.pq2_.clear(); + bd.cost1_.clear(); + bd.cost2_.clear(); + } + return std::nullopt; +} + template std::optional route_dijkstra(typename P::parameters const& params, ways const& w, @@ -849,6 +1069,36 @@ std::optional route_dijkstra(profile_parameters const& params, }); } +std::optional route_bidirectional_dijkstra( + profile_parameters const& params, + ways const& w, + lookup const& l, + search_profile const profile, + location const& from, + location const& to, + cost_t const max, + direction const dir, + double const max_match_distance, + bitvec const* blocked, + sharing_data const* sharing, + elevation_storage const* elevations) { + return with_profile(profile, [&](P&&) -> std::optional { + auto const& pp = std::get(params); + auto const from_match = + l.match

(pp, from, false, dir, max_match_distance, blocked); + auto const to_match = + l.match

(pp, to, true, dir, max_match_distance, blocked); + + if (from_match.empty() || to_match.empty()) { + return std::nullopt; + } + + return route_bidirectional_dijkstra( + pp, w, l, get_bidirectional_dijkstra

(), from, to, from_match, + to_match, max, dir, blocked, sharing, elevations); + }); +} + std::vector> route( profile_parameters const& params, ways const& w, @@ -911,6 +1161,13 @@ std::optional route(profile_parameters const& params, from_match, to_match, max, dir, blocked, sharing, elevations); }); + case routing_algorithm::kBiDijkstra: + return with_profile(profile, [&](P&&) { + return route_bidirectional_dijkstra( + std::get(params), w, l, + get_bidirectional_dijkstra

(), from, to, from_match, to_match, + max, dir, blocked, sharing, elevations); + }); } throw utl::fail("not implemented"); } @@ -942,6 +1199,10 @@ std::optional route(profile_parameters const& params, return route_bidirectional(params, w, l, profile, from, to, max, dir, max_match_distance, blocked, sharing, elevations); + case routing_algorithm::kBiDijkstra: + return route_bidirectional_dijkstra(params, w, l, profile, from, to, max, + dir, max_match_distance, blocked, + sharing, elevations); } throw utl::fail("not implemented"); } diff --git a/src/ways.cc b/src/ways.cc index 6143b0d1..fb49091b 100644 --- a/src/ways.cc +++ b/src/ways.cc @@ -25,7 +25,7 @@ ways::ways(std::filesystem::path p, cista::mmap::protection const mode) mm_vec(mm("way_has_conditional_access_no"))}, way_conditional_access_no_{mm("way_conditional_access_no")} {} -void ways::build_components() { +void ways::build_components_and_importance() { auto q = hash_set{}; auto flood_fill = [&](way_idx_t const way_idx, component_idx_t const c) { assert(q.empty()); @@ -34,6 +34,9 @@ void ways::build_components() { auto const next = *q.begin(); q.erase(q.begin()); for (auto const n : r_->way_nodes_[next]) { + r_->node_properties_[n].importance_ = + std::max(r_->node_properties_[n].importance_, + r_->way_properties_[next].importance_); for (auto const w : r_->node_ways_[n]) { auto& wc = r_->way_component_[w]; if (wc == component_idx_t::invalid()) { @@ -46,7 +49,9 @@ void ways::build_components() { }; auto pt = utl::get_active_progress_tracker_or_activate("osr"); - pt->status("Build components").in_high(n_ways()).out_bounds(75, 90); + pt->status("Build components and importance") + .in_high(n_ways()) + .out_bounds(75, 90); auto next_component_idx = component_idx_t{0U}; r_->way_component_.resize(n_ways(), component_idx_t::invalid()); diff --git a/test/dijkstrabidir_test.cc b/test/dijkstrabidir_test.cc new file mode 100644 index 00000000..b80a394d --- /dev/null +++ b/test/dijkstrabidir_test.cc @@ -0,0 +1,258 @@ +#ifdef _WIN32 +#include "windows.h" +#endif + +#include "gtest/gtest.h" + +#include +#include +#include +#include +#include +#include + +#include "cista/mmap.h" + +#include "utl/parallel_for.h" + +#include "fmt/core.h" + +#include "osr/extract/extract.h" +#include "osr/geojson.h" +#include "osr/location.h" +#include "osr/lookup.h" +#include "osr/routing/dijkstra.h" +#include "osr/routing/profile.h" +#include "osr/routing/profiles/car.h" +#include "osr/routing/route.h" +#include "osr/types.h" +#include "osr/ways.h" + +namespace fs = std::filesystem; +using namespace osr; + +constexpr auto const kUseMultithreading = true; +constexpr auto const kPrintDebugGeojson = false; +constexpr auto const kMaxMatchDistance = 100; // meters +constexpr auto const kMaxAllowedPathDifferenceRatio = 0.5; + +static void load(std::string_view raw_data, std::string_view data_dir) { + if (!fs::exists(data_dir)) { + if (fs::exists(raw_data)) { + auto const p = fs::path{data_dir}; + auto ec = std::error_code{}; + fs::remove_all(p, ec); + fs::create_directories(p, ec); + osr::extract(false, raw_data, data_dir, fs::path{}); + } + } +} + +// Compare classic Dijkstra vs bidirectional Dijkstra (kBiDijkstra) for +// consistency. +static void run_bidijkstra_consistency(ways const& w, + lookup const& l, + unsigned const n_samples, + unsigned const max_cost, + direction const dir) { + auto const pairs = [&]() { + auto prng = std::mt19937{}; + auto distr = + std::uniform_int_distribution{0, w.n_nodes() - 1}; + auto v = std::vector>{}; + v.reserve(n_samples); + for (auto i = 0U; i != n_samples; ++i) { + v.emplace_back(distr(prng), distr(prng)); + } + return v; + }(); + + auto n_congruent = std::atomic{0U}; + auto n_empty_matches = std::atomic{0U}; + auto baseline_times = std::vector{}; + auto bidir_times = std::vector{}; + auto m = std::mutex{}; + + auto const single_run = [&](std::pair const from_to) { + auto const from_node = from_to.first; + auto const to_node = from_to.second; + auto const from_loc = location{w.get_node_pos(from_node)}; + auto const to_loc = location{w.get_node_pos(to_node)}; + + auto const node_pinned_matches = + [&](location const& loc, node_idx_t const n, bool const reverse) { + auto matches = l.match(car::parameters{}, loc, reverse, dir, + kMaxMatchDistance, nullptr); + std::erase_if(matches, [&](auto const& wc) { + return wc.left_.node_ != n && wc.right_.node_ != n; + }); + return matches; // keep all (usually 0-1) + }; + + auto from_matches = node_pinned_matches(from_loc, from_node, false); + auto to_matches = node_pinned_matches(to_loc, to_node, true); + + if (from_matches.empty() || to_matches.empty()) { + ++n_empty_matches; + } + + auto const from_span = std::span{begin(from_matches), end(from_matches)}; + auto const to_span = std::span{begin(to_matches), end(to_matches)}; + + // Baseline classic Dijkstra + auto const baseline_start = std::chrono::steady_clock::now(); + auto const baseline_route = + route(car::parameters{}, w, l, search_profile::kCar, from_loc, to_loc, + from_span, to_span, max_cost, dir, nullptr, nullptr, nullptr, + routing_algorithm::kDijkstra); + auto const baseline_time = + std::chrono::steady_clock::now() - baseline_start; + + // Bidirectional Dijkstra implementation + auto const bidir_start = std::chrono::steady_clock::now(); + auto const bidir_route = + route(car::parameters{}, w, l, search_profile::kCar, from_loc, to_loc, + from_span, to_span, max_cost, dir, nullptr, nullptr, nullptr, + routing_algorithm::kBiDijkstra); + auto const bidir_time = std::chrono::steady_clock::now() - bidir_start; + + auto congruent = false; + if (baseline_route.has_value() != bidir_route.has_value()) { + congruent = false; + } else if (baseline_route && bidir_route) { + congruent = baseline_route->cost_ == bidir_route->cost_; + } else { + congruent = true; // both infeasible + } + + if (!congruent) { + auto const print_result = [&](std::string_view name, auto const& p, + auto const& t) { + fmt::println( + "{:10}: {:11} --> {:11} | {} | time: {}:{:0>3}:{:0>3} s", name, + w.node_to_osm_[from_node], w.node_to_osm_[to_node], + p ? fmt::format("cost: {:5} | dist: {:>10.2f}", p->cost_, p->dist_) + : "no result", + std::chrono::duration_cast(t).count(), + std::chrono::duration_cast(t).count() % + 1000, + std::chrono::duration_cast(t).count() % + 1000); + if (p.has_value() && kPrintDebugGeojson) { + fmt::println("{}\n", to_featurecollection(w, p)); + } + }; + print_result("dijkstra", baseline_route, baseline_time); + print_result("bi-dijk", bidir_route, bidir_time); + } else { + ++n_congruent; + } + + if (!from_matches.empty() && !to_matches.empty()) { + auto const guard = std::lock_guard{m}; + baseline_times.emplace_back(baseline_time); + bidir_times.emplace_back(bidir_time); + } + }; + + if (kUseMultithreading) { + utl::parallel_for(pairs, single_run); + } else { + for (auto const& p : pairs) { + single_run(p); + } + } + + auto const non_empty_congruent = n_congruent - n_empty_matches; + auto const non_empty_samples = n_samples - n_empty_matches; + + EXPECT_EQ(non_empty_samples, non_empty_congruent); + + fmt::println( + "dijkstra vs bidir-dijkstra congruent (non-empty): {}/{} ({:3.1f}%)", + non_empty_congruent, non_empty_samples, + non_empty_samples == 0 + ? 0.0 + : (static_cast(non_empty_congruent) / non_empty_samples) * + 100.0); + if (non_empty_congruent == non_empty_samples && !baseline_times.empty()) { + fmt::println( + "avg speed ratio (baseline/bidir): {:.2f}", + static_cast( + std::reduce(begin(baseline_times), end(baseline_times)).count()) / + static_cast( + std::reduce(begin(bidir_times), end(bidir_times)).count())); + } +} + +TEST(bidirectional_dijkstra, monaco) { + auto const raw_data = "test/monaco.osm.pbf"; + auto const data_dir = "test/monaco"; + auto const num_samples = 10000U; + auto const max_cost = 2 * 3600U; + auto constexpr dir = direction::kForward; + + if (!fs::exists(raw_data) && !fs::exists(data_dir)) { + GTEST_SKIP() << raw_data << " not found"; + } + + load(raw_data, data_dir); + auto const w = osr::ways{data_dir, cista::mmap::protection::READ}; + auto const l = osr::lookup{w, data_dir, cista::mmap::protection::READ}; + + run_bidijkstra_consistency(w, l, num_samples, max_cost, dir); +} + +TEST(bidirectional_dijkstra, hamburg) { + auto const raw_data = "test/hamburg.osm.pbf"; + auto const data_dir = "test/hamburg"; + auto const num_samples = 800U; + auto const max_cost = 3 * 3600U; + auto constexpr dir = direction::kForward; + + if (!fs::exists(raw_data) && !fs::exists(data_dir)) { + GTEST_SKIP() << raw_data << " not found"; + } + + load(raw_data, data_dir); + auto const w = osr::ways{data_dir, cista::mmap::protection::READ}; + auto const l = osr::lookup{w, data_dir, cista::mmap::protection::READ}; + + run_bidijkstra_consistency(w, l, num_samples, max_cost, dir); +} + +TEST(bidirectional_dijkstra, switzerland) { + auto const raw_data = "test/switzerland.osm.pbf"; + auto const data_dir = "test/switzerland"; + auto const num_samples = 500U; + auto const max_cost = 5 * 3600U; + auto constexpr dir = direction::kForward; + + if (!fs::exists(raw_data) && !fs::exists(data_dir)) { + GTEST_SKIP() << raw_data << " not found"; + } + + load(raw_data, data_dir); + auto const w = osr::ways{data_dir, cista::mmap::protection::READ}; + auto const l = osr::lookup{w, data_dir, cista::mmap::protection::READ}; + + run_bidijkstra_consistency(w, l, num_samples, max_cost, dir); +} + +TEST(bidirectional_dijkstra, DISABLED_germany) { + auto const raw_data = "test/germany.osm.pbf"; + auto const data_dir = "test/germany"; + auto const num_samples = 50U; + auto const max_cost = 12 * 3600U; + auto constexpr dir = direction::kForward; + + if (!fs::exists(raw_data) && !fs::exists(data_dir)) { + GTEST_SKIP() << raw_data << " not found"; + } + + load(raw_data, data_dir); + auto const w = osr::ways{data_dir, cista::mmap::protection::READ}; + auto const l = osr::lookup{w, data_dir, cista::mmap::protection::READ}; + + run_bidijkstra_consistency(w, l, num_samples, max_cost, dir); +}