diff --git a/CHANGELOG.md b/CHANGELOG.md index 16af4e7693..a2421de21e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -106,6 +106,7 @@ * ADDED: return isotile grid as geotiff [#4594](https://github.com/valhalla/valhalla/pull/4594) * ADDED: `ignore_non_vehicular_restrictions` parameter for truck costing [#4606](https://github.com/valhalla/valhalla/pull/4606) * UPDATED: tz database to 2024a [#4643](https://github.com/valhalla/valhalla/pull/4643) + * FIXED: connection check for costmatrix when the connection edge is on a correlated edge of origin OR target [#4663](https://github.com/valhalla/valhalla/pull/4663) ## Release Date: 2023-05-11 Valhalla 3.4.0 * **Removed** diff --git a/src/thor/bidirectional_astar.cc b/src/thor/bidirectional_astar.cc index 1d31b9bef2..0834d01ad6 100644 --- a/src/thor/bidirectional_astar.cc +++ b/src/thor/bidirectional_astar.cc @@ -805,16 +805,16 @@ BidirectionalAStar::GetBestPath(valhalla::Location& origin, // search tree. Check if this is the best connection so far and set the // search threshold. bool BidirectionalAStar::SetForwardConnection(GraphReader& graphreader, const BDEdgeLabel& pred) { - // Find pred on opposite side - GraphId oppedge = pred.opp_edgeid(); - EdgeStatusInfo oppedgestatus = edgestatus_reverse_.Get(oppedge); - auto opp_pred = edgelabels_reverse_[oppedgestatus.index()]; - // Disallow connections that are part of an uturn on an internal edge if (pred.internal_turn() != InternalTurn::kNoTurn) { return false; } + // Find pred on opposite side + GraphId oppedge = pred.opp_edgeid(); + EdgeStatusInfo oppedgestatus = edgestatus_reverse_.Get(oppedge); + auto opp_pred = edgelabels_reverse_[oppedgestatus.index()]; + // Disallow connections that are part of a complex restriction if (pred.on_complex_rest()) { // Lets dig deeper and test if we are really triggering these restrictions @@ -837,8 +837,11 @@ bool BidirectionalAStar::SetForwardConnection(GraphReader& graphreader, const BD } else { // If no predecessor on the forward path get the predecessor on // the reverse path to form the cost. - uint32_t predidx = opp_pred.predecessor(); - float oppcost = (predidx == kInvalidLabel) ? 0 : edgelabels_reverse_[predidx].cost().cost; + uint32_t opp_predidx = opp_pred.predecessor(); + // TODO(nils): this can't be happen, it'd mean that this is a trivial route + // , which is not supposed to be here! same in SetReverseConnection + float oppcost = + (opp_predidx == kInvalidLabel) ? 0.f : edgelabels_reverse_[opp_predidx].cost().cost; c = pred.cost().cost + oppcost + opp_pred.transition_cost().cost; } diff --git a/src/thor/costmatrix.cc b/src/thor/costmatrix.cc index b260177e41..65e0a27cbe 100644 --- a/src/thor/costmatrix.cc +++ b/src/thor/costmatrix.cc @@ -775,7 +775,7 @@ void CostMatrix::CheckForwardConnections(const uint32_t source, const BDEdgeLabel& rev_label = rev_edgelabels[rev_edgestatus.index()]; // Special case - common edge for source and target are both initial edges - if (fwd_pred.predecessor() == kInvalidLabel && rev_predidx == kInvalidLabel) { + if (fwd_pred.predecessor() == kInvalidLabel && (rev_predidx == kInvalidLabel)) { // bail if either edge wasn't allowed (see notes in SetSources/Targets) if (!fwd_pred.path_id() || !rev_label.path_id()) { return; @@ -793,23 +793,32 @@ void CostMatrix::CheckForwardConnections(const uint32_t source, best_connection_[idx].Update(fwd_pred.edgeid(), rev_edgeid, Cost(s, s), d); best_connection_[idx].found = true; + // Update status and update threshold if this is the last location + // to find for this source or target + UpdateStatus(source, target); + } else if (rev_predidx == kInvalidLabel) { + // if this is a target's correlated edge, add its attributes to the fwd_pred's predecessor + auto& fwd_pred_pred = edgelabel_[MATRIX_FORW][source][fwd_pred.predecessor()]; + auto dist = fwd_pred_pred.path_distance() + rev_label.path_distance(); + auto cost = fwd_pred_pred.cost() + rev_label.cost(); + + // there can't be any cheaper connection + best_connection_[idx].Update(fwd_pred.edgeid(), rev_edgeid, cost, dist); + best_connection_[idx].found = true; + // Update status and update threshold if this is the last location // to find for this source or target UpdateStatus(source, target); } else { - float oppcost = (rev_predidx == kInvalidLabel) ? 0.f : rev_edgelabels[rev_predidx].cost().cost; - float c = fwd_pred.cost().cost + oppcost + rev_label.transition_cost().cost; + auto& opp_pred_label = rev_edgelabels[rev_predidx]; + Cost cost = fwd_pred.cost() + opp_pred_label.cost() + rev_label.transition_cost(); // Check if best connection - if (c < best_connection_[idx].cost.cost) { - float oppsec = (rev_predidx == kInvalidLabel) ? 0.f : rev_edgelabels[rev_predidx].cost().secs; - uint32_t oppdist = - (rev_predidx == kInvalidLabel) ? 0U : rev_edgelabels[rev_predidx].path_distance(); - float s = fwd_pred.cost().secs + oppsec + rev_label.transition_cost().secs; - uint32_t d = fwd_pred.path_distance() + oppdist; + if (cost.cost < best_connection_[idx].cost.cost) { + uint32_t dist = fwd_pred.path_distance() + opp_pred_label.path_distance(); // Update best connection and set a threshold - best_connection_[idx].Update(fwd_pred.edgeid(), rev_edgeid, Cost(c, s), d); + best_connection_[idx].Update(fwd_pred.edgeid(), rev_edgeid, cost, dist); if (best_connection_[idx].max_iterations == 0) { best_connection_[idx].max_iterations = n + GetThreshold(mode_, edgelabel_[MATRIX_FORW][source].size() + @@ -860,15 +869,14 @@ void CostMatrix::CheckReverseConnections(const uint32_t target, // Iterate through the sources for (auto source : sources->second) { - uint32_t source_idx = source * locs_count_[MATRIX_REV] + target; - if (best_connection_[source_idx].found) { + uint32_t idx = source * locs_count_[MATRIX_REV] + target; + if (best_connection_[idx].found) { continue; } // Update any targets whose threshold has been reached - if (best_connection_[source_idx].max_iterations > 0 && - n > best_connection_[source_idx].max_iterations) { - best_connection_[source_idx].found = true; + if (best_connection_[idx].max_iterations > 0 && n > best_connection_[idx].max_iterations) { + best_connection_[idx].found = true; continue; } @@ -896,29 +904,36 @@ void CostMatrix::CheckReverseConnections(const uint32_t target, uint32_t d = std::abs(static_cast(rev_pred.path_distance()) + static_cast(fwd_label.path_distance()) - static_cast(fwd_label.transition_cost().secs)); - best_connection_[source_idx].Update(fwd_edgeid, rev_pred.edgeid(), Cost(s, s), d); - best_connection_[source_idx].found = true; + best_connection_[idx].Update(fwd_edgeid, rev_pred.edgeid(), Cost(s, s), d); + best_connection_[idx].found = true; + + // Update status and update threshold if this is the last location + // to find for this source or target + UpdateStatus(source, target); + } else if (fwd_predidx == kInvalidLabel) { + // if this is a source's correlated edge, add its attributes to the rev_pred's predecessor + auto& rev_pred_pred = edgelabel_[MATRIX_REV][source][rev_pred.predecessor()]; + auto dist = rev_pred_pred.path_distance() + fwd_label.path_distance(); + auto cost = rev_pred_pred.cost() + fwd_label.cost(); + + // there can't be any cheaper connection + best_connection_[idx].Update(fwd_edgeid, rev_pred.edgeid(), cost, dist); + best_connection_[idx].found = true; // Update status and update threshold if this is the last location // to find for this source or target UpdateStatus(source, target); } else { - float oppcost = (fwd_predidx == kInvalidLabel) ? 0 : fwd_edgelabels[fwd_predidx].cost().cost; - float c = rev_pred.cost().cost + oppcost + fwd_label.transition_cost().cost; + auto opp_pred_label = fwd_edgelabels[fwd_predidx]; + auto cost = rev_pred.cost() + opp_pred_label.cost() + fwd_label.transition_cost(); // Check if best connection - if (c < best_connection_[source_idx].cost.cost) { - float fwd_sec = - (fwd_predidx == kInvalidLabel) ? 0 : fwd_edgelabels[fwd_predidx].cost().secs; - uint32_t fwd_dist = - (fwd_predidx == kInvalidLabel) ? 0 : fwd_edgelabels[fwd_predidx].path_distance(); - float s = rev_pred.cost().secs + fwd_sec + fwd_label.transition_cost().secs; - uint32_t d = rev_pred.path_distance() + fwd_dist; - + if (cost.cost < best_connection_[idx].cost.cost) { // Update best connection and set a threshold - best_connection_[source_idx].Update(fwd_edgeid, rev_pred.edgeid(), Cost(c, s), d); - if (best_connection_[source_idx].max_iterations == 0) { - best_connection_[source_idx].max_iterations = + uint32_t dist = rev_pred.path_distance() + opp_pred_label.path_distance(); + best_connection_[idx].Update(fwd_edgeid, rev_pred.edgeid(), cost, dist); + if (best_connection_[idx].max_iterations == 0) { + best_connection_[idx].max_iterations = n + GetThreshold(mode_, edgelabel_[MATRIX_FORW][source].size() + edgelabel_[MATRIX_REV][target].size()); } diff --git a/test/gurka/test_matrix.cc b/test/gurka/test_matrix.cc index 3500fcdaec..323d136346 100644 --- a/test/gurka/test_matrix.cc +++ b/test/gurka/test_matrix.cc @@ -820,3 +820,23 @@ TEST(StandAlone, CostMatrixTrivialRoutes) { EXPECT_EQ(matrix.matrix().shapes(0), encoded); } } + +TEST(StandAlone, CostMatrixConnectedRoute) { + // see https://github.com/valhalla/valhalla/pull/4663#discussion_r1540245978 + const std::string ascii_map = R"( + A1---B---2C + )"; + const gurka::ways ways = { + {"AB", {{"highway", "residential"}}}, + {"BC", {{"highway", "residential"}}}, + }; + auto layout = gurka::detail::map_to_coordinates(ascii_map, 100); + auto map = gurka::buildtiles(layout, ways, {}, {}, + VALHALLA_BUILD_DIR "test/data/costmatrix_connectedroute"); + + // test the against-oneway case + auto matrix = gurka::do_action(valhalla::Options::sources_to_targets, map, {"1"}, {"2"}, "auto", + {{"/shape_format", "polyline6"}}); + EXPECT_EQ(matrix.matrix().distances(0), 800); + EXPECT_NEAR(matrix.matrix().times(0), 82.28f, 0.01f); +}