From 3d4002200d0b0df35b8adc483bec1d7231375d13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Felix=20G=C3=BCndling?= Date: Sun, 27 Jul 2025 22:07:42 +0200 Subject: [PATCH 1/3] second chance second try --- src/route.cc | 322 ++++++++++++++++++++++++++------------------------- 1 file changed, 162 insertions(+), 160 deletions(-) diff --git a/src/route.cc b/src/route.cc index 30aec39d..7b7f5ab2 100644 --- a/src/route.cc +++ b/src/route.cc @@ -30,7 +30,7 @@ namespace osr { constexpr auto const kMaxMatchingDistanceSquaredRatio = 9.0; -#define trace(...) +#define trace(...) struct connecting_way { constexpr bool valid() const { return way_ != way_idx_t::invalid(); } @@ -436,54 +436,44 @@ best_candidate(ways const& w, return std::pair{best_node, best_cost}; }; - for (auto const accept_bad_max_matching_dist : {false, true}) { - for (auto const [j, dest] : utl::enumerate(m)) { - if (w.r_->way_component_[start.way_] != w.r_->way_component_[dest.way_]) { - trace("reject {}: component mismatch {} vs {}", - w.way_osm_idx_[dest.way_], w.r_->way_component_[start.way_], - w.r_->way_component_[dest.way_]); - continue; - } - if (!should_continue && component_seen(w, m, j, 10)) { - trace("reject {}: should_continue={}, component_seen={}", - w.way_osm_idx_[dest.way_], should_continue, - component_seen(w, m, j, 10)); - continue; - } - if (accept_bad_max_matching_dist && - std::pow(dest.dist_to_way_, 2) > - limit_squared_max_matching_distance) { - trace("reject {}: limit squared matching distance: {} > {} ", - w.way_osm_idx_[dest.way_], dest.dist_to_way_, - std::sqrt(limit_squared_max_matching_distance)); - break; - } else if (std::pow(dest.dist_to_way_, 2) > - limit_squared_max_matching_distance) { - trace("WOULD reject {}: limit squared matching distance: {} > {} ", - w.way_osm_idx_[dest.way_], dest.dist_to_way_, - std::sqrt(limit_squared_max_matching_distance)); - } - auto best_node = Profile::node::invalid(); - auto best_cost = path{.cost_ = std::numeric_limits::max()}; - auto best = static_cast(nullptr); - - for (auto const x : {&dest.left_, &dest.right_}) { - if (x->valid()) { - auto const [x_node, x_cost] = get_best(dest, x); - if (x_cost.cost_ < best_cost.cost_) { - best = x; - best_node = x_node; - best_cost = x_cost; - } + for (auto const [j, dest] : utl::enumerate(m)) { + if (w.r_->way_component_[start.way_] != w.r_->way_component_[dest.way_]) { + trace("reject {}: component mismatch {} vs {}", w.way_osm_idx_[dest.way_], + w.r_->way_component_[start.way_], w.r_->way_component_[dest.way_]); + continue; + } + if (!should_continue && component_seen(w, m, j, 10)) { + trace("reject {}: should_continue={}, component_seen={}", + w.way_osm_idx_[dest.way_], should_continue, + component_seen(w, m, j, 10)); + continue; + } + if (std::pow(dest.dist_to_way_, 2) > limit_squared_max_matching_distance) { + trace("reject {}: limit squared matching distance: {} > {} ", + w.way_osm_idx_[dest.way_], dest.dist_to_way_, + std::sqrt(limit_squared_max_matching_distance)); + break; + } + auto best_node = Profile::node::invalid(); + auto best_cost = path{.cost_ = std::numeric_limits::max()}; + auto best = static_cast(nullptr); + + for (auto const x : {&dest.left_, &dest.right_}) { + if (x->valid()) { + auto const [x_node, x_cost] = get_best(dest, x); + if (x_cost.cost_ < best_cost.cost_) { + best = x; + best_node = x_node; + best_cost = x_cost; } } + } - if (best != nullptr) { - trace("MATCHED cost={} (max={})", best_cost.cost_, max); - return best_cost.cost_ < max ? std::optional{std::tuple{ - best, &dest, best_node, best_cost}} - : std::nullopt; - } + if (best != nullptr) { + trace("MATCHED cost={} (max={})", best_cost.cost_, max); + return best_cost.cost_ < max + ? std::optional{std::tuple{best, &dest, best_node, best_cost}} + : std::nullopt; } } trace("NOTHING MATCHED"); @@ -556,50 +546,45 @@ std::optional route_bidirectional(ways const& w, if (b.pq1_.empty()) { continue; } - for (auto const accept_bad_max_matching_dist : {false, true}) { - for (auto const [j, end] : utl::enumerate(to_match)) { - if (w.r_->way_component_[start.way_] != - w.r_->way_component_[end.way_]) { - continue; - } - if (b.max_reached_2_ && component_seen(w, to_match, j)) { - continue; - } - if (accept_bad_max_matching_dist && - std::pow(end.dist_to_way_, 2) > - limit_squared_max_matching_distance) { - break; - } - auto const end_way = end.way_; - for (auto const* nc : {&end.left_, &end.right_}) { - if (nc->valid() && nc->cost_ < max) { - Profile::resolve_start_node( - *w.r_, end_way, nc->node_, to.lvl_, opposite(dir), - [&](auto const node) { - auto label = typename Profile::label{node, nc->cost_}; - label.track(label, *w.r_, end_way, node.get_node(), false); - b.add_end(w, label, sharing); - }); - } - } - if (b.pq2_.empty()) { - continue; + for (auto const [j, end] : utl::enumerate(to_match)) { + if (w.r_->way_component_[start.way_] != w.r_->way_component_[end.way_]) { + continue; + } + if (b.max_reached_2_ && component_seen(w, to_match, j)) { + continue; + } + if (std::pow(end.dist_to_way_, 2) > limit_squared_max_matching_distance) { + break; + } + auto const end_way = end.way_; + for (auto const* nc : {&end.left_, &end.right_}) { + if (nc->valid() && nc->cost_ < max) { + Profile::resolve_start_node( + *w.r_, end_way, nc->node_, to.lvl_, opposite(dir), + [&](auto const node) { + auto label = typename Profile::label{node, nc->cost_}; + label.track(label, *w.r_, end_way, node.get_node(), false); + b.add_end(w, label, sharing); + }); } - auto const should_continue = - b.run(w, *w.r_, max, blocked, sharing, elevations, dir); + } + if (b.pq2_.empty()) { + continue; + } + auto const should_continue = + b.run(w, *w.r_, max, blocked, sharing, elevations, dir); - if (b.meet_point_1_.get_node() == node_idx_t::invalid()) { - if (should_continue) { - continue; - } - return std::nullopt; + if (b.meet_point_1_.get_node() == node_idx_t::invalid()) { + if (should_continue) { + continue; } + return std::nullopt; + } - auto const cost = b.get_cost_to_mp(b.meet_point_1_, b.meet_point_2_); + auto const cost = b.get_cost_to_mp(b.meet_point_1_, b.meet_point_2_); - return reconstruct_bi(w, l, blocked, sharing, elevations, b, from, to, - start, end, cost, dir); - } + return reconstruct_bi(w, l, blocked, sharing, elevations, b, from, to, + start, end, cost, dir); } b.pq1_.clear(); b.pq2_.clear(); @@ -626,45 +611,53 @@ std::optional route_dijkstra(ways const& w, return *direct; } - auto const limit_squared_max_matching_distance = - std::pow(geo::distance(from.pos_, to.pos_), 2) / - kMaxMatchingDistanceSquaredRatio; + auto const distance_lng_degrees = geo::approx_distance_lng_degrees(from.pos_); d.reset(max); auto should_continue = true; - for (auto const [i, start] : utl::enumerate(from_match)) { - if (!should_continue && component_seen(w, from_match, i)) { - continue; - } - if (utl::none_of(to_match, [&](way_candidate const& end) { - return w.r_->way_component_[start.way_] == - w.r_->way_component_[end.way_]; - })) { - continue; - } - - for (auto const* nc : {&start.left_, &start.right_}) { - if (nc->valid() && nc->cost_ < max) { - Profile::resolve_start_node( - *w.r_, start.way_, nc->node_, from.lvl_, dir, - [&](auto const node) { d.add_start(w, {node, nc->cost_}); }); + for (auto const accept_bad_max_matching_dist : {false, true}) { + auto const limit_squared_max_matching_distance = + accept_bad_max_matching_dist + ? std::numeric_limits::max() + : geo::approx_squared_distance(from.pos_, to.pos_, + distance_lng_degrees) / + kMaxMatchingDistanceSquaredRatio; + + for (auto const [i, start] : utl::enumerate(from_match)) { + if (!should_continue && component_seen(w, from_match, i)) { + continue; + } + if (utl::none_of(to_match, [&](way_candidate const& end) { + return w.r_->way_component_[start.way_] == + w.r_->way_component_[end.way_]; + })) { + continue; } - } - if (d.pq_.empty()) { - continue; - } + for (auto const* nc : {&start.left_, &start.right_}) { + if (nc->valid() && nc->cost_ < max) { + Profile::resolve_start_node( + *w.r_, start.way_, nc->node_, from.lvl_, dir, + [&](auto const node) { d.add_start(w, {node, nc->cost_}); }); + } + } - should_continue = d.run(w, *w.r_, max, blocked, sharing, elevations, dir) && - should_continue; + if (d.pq_.empty()) { + continue; + } - auto const c = - best_candidate(w, d, to.lvl_, to_match, max, dir, should_continue, - start, limit_squared_max_matching_distance); - if (c.has_value()) { - auto const [nc, wc, node, p] = *c; - return reconstruct(w, l, blocked, sharing, elevations, d, from, - to, start, *wc, *nc, node, p.cost_, dir); + should_continue = + d.run(w, *w.r_, max, blocked, sharing, elevations, dir) && + should_continue; + + auto const c = + best_candidate(w, d, to.lvl_, to_match, max, dir, should_continue, + start, limit_squared_max_matching_distance); + if (c.has_value()) { + auto const [nc, wc, node, p] = *c; + return reconstruct(w, l, blocked, sharing, elevations, d, from, + to, start, *wc, *nc, node, p.cost_, dir); + } } } @@ -697,56 +690,65 @@ std::vector> route( d.reset(max); auto should_continue = true; - for (auto const [i, start] : utl::enumerate(from_match)) { - if (!should_continue && component_seen(w, from_match, i)) { - continue; - } - auto const start_way = start.way_; - for (auto const* nc : {&start.left_, &start.right_}) { - if (nc->valid() && nc->cost_ < max) { - Profile::resolve_start_node( - *w.r_, start.way_, nc->node_, from.lvl_, dir, [&](auto const node) { - auto label = typename Profile::label{node, nc->cost_}; - label.track(label, *w.r_, start_way, node.get_node(), false); - d.add_start(w, label); - }); + + for (auto const accept_bad_max_matching_dist : {false, true}) { + for (auto const [i, start] : utl::enumerate(from_match)) { + if (!should_continue && component_seen(w, from_match, i)) { + continue; + } + auto const start_way = start.way_; + for (auto const* nc : {&start.left_, &start.right_}) { + if (nc->valid() && nc->cost_ < max) { + Profile::resolve_start_node( + *w.r_, start.way_, nc->node_, from.lvl_, dir, + [&](auto const node) { + auto label = typename Profile::label{node, nc->cost_}; + label.track(label, *w.r_, start_way, node.get_node(), false); + d.add_start(w, label); + }); + } } - } - should_continue = d.run(w, *w.r_, max, blocked, sharing, elevations, dir) && - should_continue; + should_continue = + d.run(w, *w.r_, max, blocked, sharing, elevations, dir) && + should_continue; - auto found = 0U; - for (auto const [m, t, r] : utl::zip(to_match, to, result)) { - if (r.has_value()) { - ++found; - } else if (auto const direct = try_direct(from, t); direct.has_value()) { - r = direct; - } else { - auto const limit_squared_max_matching_distance = - geo::approx_squared_distance(from.pos_, t.pos_, - distance_lng_degrees) / - kMaxMatchingDistanceSquaredRatio; - - auto const c = - best_candidate(w, d, t.lvl_, m, max, dir, should_continue, start, - limit_squared_max_matching_distance); - if (c.has_value()) { - auto [nc, wc, n, p] = *c; - d.cost_.at(n.get_key()).write(n, p); - if (do_reconstruct(p)) { - p = reconstruct(w, l, blocked, sharing, elevations, d, - from, t, start, *wc, *nc, n, p.cost_, dir); - p.uses_elevator_ = true; - } - r = std::make_optional(p); + auto found = 0U; + for (auto const [m, t, r] : utl::zip(to_match, to, result)) { + if (r.has_value()) { ++found; + } else if (auto const direct = try_direct(from, t); + direct.has_value()) { + r = direct; + } else { + auto const limit_squared_max_matching_distance = + accept_bad_max_matching_dist + ? std::numeric_limits::max() + : geo::approx_squared_distance(from.pos_, t.pos_, + distance_lng_degrees) / + kMaxMatchingDistanceSquaredRatio; + + auto const c = + best_candidate(w, d, t.lvl_, m, max, dir, should_continue, start, + limit_squared_max_matching_distance); + if (c.has_value()) { + auto [nc, wc, n, p] = *c; + d.cost_.at(n.get_key()).write(n, p); + if (do_reconstruct(p)) { + p = reconstruct(w, l, blocked, sharing, elevations, d, + from, t, start, *wc, *nc, n, p.cost_, + dir); + p.uses_elevator_ = true; + } + r = std::make_optional(p); + ++found; + } } } - } - if (found == result.size()) { - return result; + if (found == result.size()) { + return result; + } } } From f7589de94b1d06ec63e15c04677e0c567979eacc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Felix=20G=C3=BCndling?= Date: Sun, 27 Jul 2025 22:21:25 +0200 Subject: [PATCH 2/3] second chance loop for bidir --- src/route.cc | 99 +++++++++++++++++++++++++++------------------------- 1 file changed, 52 insertions(+), 47 deletions(-) diff --git a/src/route.cc b/src/route.cc index 7b7f5ab2..9a61048d 100644 --- a/src/route.cc +++ b/src/route.cc @@ -30,7 +30,7 @@ namespace osr { constexpr auto const kMaxMatchingDistanceSquaredRatio = 9.0; -#define trace(...) +#define trace(...) struct connecting_way { constexpr bool valid() const { return way_ != way_idx_t::invalid(); } @@ -528,68 +528,73 @@ std::optional route_bidirectional(ways const& w, b.distance_lon_degrees_) / kMaxMatchingDistanceSquaredRatio; - for (auto const [i, start] : utl::enumerate(from_match)) { - if (b.max_reached_1_ && component_seen(w, from_match, i)) { - continue; - } - auto const start_way = start.way_; - for (auto const* nc : {&start.left_, &start.right_}) { - if (nc->valid() && nc->cost_ < max) { - Profile::resolve_start_node( - *w.r_, start.way_, nc->node_, from.lvl_, dir, [&](auto const node) { - auto label = typename Profile::label{node, nc->cost_}; - label.track(label, *w.r_, start_way, node.get_node(), false); - b.add_start(w, label, sharing); - }); - } - } - if (b.pq1_.empty()) { - continue; - } - for (auto const [j, end] : utl::enumerate(to_match)) { - if (w.r_->way_component_[start.way_] != w.r_->way_component_[end.way_]) { - continue; - } - if (b.max_reached_2_ && component_seen(w, to_match, j)) { + for (auto const accept_bad_max_matching_dist : {false, true}) { + for (auto const [i, start] : utl::enumerate(from_match)) { + if (b.max_reached_1_ && component_seen(w, from_match, i)) { continue; } - if (std::pow(end.dist_to_way_, 2) > limit_squared_max_matching_distance) { - break; - } - auto const end_way = end.way_; - for (auto const* nc : {&end.left_, &end.right_}) { + auto const start_way = start.way_; + for (auto const* nc : {&start.left_, &start.right_}) { if (nc->valid() && nc->cost_ < max) { Profile::resolve_start_node( - *w.r_, end_way, nc->node_, to.lvl_, opposite(dir), + *w.r_, start.way_, nc->node_, from.lvl_, dir, [&](auto const node) { auto label = typename Profile::label{node, nc->cost_}; - label.track(label, *w.r_, end_way, node.get_node(), false); - b.add_end(w, label, sharing); + label.track(label, *w.r_, start_way, node.get_node(), false); + b.add_start(w, label, sharing); }); } } - if (b.pq2_.empty()) { + if (b.pq1_.empty()) { continue; } - auto const should_continue = - b.run(w, *w.r_, max, blocked, sharing, elevations, dir); - - if (b.meet_point_1_.get_node() == node_idx_t::invalid()) { - if (should_continue) { + for (auto const [j, end] : utl::enumerate(to_match)) { + if (w.r_->way_component_[start.way_] != + w.r_->way_component_[end.way_]) { continue; } - return std::nullopt; - } + if (b.max_reached_2_ && component_seen(w, to_match, j)) { + continue; + } + if (std::pow(end.dist_to_way_, 2) > + limit_squared_max_matching_distance) { + break; + } + auto const end_way = end.way_; + for (auto const* nc : {&end.left_, &end.right_}) { + if (nc->valid() && nc->cost_ < max) { + Profile::resolve_start_node( + *w.r_, end_way, nc->node_, to.lvl_, opposite(dir), + [&](auto const node) { + auto label = typename Profile::label{node, nc->cost_}; + label.track(label, *w.r_, end_way, node.get_node(), false); + b.add_end(w, label, sharing); + }); + } + } + if (b.pq2_.empty()) { + continue; + } + auto const should_continue = + b.run(w, *w.r_, max, blocked, sharing, elevations, dir); - auto const cost = b.get_cost_to_mp(b.meet_point_1_, b.meet_point_2_); + if (b.meet_point_1_.get_node() == node_idx_t::invalid()) { + if (should_continue) { + continue; + } + return std::nullopt; + } + + auto const cost = b.get_cost_to_mp(b.meet_point_1_, b.meet_point_2_); - return reconstruct_bi(w, l, blocked, sharing, elevations, b, from, to, - start, end, cost, dir); + return reconstruct_bi(w, l, blocked, sharing, elevations, b, from, to, + start, end, cost, dir); + } + b.pq1_.clear(); + b.pq2_.clear(); + b.cost2_.clear(); + b.max_reached_2_ = false; } - b.pq1_.clear(); - b.pq2_.clear(); - b.cost2_.clear(); - b.max_reached_2_ = false; } return std::nullopt; } From 0653b8919a7b5113f0ddc10f49d241d02121f65d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Felix=20G=C3=BCndling?= Date: Sun, 27 Jul 2025 22:28:52 +0200 Subject: [PATCH 3/3] wip --- src/route.cc | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/route.cc b/src/route.cc index 9a61048d..f1027ab2 100644 --- a/src/route.cc +++ b/src/route.cc @@ -518,17 +518,19 @@ std::optional route_bidirectional(ways const& w, return *direct; } - b.reset(max, from, to); - if (b.radius_ == max) { - return std::nullopt; - } + for (auto const accept_bad_max_matching_dist : {false, true}) { + auto const limit_squared_max_matching_distance = + accept_bad_max_matching_dist + ? std::numeric_limits::max() + : geo::approx_squared_distance(from.pos_, to.pos_, + b.distance_lon_degrees_) / + kMaxMatchingDistanceSquaredRatio; - auto const limit_squared_max_matching_distance = - geo::approx_squared_distance(from.pos_, to.pos_, - b.distance_lon_degrees_) / - kMaxMatchingDistanceSquaredRatio; + b.reset(max, from, to); + if (b.radius_ == max) { + return std::nullopt; + } - for (auto const accept_bad_max_matching_dist : {false, true}) { for (auto const [i, start] : utl::enumerate(from_match)) { if (b.max_reached_1_ && component_seen(w, from_match, i)) { continue; @@ -617,10 +619,10 @@ std::optional route_dijkstra(ways const& w, } auto const distance_lng_degrees = geo::approx_distance_lng_degrees(from.pos_); - - d.reset(max); - auto should_continue = true; for (auto const accept_bad_max_matching_dist : {false, true}) { + d.reset(max); + auto should_continue = true; + auto const limit_squared_max_matching_distance = accept_bad_max_matching_dist ? std::numeric_limits::max() @@ -694,9 +696,9 @@ std::vector> route( auto const distance_lng_degrees = geo::approx_distance_lng_degrees(from.pos_); d.reset(max); - auto should_continue = true; for (auto const accept_bad_max_matching_dist : {false, true}) { + auto should_continue = true; for (auto const [i, start] : utl::enumerate(from_match)) { if (!should_continue && component_seen(w, from_match, i)) { continue;