Skip to content

second chance second try #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
felixguendling wants to merge 3 commits into
base: master
Choose a base branch
from
Open

second chance second try #53

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
3d40022
second chance second try
felixguendling Jul 27, 2025
f7589de
second chance loop for bidir
felixguendling Jul 27, 2025
0653b89
wip
felixguendling Jul 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
325 changes: 167 additions & 158 deletions src/route.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<cost_t>::max()};
auto best = static_cast<node_candidate const*>(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<cost_t>::max()};
auto best = static_cast<node_candidate const*>(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");
Expand Down Expand Up @@ -528,35 +518,38 @@ std::optional<path> route_bidirectional(ways const& w,
return *direct;
}

b.reset(max, from, to);
if (b.radius_ == max) {
return std::nullopt;
}

auto const limit_squared_max_matching_distance =
geo::approx_squared_distance(from.pos_, to.pos_,
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;
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<double>::max()
: 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;
}
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);
});

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;
}
}
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_]) {
Expand All @@ -565,9 +558,8 @@ std::optional<path> route_bidirectional(ways const& w,
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) {
if (std::pow(end.dist_to_way_, 2) >
limit_squared_max_matching_distance) {
break;
}
auto const end_way = end.way_;
Expand Down Expand Up @@ -600,11 +592,11 @@ std::optional<path> route_bidirectional(ways const& w,
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;
}
Expand All @@ -626,45 +618,53 @@ std::optional<path> route_dijkstra(ways const& w,
return *direct;
}

auto const limit_squared_max_matching_distance =
std::pow(geo::distance(from.pos_, to.pos_), 2) /
kMaxMatchingDistanceSquaredRatio;

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_}); });
auto const distance_lng_degrees = geo::approx_distance_lng_degrees(from.pos_);
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<double>::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<Profile>(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<Profile>(w, l, blocked, sharing, elevations, d, from,
to, start, *wc, *nc, node, p.cost_, dir);
}
}
}

Expand Down Expand Up @@ -696,57 +696,66 @@ std::vector<std::optional<path>> route(
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;
}
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}) {
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);
});
}
}
}

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<Profile>(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<double>::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<Profile>(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;
}
}
}

Expand Down
Loading